From 951defe23c7ad82f23bfa354629daff0cb769e45 Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Mon, 11 Jul 2022 11:41:21 +0900 Subject: [PATCH] Develop ros2 (#150) * Feature/ublox navpvt (#123) * sub ublox_msg & pub rtk_msg * eagleye works well for T4 rosbags * add enu2xyz_vel.cpp for ublox_msgs::NavPVT * rename ublox_node to navpvt2rtk_node * geographiclib handles angles as degree * remove personal config & modify fix2kml for color option * Fix bun in pose (#126) * Fix/foxy build error (#130) * Fix build error in foxy * Use nullptr instead of optional * Add CI for ros foxy * Fix build.yml * Fix build.yml * Update README.md * Feature/evaluation script ros2 (#141) * Add evaluation_script * Support south and west * Feature/canless ros2 (#144) * Update eagleye_core * Update eagleye_rt * Fix yaml error * Change the node to screen output * Remove pkg_check_modules * Remove pkg_check_modules in CMakeLists.txt * Feature/ros2 csv output (#145) * Fixed for data name * Fixed a name change error * Add log output in eagleye_rt * Fix conflict Co-authored-by: YutaHoda <170447064@ccalumni.meijo-u.ac.jp> * Add processing when input twist does not come in for a certain amount of time (#146) * Add covariance output (#147) * Add converted imu (#148) Co-authored-by: Kento Yabuuchi Co-authored-by: YutaHoda <170447064@ccalumni.meijo-u.ac.jp> --- .github/workflows/build.yml | 49 +- .gitignore | 1 + CONTRIBUTING.md | 23 + README.md | 1 + eagleye_core/coordinate/CMakeLists.txt | 1 + .../eagleye_coordinate/eagleye_coordinate.hpp | 8 +- eagleye_core/coordinate/src/enu2xyz_vel.cpp | 44 ++ .../coordinate/src/geoid_per_minute.cpp | 2 - eagleye_core/coordinate/src/llh2xyz.cpp | 10 +- eagleye_core/coordinate/src/xyz2enu_vel.cpp | 2 +- eagleye_core/navigation/CMakeLists.txt | 2 + .../eagleye_navigation/eagleye_navigation.hpp | 252 +++++- eagleye_core/navigation/package.xml | 2 +- .../src/angular_velocity_offset_stop.cpp | 30 +- eagleye_core/navigation/src/distance.cpp | 13 +- .../src/enable_additional_rolling.cpp | 60 +- eagleye_core/navigation/src/heading.cpp | 29 +- .../navigation/src/heading_interpolate.cpp | 18 +- eagleye_core/navigation/src/height.cpp | 22 +- eagleye_core/navigation/src/position.cpp | 24 +- .../navigation/src/position_interpolate.cpp | 8 + eagleye_core/navigation/src/rolling.cpp | 14 +- .../navigation/src/rtk_deadreckoning.cpp | 4 + eagleye_core/navigation/src/rtk_heading.cpp | 17 +- eagleye_core/navigation/src/slip_angle.cpp | 19 +- .../navigation/src/slip_coefficient.cpp | 21 +- eagleye_core/navigation/src/smoothing.cpp | 5 +- eagleye_core/navigation/src/trajectory.cpp | 87 +- .../navigation/src/velocity_estimator.cpp | 714 +++++++++++++++++ .../navigation/src/velocity_scale_factor.cpp | 19 +- .../navigation/src/yawrate_offset.cpp | 31 +- .../navigation/src/yawrate_offset_stop.cpp | 18 +- eagleye_msgs/CMakeLists.txt | 1 + eagleye_msgs/msg/StatusStamped.msg | 3 + eagleye_msgs/msg/VelocityScaleFactor.msg | 1 - eagleye_rt/CMakeLists.txt | 15 + eagleye_rt/config/eagleye_config.yaml | 33 +- .../launch/eagleye_calibration.launch.xml | 5 +- eagleye_rt/launch/eagleye_rt.launch.xml | 5 +- .../launch/eagleye_rt_canless.launch.xml | 105 +++ eagleye_rt/launch/navpvt_rt.launch.xml | 16 + eagleye_rt/log/.gitignore | 0 eagleye_rt/package.xml | 5 + .../src/angular_velocity_offset_stop_node.cpp | 9 +- eagleye_rt/src/correction_imu.cpp | 29 +- eagleye_rt/src/distance_node.cpp | 40 +- .../src/enable_additional_rolling_node.cpp | 60 +- eagleye_rt/src/heading_interpolate_node.cpp | 41 +- eagleye_rt/src/heading_node.cpp | 53 +- eagleye_rt/src/height_node.cpp | 34 +- eagleye_rt/src/monitor_node.cpp | 746 ++++++++++++------ eagleye_rt/src/navpvt2rtk_node.cpp | 97 +++ eagleye_rt/src/position_node.cpp | 47 +- eagleye_rt/src/rolling_node.cpp | 33 +- eagleye_rt/src/rtk_heading_node.cpp | 45 +- eagleye_rt/src/slip_angle_node.cpp | 40 +- eagleye_rt/src/slip_coefficient_node.cpp | 46 +- eagleye_rt/src/smoothing_node.cpp | 25 +- eagleye_rt/src/tf_converted_imu.cpp | 134 ++++ eagleye_rt/src/trajectory_node.cpp | 163 +++- eagleye_rt/src/velocity_estimator_node.cpp | 126 +++ eagleye_rt/src/velocity_scale_factor_node.cpp | 29 +- eagleye_rt/src/yawrate_offset_node.cpp | 40 +- eagleye_rt/src/yawrate_offset_stop_node.cpp | 11 +- .../fix2kml/include/fix2kml/KmlGenerator.hpp | 6 +- eagleye_util/fix2kml/launch/fix2kml.xml | 10 +- eagleye_util/fix2kml/src/fix2kml.cpp | 39 +- eagleye_util/fix2pose/src/fix2pose.cpp | 23 +- eagleye_util/nmea2fix/src/nmea2fix_core.cpp | 12 +- eagleye_util/trajectory_plot/README.md | 100 +++ .../config/trajectory_plot.yaml | 97 +++ .../scripts/eagleye_pp_single_evaluation.py | 111 +++ .../scripts/evaluation_plot.py | 250 ++++++ .../scripts/twist_evaluation.py | 190 +++++ .../trajectory_plot/scripts/util/calc.py | 658 +++++++++++++++ .../trajectory_plot/scripts/util/plot.py | 294 +++++++ .../scripts/util/preprocess.py | 478 +++++++++++ 77 files changed, 4953 insertions(+), 902 deletions(-) create mode 100644 .gitignore create mode 100644 CONTRIBUTING.md create mode 100644 eagleye_core/coordinate/src/enu2xyz_vel.cpp create mode 100644 eagleye_core/navigation/src/velocity_estimator.cpp create mode 100644 eagleye_msgs/msg/StatusStamped.msg create mode 100644 eagleye_rt/launch/eagleye_rt_canless.launch.xml create mode 100644 eagleye_rt/launch/navpvt_rt.launch.xml create mode 100644 eagleye_rt/log/.gitignore create mode 100644 eagleye_rt/src/navpvt2rtk_node.cpp create mode 100644 eagleye_rt/src/tf_converted_imu.cpp create mode 100644 eagleye_rt/src/velocity_estimator_node.cpp create mode 100644 eagleye_util/trajectory_plot/README.md create mode 100644 eagleye_util/trajectory_plot/config/trajectory_plot.yaml create mode 100755 eagleye_util/trajectory_plot/scripts/eagleye_pp_single_evaluation.py create mode 100644 eagleye_util/trajectory_plot/scripts/evaluation_plot.py create mode 100644 eagleye_util/trajectory_plot/scripts/twist_evaluation.py create mode 100644 eagleye_util/trajectory_plot/scripts/util/calc.py create mode 100644 eagleye_util/trajectory_plot/scripts/util/plot.py create mode 100644 eagleye_util/trajectory_plot/scripts/util/preprocess.py diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index f52b819e..e6c43045 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,13 +1,8 @@ name: build -on: - push: - branches: - - ros2-main - pull_request: - branches: - - ros2-main +on: pull_request + jobs: - build: + galactic_build: runs-on: ubuntu-20.04 container: ros:galactic @@ -43,4 +38,42 @@ jobs: 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 + + container: ros:foxy + 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/foxy/setup.bash + rosdep install --from-paths src --ignore-src -r -y colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests \ No newline at end of file diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..1d74e219 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode/ diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..66eb02a8 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,23 @@ +# Contributing to Eagleye +## Repository branch structure +- main-ros1 +This is the latest stable version of Eagleye ROS1. It is updated every three months. +- **develop-ros1** +If contributors wish to add functionality to eagleye, they should create a new branch from this develop branch and submit a pull request. +- main-ros2 +This is the latest stable version of Eagleye ROS2. It is updated every three months. +- develop-ros2 +This is a branch to reflect changes in develop-ros1 to ros2 as well. +MapIV develops eagleye functions mainly in ROS1, but if you are using eagleye in ROS2 and want to submit a PR, +please submit a PR in this branch. +- feature/ +Branches for new features and improvements +- fix/ +Branch for fixing non-hotfix bugs +- hotfix/ +Branch for fixing serious bugs in main branch +## Pull Request +When making a pull request, please describe what the pull request is about and how the reviewer can verify that it works. +If you find after submitting a PR that it cannot be merged because it needs to be corrected, add [WIP] to the title of the PR and remove [WIP] from the title as soon as the correction is complete. +## Coding Rule +Please refer to the coding rules in the [ROS developer's guide](http://wiki.ros.org/DevelopersGuide) \ No newline at end of file diff --git a/README.md b/README.md index 41a12459..0e4186c6 100755 --- a/README.md +++ b/README.md @@ -47,6 +47,7 @@ Clone and build the necessary packages for Eagleye. ([rtklib_ros_bridge](https:/ 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 sudo apt-get install -y libgeographic-dev geographiclib-tools geographiclib-doc + cd .. rosdep install --from-paths src --ignore-src -r -y colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release diff --git a/eagleye_core/coordinate/CMakeLists.txt b/eagleye_core/coordinate/CMakeLists.txt index b939c4c1..e6edc59c 100755 --- a/eagleye_core/coordinate/CMakeLists.txt +++ b/eagleye_core/coordinate/CMakeLists.txt @@ -35,6 +35,7 @@ link_directories( ament_auto_add_library(eagleye_coordinate SHARED src/ecef2llh.cpp src/enu2llh.cpp + src/enu2xyz_vel.cpp src/ll2xy_mgrs.cpp src/ll2xy.cpp src/llh2xyz.cpp diff --git a/eagleye_core/coordinate/include/eagleye_coordinate/eagleye_coordinate.hpp b/eagleye_core/coordinate/include/eagleye_coordinate/eagleye_coordinate.hpp index 36435e5f..87c17e75 100755 --- a/eagleye_core/coordinate/include/eagleye_coordinate/eagleye_coordinate.hpp +++ b/eagleye_core/coordinate/include/eagleye_coordinate/eagleye_coordinate.hpp @@ -28,7 +28,6 @@ #include #include -//#include "geographic_msgs/GeoPoint.h" #include "geographic_msgs/msg/geo_point.hpp" #include #include @@ -43,7 +42,7 @@ class ConvertHeight double convert2ellipsoid(); double getGeoidPerMinute(); double getGeoidPerDegree(); - void setLLH(double,double,double); + void setLLH(double, double, double); private: double _latitude; @@ -58,11 +57,12 @@ extern void ll2xy(int, double*, double*); extern void ll2xy_mgrs(double*, double*); extern void ecef2llh(double*, double*); extern void enu2llh(double*, double*, double*); +extern void enu2xyz_vel(double*, double*, double*); extern void llh2xyz(double*, double*); extern void xyz2enu(double*, double*, double*); extern void xyz2enu_vel(double*, double*, double*); -extern double geoid_per_degree(double,double); -extern double geoid_per_minute(double,double,double**); +extern double geoid_per_degree(double, double); +extern double geoid_per_minute(double, double, double**); extern double** read_geoid_map(); diff --git a/eagleye_core/coordinate/src/enu2xyz_vel.cpp b/eagleye_core/coordinate/src/enu2xyz_vel.cpp new file mode 100644 index 00000000..3aafd227 --- /dev/null +++ b/eagleye_core/coordinate/src/enu2xyz_vel.cpp @@ -0,0 +1,44 @@ +// 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 "eagleye_coordinate/eagleye_coordinate.hpp" +#include +#include + +void enu2xyz_vel(double enu_vel[3], double ecef_base_pos[3], double xyz_vel[3]) +{ + using namespace GeographicLib; + Geocentric earth(Constants::WGS84_a(), Constants::WGS84_f()); + + std::vector rotation(9); + double llh[3]; + earth.Reverse(ecef_base_pos[0], ecef_base_pos[1], ecef_base_pos[2], llh[0], llh[1], llh[2], rotation); + + Eigen::Matrix3d R(rotation.data()); + Eigen::Vector3d v_enu(enu_vel); + + Eigen::Map v_xyz(xyz_vel); + v_xyz = R.transpose() * v_enu; +} \ No newline at end of file diff --git a/eagleye_core/coordinate/src/geoid_per_minute.cpp b/eagleye_core/coordinate/src/geoid_per_minute.cpp index 4606791e..8f8d3a0c 100755 --- a/eagleye_core/coordinate/src/geoid_per_minute.cpp +++ b/eagleye_core/coordinate/src/geoid_per_minute.cpp @@ -35,7 +35,6 @@ #include #include -//#include #include @@ -44,7 +43,6 @@ double** read_geoid_map() double** data; - // std::string path = ros::package::getPath("eagleye_coordinate") + "/data/"; std::string path = ament_index_cpp::get_package_share_directory("eagleye_coordinate") + "/data/"; std::string file_name = "gsigeo2011_ver2.asc"; std::ifstream ifs(path+file_name); diff --git a/eagleye_core/coordinate/src/llh2xyz.cpp b/eagleye_core/coordinate/src/llh2xyz.cpp index 3be58f70..6d57faf9 100644 --- a/eagleye_core/coordinate/src/llh2xyz.cpp +++ b/eagleye_core/coordinate/src/llh2xyz.cpp @@ -30,7 +30,7 @@ void llh2xyz(double llh_pos[3], double ecef_pos[3]) { double semi_major_axis = 6378137.0000; double semi_minor_axis = 6356752.3142; - double a1 = sqrt (1-pow((semi_minor_axis/semi_major_axis), 2.0)); + double a1 = sqrt(1 - pow((semi_minor_axis / semi_major_axis), 2.0)); double a2 = a1 * a1; double phi = llh_pos[0]; @@ -43,9 +43,9 @@ void llh2xyz(double llh_pos[3], double ecef_pos[3]) double sin_lam = sin(lam); double tmp1 = 1 - a2; - double tmp2 = sqrt(1 - a2*sin_phi*sin_phi); + double tmp2 = sqrt(1 - a2 * sin_phi * sin_phi); - ecef_pos[0] = (semi_major_axis/tmp2 + hei)*cos_lam*cos_phi; - ecef_pos[1] = (semi_major_axis/tmp2 + hei)*sin_lam*cos_phi; - ecef_pos[2] = (semi_major_axis/tmp2*tmp1 + hei)*sin_phi; + ecef_pos[0] = (semi_major_axis / tmp2 + hei) * cos_lam * cos_phi; + ecef_pos[1] = (semi_major_axis / tmp2 + hei) * sin_lam * cos_phi; + ecef_pos[2] = (semi_major_axis / tmp2 * tmp1 + hei) * sin_phi; } diff --git a/eagleye_core/coordinate/src/xyz2enu_vel.cpp b/eagleye_core/coordinate/src/xyz2enu_vel.cpp index 15849210..f2347670 100644 --- a/eagleye_core/coordinate/src/xyz2enu_vel.cpp +++ b/eagleye_core/coordinate/src/xyz2enu_vel.cpp @@ -30,7 +30,7 @@ void xyz2enu_vel(double ecef_vel[3], double ecef_base_pos[3], double enu_vel[3]) { double llh_base_pos[3]; - ecef2llh(ecef_base_pos,llh_base_pos); + ecef2llh(ecef_base_pos, llh_base_pos); enu_vel[0] = (-ecef_vel[0] * (sin(llh_base_pos[1]))) + (ecef_vel[1] * (cos(llh_base_pos[1]))); enu_vel[1] = (-ecef_vel[0] * (cos(llh_base_pos[1])) * (sin(llh_base_pos[0]))) - (ecef_vel[1] * (sin(llh_base_pos[1])) * (sin(llh_base_pos[0]))) + (ecef_vel[2] * (cos(llh_base_pos[0]))); enu_vel[2] = (ecef_vel[0] * (cos(llh_base_pos[1])) * (cos(llh_base_pos[0]))) + (ecef_vel[1] * (sin(llh_base_pos[1])) * (cos(llh_base_pos[0]))) + (ecef_vel[2] * (sin(llh_base_pos[0]))); diff --git a/eagleye_core/navigation/CMakeLists.txt b/eagleye_core/navigation/CMakeLists.txt index 01d185e6..c04e2a81 100755 --- a/eagleye_core/navigation/CMakeLists.txt +++ b/eagleye_core/navigation/CMakeLists.txt @@ -18,6 +18,7 @@ endif() # find dependencies find_package(ament_cmake_auto REQUIRED) find_package(eagleye_coordinate REQUIRED) +find_package(PkgConfig REQUIRED) ament_auto_find_build_dependencies() @@ -42,6 +43,7 @@ ament_auto_add_library(eagleye_navigation SHARED src/yawrate_offset.cpp src/rtk_deadreckoning.cpp src/rtk_heading.cpp + src/velocity_estimator.cpp include/eagleye_navigation/eagleye_navigation.hpp ) diff --git a/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp b/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp index ced86604..62046a8c 100644 --- a/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp +++ b/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp @@ -22,6 +22,7 @@ // 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 "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" @@ -33,6 +34,8 @@ #include "nmea_msgs/msg/gprmc.hpp" #include "rtklib_msgs/msg/rtklib_nav.hpp" +#include "eagleye_msgs/msg/status.hpp" +#include "eagleye_msgs/msg/status_stamped.hpp" #include "eagleye_msgs/msg/distance.hpp" #include "eagleye_msgs/msg/yawrate_offset.hpp" #include "eagleye_msgs/msg/velocity_scale_factor.hpp" @@ -85,7 +88,6 @@ struct DistanceStatus struct YawrateOffsetStopParameter { - bool reverse_imu; double stop_judgment_velocity_threshold; double estimated_number; double outlier_threshold; @@ -101,7 +103,6 @@ struct YawrateOffsetStopStatus struct YawrateOffsetParameter { - bool reverse_imu; double estimated_number_min; double estimated_number_max; double estimated_coefficient; @@ -125,7 +126,6 @@ struct YawrateOffsetStatus struct HeadingParameter { - bool reverse_imu; double estimated_number_min; double estimated_number_max; double estimated_gnss_coefficient; @@ -154,7 +154,6 @@ struct HeadingStatus struct RtkHeadingParameter { - bool reverse_imu; double estimated_distance; int estimated_heading_buffer_min; double estimated_number_min; @@ -191,7 +190,6 @@ struct RtkHeadingStatus struct HeadingInterpolateParameter { - bool reverse_imu; double stop_judgment_velocity_threshold; double number_buffer_max; }; @@ -271,14 +269,12 @@ struct PositionInterpolateStatus struct SlipangleParameter { - bool reverse_imu; double stop_judgment_velocity_threshold; double manual_coefficient; }; struct SlipCoefficientParameter { - bool reverse_imu; double estimated_number_min; double estimated_number_max; double estimated_velocity_threshold; @@ -315,7 +311,6 @@ struct SmoothingStatus struct TrajectoryParameter { - bool reverse_imu; double stop_judgment_velocity_threshold; double stop_judgment_yawrate_threshold; }; @@ -371,7 +366,6 @@ struct HeightStatus struct AngularVelocityOffsetStopParameter { - bool reverse_imu; double stop_judgment_velocity_threshold; double estimated_number; double outlier_threshold; @@ -426,9 +420,6 @@ struct RtkDeadreckoningStatus struct EnableAdditionalRollingParameter { - bool reverse_imu; - bool reverse_imu_angular_velocity_x; - bool reverse_imu_linear_acceleration_y; double matching_update_distance; double stop_judgment_velocity_threshold; double rolling_buffer_num; @@ -459,7 +450,6 @@ struct EnableAdditionalRollingStatus struct RollingParameter { - bool reverse_imu; double stop_judgment_velocity_threshold; double filter_process_noise; double filter_observation_noise; @@ -474,42 +464,44 @@ struct RollingStatus }; extern void velocity_scale_factor_estimate(const rtklib_msgs::msg::RtklibNav, const geometry_msgs::msg::TwistStamped, const VelocityScaleFactorParameter, - VelocityScaleFactorStatus*, eagleye_msgs::msg::VelocityScaleFactor*); + VelocityScaleFactorStatus*, geometry_msgs::msg::TwistStamped*, eagleye_msgs::msg::VelocityScaleFactor*); extern void velocity_scale_factor_estimate(const nmea_msgs::msg::Gprmc, const geometry_msgs::msg::TwistStamped, const VelocityScaleFactorParameter, - VelocityScaleFactorStatus*, eagleye_msgs::msg::VelocityScaleFactor*); -extern void distance_estimate(const eagleye_msgs::msg::VelocityScaleFactor, DistanceStatus*,eagleye_msgs::msg::Distance*); + VelocityScaleFactorStatus*, geometry_msgs::msg::TwistStamped*, eagleye_msgs::msg::VelocityScaleFactor*); +extern void distance_estimate(const geometry_msgs::msg::TwistStamped, DistanceStatus*, eagleye_msgs::msg::Distance*); extern void yawrate_offset_stop_estimate(const geometry_msgs::msg::TwistStamped, const sensor_msgs::msg::Imu, const YawrateOffsetStopParameter, YawrateOffsetStopStatus*, eagleye_msgs::msg::YawrateOffset*); -extern void yawrate_offset_estimate(const eagleye_msgs::msg::VelocityScaleFactor, const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::Heading, +extern void yawrate_offset_estimate(const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::Heading, const sensor_msgs::msg::Imu, const YawrateOffsetParameter, YawrateOffsetStatus*, eagleye_msgs::msg::YawrateOffset*); -extern void heading_estimate(const rtklib_msgs::msg::RtklibNav, const sensor_msgs::msg::Imu, const eagleye_msgs::msg::VelocityScaleFactor, +extern void heading_estimate(const rtklib_msgs::msg::RtklibNav, 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 nmea_msgs::msg::Gprmc, const sensor_msgs::msg::Imu, const eagleye_msgs::msg::VelocityScaleFactor, 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 eagleye_msgs::msg::VelocityScaleFactor, const eagleye_msgs::msg::Distance, - const eagleye_msgs::msg::Heading, const geometry_msgs::msg::Vector3Stamped, const PositionParameter, PositionStatus*, eagleye_msgs::msg::Position*); -extern void position_estimate(const nmea_msgs::msg::Gpgga, const eagleye_msgs::msg::VelocityScaleFactor, const eagleye_msgs::msg::Distance, +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 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*); +extern void position_estimate(const nmea_msgs::msg::Gpgga, 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*); -extern void slip_angle_estimate(const sensor_msgs::msg::Imu, const eagleye_msgs::msg::VelocityScaleFactor,const eagleye_msgs::msg::YawrateOffset, - const eagleye_msgs::msg::YawrateOffset,const SlipangleParameter,eagleye_msgs::msg::SlipAngle*); -extern void slip_coefficient_estimate(const sensor_msgs::msg::Imu, const rtklib_msgs::msg::RtklibNav,const eagleye_msgs::msg::VelocityScaleFactor, +extern void slip_angle_estimate(const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::StatusStamped, + const eagleye_msgs::msg::YawrateOffset, const eagleye_msgs::msg::YawrateOffset,const SlipangleParameter,eagleye_msgs::msg::SlipAngle*); +extern void slip_coefficient_estimate(const sensor_msgs::msg::Imu, const rtklib_msgs::msg::RtklibNav,const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::Heading,const SlipCoefficientParameter, SlipCoefficientStatus*,double*); -extern void smoothing_estimate(const rtklib_msgs::msg::RtklibNav,const eagleye_msgs::msg::VelocityScaleFactor,const SmoothingParameter,SmoothingStatus*, +extern void smoothing_estimate(const rtklib_msgs::msg::RtklibNav,const geometry_msgs::msg::TwistStamped,const SmoothingParameter,SmoothingStatus*, eagleye_msgs::msg::Position*); -extern void trajectory_estimate(const sensor_msgs::msg::Imu, const eagleye_msgs::msg::VelocityScaleFactor,const eagleye_msgs::msg::Heading, - const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::YawrateOffset,const TrajectoryParameter,TrajectoryStatus*, +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*); -extern void heading_interpolate_estimate(const sensor_msgs::msg::Imu, const eagleye_msgs::msg::VelocityScaleFactor, - 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*); -extern void position_interpolate_estimate(const eagleye_msgs::msg::Position,const geometry_msgs::msg::Vector3Stamped,const eagleye_msgs::msg::Position, - const eagleye_msgs::msg::Height,const PositionInterpolateParameter,PositionInterpolateStatus*,eagleye_msgs::msg::Position*,sensor_msgs::msg::NavSatFix*); -extern void pitching_estimate(const sensor_msgs::msg::Imu, const nmea_msgs::msg::Gpgga, const eagleye_msgs::msg::VelocityScaleFactor, +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*); +extern void position_interpolate_estimate(const eagleye_msgs::msg::Position, const geometry_msgs::msg::Vector3Stamped, const eagleye_msgs::msg::Position, + const eagleye_msgs::msg::Height,const PositionInterpolateParameter, PositionInterpolateStatus*,eagleye_msgs::msg::Position*,sensor_msgs::msg::NavSatFix*); +extern void pitching_estimate(const sensor_msgs::msg::Imu, const nmea_msgs::msg::Gpgga, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::Distance,const HeightParameter,HeightStatus*,eagleye_msgs::msg::Height*,eagleye_msgs::msg::Pitching*,eagleye_msgs::msg::AccXOffset*, eagleye_msgs::msg::AccXScaleFactor*); -extern void trajectory3d_estimate(const sensor_msgs::msg::Imu, const eagleye_msgs::msg::VelocityScaleFactor,const eagleye_msgs::msg::Heading, +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*); extern void angular_velocity_offset_stop_estimate(const geometry_msgs::msg::TwistStamped, const sensor_msgs::msg::Imu, @@ -518,12 +510,192 @@ extern void rtk_deadreckoning_estimate(const rtklib_msgs::msg::RtklibNav,const g const eagleye_msgs::msg::Heading,const RtkDeadreckoningParameter,RtkDeadreckoningStatus*,eagleye_msgs::msg::Position*,sensor_msgs::msg::NavSatFix*); extern void rtk_deadreckoning_estimate(const geometry_msgs::msg::Vector3Stamped,const nmea_msgs::msg::Gpgga, const eagleye_msgs::msg::Heading, const RtkDeadreckoningParameter,RtkDeadreckoningStatus*,eagleye_msgs::msg::Position*,sensor_msgs::msg::NavSatFix*); -extern void rtk_heading_estimate(const nmea_msgs::msg::Gpgga, const sensor_msgs::msg::Imu, const eagleye_msgs::msg::VelocityScaleFactor, +extern void rtk_heading_estimate(const nmea_msgs::msg::Gpgga, const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::Distance,const eagleye_msgs::msg::YawrateOffset, const eagleye_msgs::msg::YawrateOffset, const eagleye_msgs::msg::SlipAngle, const eagleye_msgs::msg::Heading, const RtkHeadingParameter, RtkHeadingStatus*,eagleye_msgs::msg::Heading*); -extern void enable_additional_rolling_estimate(const eagleye_msgs::msg::VelocityScaleFactor,const eagleye_msgs::msg::YawrateOffset ,const eagleye_msgs::msg::YawrateOffset, - const eagleye_msgs::msg::Distance,const sensor_msgs::msg::Imu,const geometry_msgs::msg::PoseStamped,const eagleye_msgs::msg::AngularVelocityOffset, +extern void enable_additional_rolling_estimate(const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::StatusStamped, + const eagleye_msgs::msg::YawrateOffset ,const eagleye_msgs::msg::YawrateOffset, const eagleye_msgs::msg::Distance, + const sensor_msgs::msg::Imu,const geometry_msgs::msg::PoseStamped,const eagleye_msgs::msg::AngularVelocityOffset, const EnableAdditionalRollingParameter,EnableAdditionalRollingStatus*,eagleye_msgs::msg::Rolling*,eagleye_msgs::msg::AccYOffset*); -extern void rolling_estimate(const sensor_msgs::msg::Imu,const eagleye_msgs::msg::VelocityScaleFactor,const eagleye_msgs::msg::YawrateOffset ,const eagleye_msgs::msg::YawrateOffset, +extern void rolling_estimate(const sensor_msgs::msg::Imu,const geometry_msgs::msg::TwistStamped,const eagleye_msgs::msg::YawrateOffset ,const eagleye_msgs::msg::YawrateOffset, const RollingParameter,RollingStatus*,eagleye_msgs::msg::Rolling*); + +class VelocityEstimator +{ + public: + VelocityEstimator(); + + // Parameter setting + void setParam(std::string yaml_file); + + // Main estimate function + void VelocityEstimate(const sensor_msgs::msg::Imu, const rtklib_msgs::msg::RtklibNav, const nmea_msgs::msg::Gpgga, geometry_msgs::msg::TwistStamped*); + + eagleye_msgs::msg::Status getStatus(); + + private: + + class PitchrateOffsetStopEstimator + { + public: + PitchrateOffsetStopEstimator(); + + void setParam(std::string yaml_file); + bool PitchrateOffsetStopEstimate(double pitchrate, double stop_status); + + double pitchrate_offset; + eagleye_msgs::msg::Status pitchrate_offset_status; + + private: + struct Param + { + int buffer_count_max; + }; + PitchrateOffsetStopEstimator::Param param; + + int stop_count; + std::vector pitchrate_buffer; + }; + + class PitchingEstimator + { + public: + PitchingEstimator(); + + void setParam(std::string yaml_file); + bool PitchingEstimate(double imu_time_last, double doppler_velocity, double rtkfix_velocity, + double pitchrate, double pitchrate_offset, double rtkfix_pitching, + bool navsat_update_status, bool stop_status); + + double pitching; + eagleye_msgs::msg::Status pitching_status; + + private: + struct Param + { + int buffer_max; + double outlier_threshold; + double estimated_velocity_threshold; + double estimated_gnss_coefficient; + double estimated_coefficient; + }; + PitchingEstimator::Param param; + + std::vector time_buffer; + std::vector corrected_pitchrate_buffer; + std::vector rtkfix_pitching_buffer; + std::vector use_gnss_status_buffer; + std::vector navsat_update_status_buffer; + }; + + class AccelerationOffsetEstimator + { + public: + AccelerationOffsetEstimator(); + + void setParam(std::string yaml_file); + bool AccelerationOffsetEstimate(double imu_time_last, double rtkfix_velocity, double pitching, + double acceleration, bool navsat_update_status); + + double filtered_acceleration; + double acceleration_offset; + eagleye_msgs::msg::Status acceleration_offset_status; + + private: + struct Param + { + double buffer_min; + double buffer_max; + double filter_process_noise; + double filter_observation_noise; + }; + AccelerationOffsetEstimator::Param param; + + double acceleration_last; + double acceleration_variance_last; + std::vector time_buffer; + std::vector pitching_buffer; + std::vector filtered_acceleration_buffer; + std::vector rtkfix_velocity_buffer; + std::vector navsat_update_status_buffer; + }; + + // Parameter + struct Param + { + double ecef_base_pos_x; + double ecef_base_pos_y; + double ecef_base_pos_z; + bool use_ecef_base_position; + + double gga_downsample_time; + double stop_judgment_velocity_threshold; + double stop_judgment_buffer_maxnum; + double variance_threshold; + + // doppler fusion parameter + double buffer_max; + double estimated_gnss_coefficient; + double estimated_coefficient; + double outlier_threshold; + }; + VelocityEstimator::Param param; + + // imu variables + double acceleration; + double pitchrate; + double imu_time_last; + + // rtklib_nav variables + double doppler_velocity; + double rtklib_nav_time_last; + bool rtklib_update_status; + + // gga variables + double ecef_base_position[3]; + bool ecef_base_position_status; + double gga_time_last; + double gga_position_enu_last[3]; + int gga_status_last; + double rtkfix_velocity; + double rtkfix_pitching; + bool navsat_update_status; + + // stop judgment variables + bool stop_status; + std::vector angular_velocity_x_buffer; + std::vector angular_velocity_y_buffer; + std::vector angular_velocity_z_buffer; + + // // PitchrateOffsetStopEstimator variables + PitchrateOffsetStopEstimator pitchrate_offset_stop_estimator; + double pitchrate_offset; + eagleye_msgs::msg::Status pitchrate_offset_status; + + // PitchingEstimator variables + PitchingEstimator pitching_estimator; + double pitching; + eagleye_msgs::msg::Status pitching_status; + + // AccelerationOffsetEstimator + AccelerationOffsetEstimator acceleration_offset_estimator; + double filtered_acceleration; + double acceleration_offset; + eagleye_msgs::msg::Status acceleration_offset_status; + + //DopplerFusion + double velocity; + std::vector time_buffer; + std::vector doppler_velocity_buffer; + std::vector corrected_acceleration_buffer; + std::vector rtklib_update_status_buffer; + eagleye_msgs::msg::Status velocity_status; + + bool updateImu(const sensor_msgs::msg::Imu); + bool updateRtklibNav(const rtklib_msgs::msg::RtklibNav); + bool updateGGA(const nmea_msgs::msg::Gpgga); + bool StopJudgment(const sensor_msgs::msg::Imu); + bool DopplerFusion(); +}; + #endif /*NAVIGATION_H */ diff --git a/eagleye_core/navigation/package.xml b/eagleye_core/navigation/package.xml index 91bc5b34..7378b78c 100755 --- a/eagleye_core/navigation/package.xml +++ b/eagleye_core/navigation/package.xml @@ -8,7 +8,7 @@ BSD ament_cmake - + yaml_cpp_vendor rclcpp geodesy eagleye_coordinate diff --git a/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp b/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp index 80241419..d529770d 100755 --- a/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp +++ b/eagleye_core/navigation/src/angular_velocity_offset_stop.cpp @@ -45,33 +45,15 @@ void angular_velocity_offset_stop_estimate(const geometry_msgs::msg::TwistStampe // data buffer generate if (angular_velocity_stop_status->estimate_start_status == false) { - if (angular_velocity_stop_parameter.reverse_imu == false) - { - angular_velocity_stop_status->rollrate_buffer.push_back(imu.angular_velocity.x); - angular_velocity_stop_status->pitchrate_buffer.push_back(imu.angular_velocity.y); - angular_velocity_stop_status->yawrate_buffer.push_back(imu.angular_velocity.z); - } - else if (angular_velocity_stop_parameter.reverse_imu == true) - { - angular_velocity_stop_status->rollrate_buffer.push_back(imu.angular_velocity.x); - angular_velocity_stop_status->pitchrate_buffer.push_back(imu.angular_velocity.y); - angular_velocity_stop_status->yawrate_buffer.push_back(-1 * imu.angular_velocity.z); - } + angular_velocity_stop_status->rollrate_buffer.push_back(imu.angular_velocity.x); + angular_velocity_stop_status->pitchrate_buffer.push_back(imu.angular_velocity.y); + angular_velocity_stop_status->yawrate_buffer.push_back(imu.angular_velocity.z); } else if ( std::fabs(std::fabs(angular_velocity_stop_status->yawrate_offset_stop_last) - std::fabs(imu.angular_velocity.z)) < angular_velocity_stop_parameter.outlier_threshold && angular_velocity_stop_status->estimate_start_status == true) { - if (angular_velocity_stop_parameter.reverse_imu == false) - { - angular_velocity_stop_status->rollrate_buffer.push_back(imu.angular_velocity.x); - angular_velocity_stop_status->pitchrate_buffer.push_back(imu.angular_velocity.y); - angular_velocity_stop_status->yawrate_buffer.push_back(imu.angular_velocity.z); - } - else if (angular_velocity_stop_parameter.reverse_imu == true) - { - angular_velocity_stop_status->rollrate_buffer.push_back(imu.angular_velocity.x); - angular_velocity_stop_status->pitchrate_buffer.push_back(imu.angular_velocity.y); - angular_velocity_stop_status->yawrate_buffer.push_back(-1 * imu.angular_velocity.z); - } + angular_velocity_stop_status->rollrate_buffer.push_back(imu.angular_velocity.x); + angular_velocity_stop_status->pitchrate_buffer.push_back(imu.angular_velocity.y); + angular_velocity_stop_status->yawrate_buffer.push_back(imu.angular_velocity.z); } rollrate_buffer_length = std::distance(angular_velocity_stop_status->rollrate_buffer.begin(), angular_velocity_stop_status->rollrate_buffer.end()); diff --git a/eagleye_core/navigation/src/distance.cpp b/eagleye_core/navigation/src/distance.cpp index 76edae82..7d6f497a 100755 --- a/eagleye_core/navigation/src/distance.cpp +++ b/eagleye_core/navigation/src/distance.cpp @@ -31,20 +31,21 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -void distance_estimate(const eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, DistanceStatus* distance_status,eagleye_msgs::msg::Distance* distance) +void distance_estimate(const geometry_msgs::msg::TwistStamped velocity, DistanceStatus* distance_status,eagleye_msgs::msg::Distance* distance) { - rclcpp::Time ros_clock(velocity_scale_factor.header.stamp); - auto velocity_scale_factor_time = ros_clock.seconds(); + rclcpp::Time ros_clock(velocity.header.stamp); + auto velocity_time = ros_clock.seconds(); if(distance_status->time_last != 0) { - distance->distance = distance->distance + velocity_scale_factor.correction_velocity.linear.x * std::abs((velocity_scale_factor_time - distance_status->time_last)); + distance->distance = distance->distance + velocity.twist.linear.x * std::abs((velocity_time - + distance_status->time_last)); distance->status.enabled_status = distance->status.estimate_status = true; - distance_status->time_last = velocity_scale_factor_time; + distance_status->time_last = velocity_time; } else { - distance_status->time_last = velocity_scale_factor_time; + distance_status->time_last = velocity_time; } } diff --git a/eagleye_core/navigation/src/enable_additional_rolling.cpp b/eagleye_core/navigation/src/enable_additional_rolling.cpp index cbbaedba..780c2fec 100644 --- a/eagleye_core/navigation/src/enable_additional_rolling.cpp +++ b/eagleye_core/navigation/src/enable_additional_rolling.cpp @@ -33,11 +33,11 @@ #define g 9.80665 -void enable_additional_rolling_estimate(const eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, +void enable_additional_rolling_estimate(const geometry_msgs::msg::TwistStamped velocity, const eagleye_msgs::msg::StatusStamped velocity_status, const eagleye_msgs::msg::YawrateOffset yawrate_offset_2nd,const eagleye_msgs::msg::YawrateOffset yawrate_offset_stop, - const eagleye_msgs::msg::Distance distance,const sensor_msgs::msg::Imu imu,const geometry_msgs::msg::PoseStamped localization_pose, - const eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop,const EnableAdditionalRollingParameter rolling_parameter, - EnableAdditionalRollingStatus* rolling_status,eagleye_msgs::msg::Rolling* rolling_angle,eagleye_msgs::msg::AccYOffset* acc_y_offset) + const eagleye_msgs::msg::Distance distance, const sensor_msgs::msg::Imu imu, const geometry_msgs::msg::PoseStamped localization_pose, + const eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop, const EnableAdditionalRollingParameter rolling_parameter, + EnableAdditionalRollingStatus* rolling_status, eagleye_msgs::msg::Rolling* rolling_angle, eagleye_msgs::msg::AccYOffset* acc_y_offset) { bool acc_offset_status = false; bool rolling_buffer_status = false; @@ -56,50 +56,24 @@ void enable_additional_rolling_estimate(const eagleye_msgs::msg::VelocityScaleFa rclcpp::Time localization_pose_clock(localization_pose.header.stamp); double localization_pose_time = localization_pose_clock.seconds(); - /// reverse_imu /// - if (!rolling_parameter.reverse_imu) - { - rolling_status->yawrate = imu.angular_velocity.z; - } - else - { - rolling_status->yawrate = -1 * imu.angular_velocity.z; - } - - /// reverse_imu rollrate /// - if (!rolling_parameter.reverse_imu_angular_velocity_x) - { - rolling_status->rollrate = imu.angular_velocity.x; - rolling_status->rollrate_offset_stop = angular_velocity_offset_stop.angular_velocity_offset.x; - } - else - { - rolling_status->rollrate = -1* imu.angular_velocity.x; - rolling_status->rollrate_offset_stop = -1 * angular_velocity_offset_stop.angular_velocity_offset.x; - } + rolling_status->yawrate = imu.angular_velocity.z; - /// reverse_imu y acc /// - if (!rolling_parameter.reverse_imu_linear_acceleration_y) - { - rolling_status->imu_acceleration_y = imu.linear_acceleration.y; - } - else - { - rolling_status->imu_acceleration_y = -1* imu.linear_acceleration.y; - } + rolling_status->rollrate = imu.angular_velocity.x; + rolling_status->rollrate_offset_stop = angular_velocity_offset_stop.angular_velocity_offset.x; + rolling_status->imu_acceleration_y = imu.linear_acceleration.y; // data buffer - if (rolling_status->imu_time_buffer.size() < rolling_parameter.imu_buffer_num && velocity_scale_factor.status.enabled_status) + if (rolling_status->imu_time_buffer.size() < rolling_parameter.imu_buffer_num && velocity_status.status.enabled_status) { rolling_status->imu_time_buffer.push_back(imu_time); rolling_status->yawrate_buffer.push_back(rolling_status->yawrate); - rolling_status->velocity_buffer.push_back(velocity_scale_factor.correction_velocity.linear.x); + rolling_status->velocity_buffer.push_back(velocity.twist.linear.x); rolling_status->yawrate_offset_buffer.push_back(yawrate_offset_2nd.yawrate_offset); rolling_status->acceleration_y_buffer.push_back(rolling_status->imu_acceleration_y); rolling_status->distance_buffer.push_back(distance.distance); } - else if (velocity_scale_factor.status.enabled_status) + else if (velocity_status.status.enabled_status) { rolling_status->imu_time_buffer.erase(rolling_status->imu_time_buffer.begin()); rolling_status->yawrate_buffer.erase(rolling_status->yawrate_buffer.begin()); @@ -110,7 +84,7 @@ void enable_additional_rolling_estimate(const eagleye_msgs::msg::VelocityScaleFa rolling_status->imu_time_buffer.push_back(imu_time); rolling_status->yawrate_buffer.push_back(rolling_status->yawrate); - rolling_status->velocity_buffer.push_back(velocity_scale_factor.correction_velocity.linear.x); + rolling_status->velocity_buffer.push_back(velocity.twist.linear.x); rolling_status->yawrate_offset_buffer.push_back(yawrate_offset_2nd.yawrate_offset); rolling_status->acceleration_y_buffer.push_back(rolling_status->imu_acceleration_y); rolling_status->distance_buffer.push_back(distance.distance); @@ -163,13 +137,15 @@ void enable_additional_rolling_estimate(const eagleye_msgs::msg::VelocityScaleFa /// estimated rolling angle /// if (acc_y_offset->status.enabled_status) { - if (velocity_scale_factor.correction_velocity.linear.x > rolling_parameter.stop_judgment_velocity_threshold) + if (velocity.twist.linear.x > rolling_parameter.stop_judgment_velocity_threshold) { - rolling_estimated_tmp = std::asin((velocity_scale_factor.correction_velocity.linear.x*(rolling_status->yawrate+yawrate_offset_2nd.yawrate_offset)/g)-(rolling_status->imu_acceleration_y-acc_y_offset->acc_y_offset)/g); + rolling_estimated_tmp = std::asin((velocity.twist.linear.x*(rolling_status->yawrate+yawrate_offset_2nd.yawrate_offset) / g) + - (rolling_status->imu_acceleration_y-acc_y_offset->acc_y_offset) / g); } else { - rolling_estimated_tmp = std::asin((velocity_scale_factor.correction_velocity.linear.x*(rolling_status->yawrate+yawrate_offset_stop.yawrate_offset)/g)-(rolling_status->imu_acceleration_y-acc_y_offset->acc_y_offset)/g); + rolling_estimated_tmp = std::asin((velocity.twist.linear.x*(rolling_status->yawrate+yawrate_offset_stop.yawrate_offset) / g) + - (rolling_status->imu_acceleration_y-acc_y_offset->acc_y_offset) / g); } } @@ -194,7 +170,7 @@ void enable_additional_rolling_estimate(const eagleye_msgs::msg::VelocityScaleFa { rolling_status->rolling_estimated_buffer.push_back(rolling_estimated_tmp); } - else if (velocity_scale_factor.status.enabled_status && acc_y_offset->status.enabled_status) + else if (velocity_status.status.enabled_status && acc_y_offset->status.enabled_status) { rolling_status->rolling_estimated_buffer.erase(rolling_status->rolling_estimated_buffer.begin()); rolling_status->rolling_estimated_buffer.push_back(rolling_estimated_tmp); diff --git a/eagleye_core/navigation/src/heading.cpp b/eagleye_core/navigation/src/heading.cpp index 5c6181f5..f85fa883 100755 --- a/eagleye_core/navigation/src/heading.cpp +++ b/eagleye_core/navigation/src/heading.cpp @@ -31,9 +31,9 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -void heading_estimate_(sensor_msgs::msg::Imu imu,eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor,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) +void heading_estimate_(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) { int i,index_max; double yawrate = 0.0; @@ -57,19 +57,12 @@ void heading_estimate_(sensor_msgs::msg::Imu imu,eagleye_msgs::msg::VelocityScal heading_status->estimated_number = heading_parameter.estimated_number_max; } - if (heading_parameter.reverse_imu == false) - { - yawrate = imu.angular_velocity.z; - } - else if (heading_parameter.reverse_imu == true) - { - yawrate = -1 * imu.angular_velocity.z; - } + yawrate = imu.angular_velocity.z; // data buffer generate heading_status->time_buffer .push_back(imu_time); heading_status->yawrate_buffer .push_back(yawrate); - heading_status->correction_velocity_buffer .push_back(velocity_scale_factor.correction_velocity.linear.x); + heading_status->correction_velocity_buffer .push_back(velocity.twist.linear.x); heading_status->yawrate_offset_stop_buffer .push_back(yawrate_offset_stop.yawrate_offset); heading_status->yawrate_offset_buffer .push_back(yawrate_offset.yawrate_offset); heading_status->slip_angle_buffer .push_back(slip_angle.slip_angle); @@ -245,7 +238,7 @@ void heading_estimate_(sensor_msgs::msg::Imu imu,eagleye_msgs::msg::VelocityScal } } -void heading_estimate(rtklib_msgs::msg::RtklibNav rtklib_nav,sensor_msgs::msg::Imu imu,eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, +void heading_estimate(rtklib_msgs::msg::RtklibNav rtklib_nav,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) { @@ -298,10 +291,10 @@ void heading_estimate(rtklib_msgs::msg::RtklibNav rtklib_nav,sensor_msgs::msg::I heading_status->heading_angle_buffer .push_back(doppler_heading_angle); heading_status->gnss_status_buffer .push_back(gnss_status); - heading_estimate_(imu,velocity_scale_factor,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,heading_status,heading); + heading_estimate_(imu, velocity, yawrate_offset_stop, yawrate_offset, slip_angle, heading_interpolate, heading_parameter, heading_status, heading); } -void heading_estimate(const nmea_msgs::msg::Gprmc nmea_rmc,sensor_msgs::msg::Imu imu,eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, +void heading_estimate(const nmea_msgs::msg::Gprmc nmea_rmc,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) { @@ -321,8 +314,8 @@ void heading_estimate(const nmea_msgs::msg::Gprmc nmea_rmc,sensor_msgs::msg::Imu heading_status->rmc_time_last = nmea_rmc.utc_seconds; } - heading_status->heading_angle_buffer .push_back(doppler_heading_angle); - heading_status->gnss_status_buffer .push_back(gnss_status); + heading_status->heading_angle_buffer.push_back(doppler_heading_angle); + heading_status->gnss_status_buffer.push_back(gnss_status); - heading_estimate_(imu,velocity_scale_factor,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,heading_status,heading); + heading_estimate_(imu, velocity, yawrate_offset_stop, yawrate_offset, slip_angle, heading_interpolate, heading_parameter, heading_status, heading); } \ 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 d8fbf114..878abd4f 100755 --- a/eagleye_core/navigation/src/heading_interpolate.cpp +++ b/eagleye_core/navigation/src/heading_interpolate.cpp @@ -31,7 +31,10 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -void heading_interpolate_estimate(const sensor_msgs::msg::Imu imu, const eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, const eagleye_msgs::msg::YawrateOffset yawrate_offset_stop,const eagleye_msgs::msg::YawrateOffset yawrate_offset,const eagleye_msgs::msg::Heading heading,const eagleye_msgs::msg::SlipAngle slip_angle,const HeadingInterpolateParameter heading_interpolate_parameter, HeadingInterpolateStatus* heading_interpolate_status,eagleye_msgs::msg::Heading* heading_interpolate) +void heading_interpolate_estimate(const sensor_msgs::msg::Imu imu, const geometry_msgs::msg::TwistStamped velocity, + const eagleye_msgs::msg::YawrateOffset yawrate_offset_stop, const eagleye_msgs::msg::YawrateOffset yawrate_offset, const eagleye_msgs::msg::Heading heading, + const eagleye_msgs::msg::SlipAngle slip_angle,const HeadingInterpolateParameter heading_interpolate_parameter, HeadingInterpolateStatus* heading_interpolate_status, + eagleye_msgs::msg::Heading* heading_interpolate) { int i; int estimate_index = 0; @@ -45,16 +48,9 @@ void heading_interpolate_estimate(const sensor_msgs::msg::Imu imu, const eagleye auto heading_time = ros_clock.seconds(); auto imu_time = ros_clock2.seconds(); - if (heading_interpolate_parameter.reverse_imu == false) - { - yawrate = imu.angular_velocity.z; - } - else if (heading_interpolate_parameter.reverse_imu == true) - { - yawrate = -1 * imu.angular_velocity.z; - } + yawrate = imu.angular_velocity.z; - if (std::abs(velocity_scale_factor.correction_velocity.linear.x) > heading_interpolate_parameter.stop_judgment_velocity_threshold) + if (std::abs(velocity.twist.linear.x) > heading_interpolate_parameter.stop_judgment_velocity_threshold) { yawrate = yawrate + yawrate_offset.yawrate_offset; } @@ -83,7 +79,7 @@ void heading_interpolate_estimate(const sensor_msgs::msg::Imu imu, const eagleye heading_estimate_status = false; } - if(heading_interpolate_status->time_last != 0 && std::abs(velocity_scale_factor.correction_velocity.linear.x) > heading_interpolate_parameter.stop_judgment_velocity_threshold) + if(heading_interpolate_status->time_last != 0 && std::abs(velocity.twist.linear.x) > heading_interpolate_parameter.stop_judgment_velocity_threshold) { heading_interpolate_status->provisional_heading_angle = heading_interpolate_status->provisional_heading_angle + (yawrate * (imu_time - heading_interpolate_status->time_last)); } diff --git a/eagleye_core/navigation/src/height.cpp b/eagleye_core/navigation/src/height.cpp index 27bedbb9..1a05659b 100755 --- a/eagleye_core/navigation/src/height.cpp +++ b/eagleye_core/navigation/src/height.cpp @@ -33,7 +33,7 @@ #define g 9.80665 -void pitching_estimate(const sensor_msgs::msg::Imu imu,const nmea_msgs::msg::Gpgga gga,const eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, +void pitching_estimate(const sensor_msgs::msg::Imu imu, const nmea_msgs::msg::Gpgga gga, const geometry_msgs::msg::TwistStamped velocity, const eagleye_msgs::msg::Distance distance,const HeightParameter height_parameter,HeightStatus* height_status,eagleye_msgs::msg::Height* height, eagleye_msgs::msg::Pitching* pitching,eagleye_msgs::msg::AccXOffset* acc_x_offset,eagleye_msgs::msg::AccXScaleFactor* acc_x_scale_factor) { @@ -90,11 +90,11 @@ void pitching_estimate(const sensor_msgs::msg::Imu imu,const nmea_msgs::msg::Gpg height_status->flag_reliability = false; /// relative_height /// - if (velocity_scale_factor.correction_velocity.linear.x > 0 && height_status->time_last != 0) + if (velocity.twist.linear.x > 0 && height_status->time_last != 0) { - height_status->relative_height_G += imu.linear_acceleration.x * velocity_scale_factor.correction_velocity.linear.x*(imu_time-height_status->time_last)/g; - height_status->relative_height_diffvel += - (velocity_scale_factor.correction_velocity.linear.x-height_status->correction_velocity_x_last) * velocity_scale_factor.correction_velocity.linear.x/g; - height_status->relative_height_offset += velocity_scale_factor.correction_velocity.linear.x*(imu_time-height_status->time_last)/g; + height_status->relative_height_G += imu.linear_acceleration.x * velocity.twist.linear.x*(imu_time-height_status->time_last)/g; + height_status->relative_height_diffvel += - (velocity.twist.linear.x-height_status->correction_velocity_x_last) * velocity.twist.linear.x/g; + height_status->relative_height_offset += velocity.twist.linear.x*(imu_time-height_status->time_last)/g; correction_relative_height = height_status->relative_height_G + height_status->relative_height_offset + height_status->relative_height_diffvel; } @@ -106,7 +106,7 @@ void pitching_estimate(const sensor_msgs::msg::Imu imu,const nmea_msgs::msg::Gpg height_status->relative_height_diffvel_buffer.push_back(height_status->relative_height_diffvel); height_status->relative_height_offset_buffer.push_back(height_status->relative_height_offset); height_status->correction_relative_height_buffer.push_back(correction_relative_height); - height_status->correction_velocity_buffer.push_back(velocity_scale_factor.correction_velocity.linear.x); + height_status->correction_velocity_buffer.push_back(velocity.twist.linear.x); height_status->distance_buffer.push_back(distance.distance); data_status = true; @@ -200,7 +200,7 @@ void pitching_estimate(const sensor_msgs::msg::Imu imu,const nmea_msgs::msg::Gpg /// height estimate /// if (height_status->estimate_start_status == true) { - if (distance.distance > height_parameter.estimated_distance && gnss_status == true && gps_quality == 4 && data_status == true && velocity_scale_factor.correction_velocity.linear.x > height_parameter.estimated_velocity_threshold ) + if (distance.distance > height_parameter.estimated_distance && gnss_status == true && gps_quality == 4 && data_status == true && velocity.twist.linear.x > height_parameter.estimated_velocity_threshold ) { height_status->correction_relative_height_buffer2.clear(); height_status->height_buffer2.clear(); @@ -362,8 +362,8 @@ void pitching_estimate(const sensor_msgs::msg::Imu imu,const nmea_msgs::msg::Gpg else { height_status->height_last += ((imu.linear_acceleration.x * height_status->acceleration_SF_linear_x_last + height_status->acceleration_offset_linear_x_last) - - (velocity_scale_factor.correction_velocity.linear.x-height_status->correction_velocity_x_last)/(imu_time-height_status->time_last)) - * velocity_scale_factor.correction_velocity.linear.x*(imu_time-height_status->time_last)/g; + - (velocity.twist.linear.x-height_status->correction_velocity_x_last)/(imu_time-height_status->time_last)) + * velocity.twist.linear.x*(imu_time-height_status->time_last)/g; height->status.enabled_status = true; height->status.estimate_status = false; } @@ -371,7 +371,7 @@ void pitching_estimate(const sensor_msgs::msg::Imu imu,const nmea_msgs::msg::Gpg /// pitch /// correction_acceleration_linear_x = imu.linear_acceleration.x * height_status->acceleration_SF_linear_x_last + height_status->acceleration_offset_linear_x_last; - height_status->acc_buffer.push_back((correction_acceleration_linear_x - (velocity_scale_factor.correction_velocity.linear.x-height_status->correction_velocity_x_last)/(imu_time-height_status->time_last))); + height_status->acc_buffer.push_back((correction_acceleration_linear_x - (velocity.twist.linear.x-height_status->correction_velocity_x_last)/(imu_time-height_status->time_last))); data_num_acc = height_status->acc_buffer.size(); if (data_num_acc > height_parameter.average_num) @@ -416,6 +416,6 @@ if (data_num_acc >= height_parameter.average_num && height_status->estimate_star pitching->pitching_angle = tmp_pitch; height_status->time_last = imu_time; - height_status->correction_velocity_x_last = velocity_scale_factor.correction_velocity.linear.x; + height_status->correction_velocity_x_last = velocity.twist.linear.x; height_status->pitching_angle_last = tmp_pitch; } diff --git a/eagleye_core/navigation/src/position.cpp b/eagleye_core/navigation/src/position.cpp index b9192563..5ae75c05 100755 --- a/eagleye_core/navigation/src/position.cpp +++ b/eagleye_core/navigation/src/position.cpp @@ -31,13 +31,13 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -void position_estimate_(eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor,eagleye_msgs::msg::Distance distance, +void position_estimate_(geometry_msgs::msg::TwistStamped velocity,eagleye_msgs::msg::StatusStamped velocity_status,eagleye_msgs::msg::Distance distance, eagleye_msgs::msg::Heading heading_interpolate_3rd,geometry_msgs::msg::Vector3Stamped enu_vel,PositionParameter position_parameter, PositionStatus* position_status, eagleye_msgs::msg::Position* enu_absolute_pos) { int i; - int estimated_number_max = position_parameter.estimated_distance/position_parameter.separation_distance; + int estimated_number_max = position_parameter.estimated_distance / position_parameter.separation_distance; int max_x_index, max_y_index; double enu_pos[3]; double avg_x, avg_y, avg_z; @@ -94,7 +94,7 @@ void position_estimate_(eagleye_msgs::msg::VelocityScaleFactor velocity_scale_fa gnss_status = false; } - if (heading_interpolate_3rd.status.estimate_status == true && velocity_scale_factor.status.enabled_status == true) + if (heading_interpolate_3rd.status.estimate_status == true && velocity_status.status.enabled_status == true) { heading_interpolate_3rd.status.estimate_status = false; //in order to prevent being judged many times ++position_status->heading_estimate_status_count; @@ -125,7 +125,7 @@ void position_estimate_(eagleye_msgs::msg::VelocityScaleFactor velocity_scale_fa position_status->enu_pos_y_buffer.push_back(enu_pos[1]); //enu_pos_z_buffer.push_back(enu_pos[2]); position_status->enu_pos_z_buffer.push_back(0); - position_status->correction_velocity_buffer.push_back(velocity_scale_factor.correction_velocity.linear.x); + position_status->correction_velocity_buffer.push_back(velocity.twist.linear.x); position_status->enu_relative_pos_x_buffer.push_back(position_status->enu_relative_pos_x); position_status->enu_relative_pos_y_buffer.push_back(position_status->enu_relative_pos_y); //position_status->enu_relative_pos_z_buffer.push_back(position_status->enu_relative_pos_z); @@ -148,11 +148,11 @@ void position_estimate_(eagleye_msgs::msg::VelocityScaleFactor velocity_scale_fa position_status->distance_last = distance.distance; } - if (data_status == true) + if (data_status) { - if (distance.distance > position_parameter.estimated_distance && gnss_status == true && - velocity_scale_factor.correction_velocity.linear.x > position_parameter.estimated_velocity_threshold && position_status->heading_estimate_status_count > 0) + if (distance.distance > position_parameter.estimated_distance && gnss_status && + velocity.twist.linear.x > position_parameter.estimated_velocity_threshold && position_status->heading_estimate_status_count > 0) { std::vector distance_index; std::vector velocity_index; @@ -306,7 +306,9 @@ void position_estimate_(eagleye_msgs::msg::VelocityScaleFactor velocity_scale_fa data_status = false; } -void position_estimate(rtklib_msgs::msg::RtklibNav rtklib_nav,eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor,eagleye_msgs::msg::Distance distance,eagleye_msgs::msg::Heading heading_interpolate_3rd,geometry_msgs::msg::Vector3Stamped enu_vel,PositionParameter position_parameter, PositionStatus* position_status, eagleye_msgs::msg::Position* enu_absolute_pos) +void position_estimate(rtklib_msgs::msg::RtklibNav rtklib_nav, geometry_msgs::msg::TwistStamped velocity,eagleye_msgs::msg::StatusStamped velocity_status, + eagleye_msgs::msg::Distance distance,eagleye_msgs::msg::Heading heading_interpolate_3rd,geometry_msgs::msg::Vector3Stamped enu_vel, + PositionParameter position_parameter, PositionStatus* position_status, eagleye_msgs::msg::Position* enu_absolute_pos) { double enu_pos[3]; double ecef_pos[3]; @@ -374,10 +376,10 @@ void position_estimate(rtklib_msgs::msg::RtklibNav rtklib_nav,eagleye_msgs::msg: position_status->gnss_update_failure = gnss_update_failure; position_status->tow_last = rtklib_nav.tow; - position_estimate_(velocity_scale_factor, distance, heading_interpolate_3rd, enu_vel, position_parameter, position_status, enu_absolute_pos); + position_estimate_(velocity, velocity_status, distance, heading_interpolate_3rd, enu_vel, position_parameter, position_status, enu_absolute_pos); } -void position_estimate(nmea_msgs::msg::Gpgga gga,eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, +void position_estimate(nmea_msgs::msg::Gpgga gga,geometry_msgs::msg::TwistStamped velocity, eagleye_msgs::msg::StatusStamped velocity_status, eagleye_msgs::msg::Distance distance,eagleye_msgs::msg::Heading heading_interpolate_3rd,geometry_msgs::msg::Vector3Stamped enu_vel, PositionParameter position_parameter, PositionStatus* position_status, eagleye_msgs::msg::Position* enu_absolute_pos) { @@ -457,5 +459,5 @@ void position_estimate(nmea_msgs::msg::Gpgga gga,eagleye_msgs::msg::VelocityScal position_status->gnss_update_failure = gnss_update_failure; position_status->nmea_time_last = gga_time; - position_estimate_(velocity_scale_factor, distance, heading_interpolate_3rd, enu_vel, position_parameter, position_status, enu_absolute_pos); + position_estimate_(velocity, velocity_status, distance, heading_interpolate_3rd, enu_vel, position_parameter, position_status, enu_absolute_pos); } diff --git a/eagleye_core/navigation/src/position_interpolate.cpp b/eagleye_core/navigation/src/position_interpolate.cpp index c49caea3..b4f5bfc8 100755 --- a/eagleye_core/navigation/src/position_interpolate.cpp +++ b/eagleye_core/navigation/src/position_interpolate.cpp @@ -170,6 +170,11 @@ void position_interpolate_estimate(eagleye_msgs::msg::Position enu_absolute_pos, eagleye_fix->altitude = llh_pos[2]; + // TODO(Map IV): temporary covariance value + eagleye_fix->position_covariance[0] = 1.5 * 1.5; // [m^2] + eagleye_fix->position_covariance[4] = 1.5 * 1.5; // [m^2] + eagleye_fix->position_covariance[8] = 1.5 * 1.5; // [m^2] + } else { @@ -179,6 +184,9 @@ void position_interpolate_estimate(eagleye_msgs::msg::Position enu_absolute_pos, eagleye_fix->longitude = 0; eagleye_fix->latitude = 0; eagleye_fix->altitude = 0; + eagleye_fix->position_covariance[0] = 100 * 100; // [m^2] + eagleye_fix->position_covariance[4] = 100 * 100; // [m^2] + eagleye_fix->position_covariance[8] = 100 * 100; // [m^2] } position_interpolate_status->time_last = enu_vel_time; diff --git a/eagleye_core/navigation/src/rolling.cpp b/eagleye_core/navigation/src/rolling.cpp index 1360b74a..dfab5831 100644 --- a/eagleye_core/navigation/src/rolling.cpp +++ b/eagleye_core/navigation/src/rolling.cpp @@ -33,7 +33,7 @@ #define g 9.80665 -void rolling_estimate(sensor_msgs::msg::Imu imu, eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, +void rolling_estimate(sensor_msgs::msg::Imu imu, geometry_msgs::msg::TwistStamped correction_velocity, eagleye_msgs::msg::YawrateOffset yawrate_offset_stop, eagleye_msgs::msg::YawrateOffset yawrate_offset, RollingParameter rolling_parameter, RollingStatus* rolling_status, eagleye_msgs::msg::Rolling* rolling) { @@ -51,21 +51,15 @@ void rolling_estimate(sensor_msgs::msg::Imu imu, eagleye_msgs::msg::VelocityScal // Input data setup acceleration_y = imu.linear_acceleration.y; - velocity = velocity_scale_factor.correction_velocity.linear.x; + velocity = correction_velocity.twist.linear.x; if (std::abs(velocity) > rolling_parameter.stop_judgment_velocity_threshold) { - if (rolling_parameter.reverse_imu) - yawrate = -1 * (imu.angular_velocity.z + yawrate_offset.yawrate_offset); - else - yawrate = imu.angular_velocity.z + yawrate_offset.yawrate_offset; + yawrate = imu.angular_velocity.z + yawrate_offset.yawrate_offset; } else { - if (rolling_parameter.reverse_imu) - yawrate = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset); - else - yawrate = imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset; + yawrate = imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset; } if (!rolling_status->data_status) diff --git a/eagleye_core/navigation/src/rtk_deadreckoning.cpp b/eagleye_core/navigation/src/rtk_deadreckoning.cpp index e0c1f18c..effa0f2b 100755 --- a/eagleye_core/navigation/src/rtk_deadreckoning.cpp +++ b/eagleye_core/navigation/src/rtk_deadreckoning.cpp @@ -121,6 +121,10 @@ void rtk_deadreckoning_estimate_(geometry_msgs::msg::Vector3Stamped enu_vel, nme 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] enu_absolute_rtk_deadreckoning->enu_pos.x = enu_pos[0]; enu_absolute_rtk_deadreckoning->enu_pos.y = enu_pos[1]; diff --git a/eagleye_core/navigation/src/rtk_heading.cpp b/eagleye_core/navigation/src/rtk_heading.cpp index 8d609ba8..d1a97043 100755 --- a/eagleye_core/navigation/src/rtk_heading.cpp +++ b/eagleye_core/navigation/src/rtk_heading.cpp @@ -31,7 +31,9 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -void rtk_heading_estimate(nmea_msgs::msg::Gpgga gga,sensor_msgs::msg::Imu imu,eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor,eagleye_msgs::msg::Distance distance,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,RtkHeadingParameter heading_parameter, RtkHeadingStatus* heading_status,eagleye_msgs::msg::Heading* heading) +void rtk_heading_estimate(nmea_msgs::msg::Gpgga gga,sensor_msgs::msg::Imu imu,geometry_msgs::msg::TwistStamped velocity,eagleye_msgs::msg::Distance distance, + 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,RtkHeadingParameter heading_parameter, RtkHeadingStatus* heading_status,eagleye_msgs::msg::Heading* heading) { int i,index_max; @@ -58,14 +60,7 @@ void rtk_heading_estimate(nmea_msgs::msg::Gpgga gga,sensor_msgs::msg::Imu imu,ea heading_status->estimated_number = heading_parameter.estimated_number_max; } - if (heading_parameter.reverse_imu == false) - { - yawrate = imu.angular_velocity.z; - } - else if (heading_parameter.reverse_imu == true) - { - yawrate = -1 * imu.angular_velocity.z; - } + yawrate = imu.angular_velocity.z; // heading set // double enu_pos[3]; @@ -124,7 +119,7 @@ void rtk_heading_estimate(nmea_msgs::msg::Gpgga gga,sensor_msgs::msg::Imu imu,ea } if (heading_status->tow_last == gga_time || gga_time == 0 || rtk_heading_angle == 0 - || heading_status->last_rtk_heading_angle == rtk_heading_angle || velocity_scale_factor.correction_velocity.linear.x < heading_parameter.stop_judgment_velocity_threshold) + || heading_status->last_rtk_heading_angle == rtk_heading_angle || velocity.twist.linear.x < heading_parameter.stop_judgment_velocity_threshold) { gnss_status = false; rtk_heading_angle = 0; @@ -142,7 +137,7 @@ void rtk_heading_estimate(nmea_msgs::msg::Gpgga gga,sensor_msgs::msg::Imu imu,ea heading_status->time_buffer .push_back(imu_time); heading_status->heading_angle_buffer .push_back(rtk_heading_angle); heading_status->yawrate_buffer .push_back(yawrate); - heading_status->correction_velocity_buffer .push_back(velocity_scale_factor.correction_velocity.linear.x); + heading_status->correction_velocity_buffer .push_back(velocity.twist.linear.x); heading_status->yawrate_offset_stop_buffer .push_back(yawrate_offset_stop.yawrate_offset); heading_status->yawrate_offset_buffer .push_back(yawrate_offset.yawrate_offset); heading_status->slip_angle_buffer .push_back(slip_angle.slip_angle); diff --git a/eagleye_core/navigation/src/slip_angle.cpp b/eagleye_core/navigation/src/slip_angle.cpp index 94a80014..a6ff88c4 100755 --- a/eagleye_core/navigation/src/slip_angle.cpp +++ b/eagleye_core/navigation/src/slip_angle.cpp @@ -31,7 +31,9 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -void slip_angle_estimate(sensor_msgs::msg::Imu imu, eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, eagleye_msgs::msg::YawrateOffset yawrate_offset_stop, eagleye_msgs::msg::YawrateOffset yawrate_offset_2nd, SlipangleParameter slip_angle_parameter,eagleye_msgs::msg::SlipAngle* slip_angle) +void slip_angle_estimate(sensor_msgs::msg::Imu imu, geometry_msgs::msg::TwistStamped velocity, eagleye_msgs::msg::StatusStamped velocity_status, + eagleye_msgs::msg::YawrateOffset yawrate_offset_stop, eagleye_msgs::msg::YawrateOffset yawrate_offset_2nd, SlipangleParameter slip_angle_parameter, + eagleye_msgs::msg::SlipAngle* slip_angle) { int i; @@ -39,16 +41,9 @@ void slip_angle_estimate(sensor_msgs::msg::Imu imu, eagleye_msgs::msg::VelocityS double yawrate; double acceleration_y; - if (slip_angle_parameter.reverse_imu == false) - { - yawrate = imu.angular_velocity.z; - } - else if (slip_angle_parameter.reverse_imu == true) - { - yawrate = -1 * imu.angular_velocity.z; - } + yawrate = imu.angular_velocity.z; - if (std::abs(velocity_scale_factor.correction_velocity.linear.x) > slip_angle_parameter.stop_judgment_velocity_threshold) + if (std::abs(velocity.twist.linear.x) > slip_angle_parameter.stop_judgment_velocity_threshold) { yawrate = yawrate + yawrate_offset_2nd.yawrate_offset; } @@ -57,9 +52,9 @@ void slip_angle_estimate(sensor_msgs::msg::Imu imu, eagleye_msgs::msg::VelocityS yawrate = yawrate + yawrate_offset_stop.yawrate_offset; } - acceleration_y = velocity_scale_factor.correction_velocity.linear.x * yawrate; + acceleration_y = velocity.twist.linear.x * yawrate; - if (velocity_scale_factor.status.enabled_status == true && yawrate_offset_stop.status.enabled_status == true && yawrate_offset_2nd.status.enabled_status == true) + if (velocity_status.status.enabled_status == true && yawrate_offset_stop.status.enabled_status == true && yawrate_offset_2nd.status.enabled_status == true) { slip_angle->coefficient = slip_angle_parameter.manual_coefficient; slip_angle->slip_angle = slip_angle_parameter.manual_coefficient * acceleration_y; diff --git a/eagleye_core/navigation/src/slip_coefficient.cpp b/eagleye_core/navigation/src/slip_coefficient.cpp index 52f1bc20..24a16dad 100755 --- a/eagleye_core/navigation/src/slip_coefficient.cpp +++ b/eagleye_core/navigation/src/slip_coefficient.cpp @@ -31,7 +31,9 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -void slip_coefficient_estimate(sensor_msgs::msg::Imu imu,rtklib_msgs::msg::RtklibNav rtklib_nav,eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor,eagleye_msgs::msg::YawrateOffset yawrate_offset_stop,eagleye_msgs::msg::YawrateOffset yawrate_offset_2nd,eagleye_msgs::msg::Heading heading_interpolate_3rd,SlipCoefficientParameter slip_coefficient_parameter,SlipCoefficientStatus* slip_coefficient_status,double* estimate_coefficient) +void slip_coefficient_estimate(sensor_msgs::msg::Imu imu,rtklib_msgs::msg::RtklibNav rtklib_nav, geometry_msgs::msg::TwistStamped velocity, + eagleye_msgs::msg::YawrateOffset yawrate_offset_stop,eagleye_msgs::msg::YawrateOffset yawrate_offset_2nd,eagleye_msgs::msg::Heading heading_interpolate_3rd, + SlipCoefficientParameter slip_coefficient_parameter,SlipCoefficientStatus* slip_coefficient_status,double* estimate_coefficient) { int i; @@ -68,16 +70,9 @@ void slip_coefficient_estimate(sensor_msgs::msg::Imu imu,rtklib_msgs::msg::Rtkli doppler_heading_angle = doppler_heading_angle + 2*M_PI; } - if (slip_coefficient_parameter.reverse_imu == false) - { - yawrate = imu.angular_velocity.z; - } - else if (slip_coefficient_parameter.reverse_imu == true) - { - yawrate = -1 * imu.angular_velocity.z; - } + yawrate = imu.angular_velocity.z; - if (std::abs(velocity_scale_factor.correction_velocity.linear.x) > slip_coefficient_parameter.stop_judgment_velocity_threshold) + if (std::abs(velocity.twist.linear.x) > slip_coefficient_parameter.stop_judgment_velocity_threshold) { yawrate = yawrate + yawrate_offset_2nd.yawrate_offset; } @@ -86,11 +81,11 @@ void slip_coefficient_estimate(sensor_msgs::msg::Imu imu,rtklib_msgs::msg::Rtkli yawrate = yawrate + yawrate_offset_stop.yawrate_offset; } - acceleration_y = velocity_scale_factor.correction_velocity.linear.x * yawrate; + acceleration_y = velocity.twist.linear.x * yawrate; if (heading_interpolate_3rd.status.estimate_status == true) { - if ((velocity_scale_factor.correction_velocity.linear.x > slip_coefficient_parameter.estimated_velocity_threshold) && (fabs(yawrate) > slip_coefficient_parameter.estimated_yawrate_threshold)) + if ((velocity.twist.linear.x > slip_coefficient_parameter.estimated_velocity_threshold) && (fabs(yawrate) > slip_coefficient_parameter.estimated_yawrate_threshold)) { double imu_heading; @@ -102,7 +97,7 @@ void slip_coefficient_estimate(sensor_msgs::msg::Imu imu,rtklib_msgs::msg::Rtkli doppler_slip = (imu_heading - doppler_heading_angle); - rear_slip = doppler_slip + slip_coefficient_parameter.lever_arm*yawrate/velocity_scale_factor.correction_velocity.linear.x; + rear_slip = doppler_slip + slip_coefficient_parameter.lever_arm*yawrate/velocity.twist.linear.x; if(fabs(rear_slip)<(2*M_PI/180)) { diff --git a/eagleye_core/navigation/src/smoothing.cpp b/eagleye_core/navigation/src/smoothing.cpp index 543230e8..e712d7d2 100755 --- a/eagleye_core/navigation/src/smoothing.cpp +++ b/eagleye_core/navigation/src/smoothing.cpp @@ -31,7 +31,8 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -void smoothing_estimate(rtklib_msgs::msg::RtklibNav rtklib_nav, eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor,SmoothingParameter smoothing_parameter, SmoothingStatus* smoothing_status,eagleye_msgs::msg::Position* gnss_smooth_pos_enu) +void smoothing_estimate(rtklib_msgs::msg::RtklibNav rtklib_nav, geometry_msgs::msg::TwistStamped velocity, + SmoothingParameter smoothing_parameter, SmoothingStatus* smoothing_status,eagleye_msgs::msg::Position* gnss_smooth_pos_enu) { int i; @@ -91,7 +92,7 @@ void smoothing_estimate(rtklib_msgs::msg::RtklibNav rtklib_nav, eagleye_msgs::ms smoothing_status->enu_pos_x_buffer.push_back(enu_pos[0]); smoothing_status->enu_pos_y_buffer.push_back(enu_pos[1]); smoothing_status->enu_pos_z_buffer.push_back(enu_pos[2]); - smoothing_status->correction_velocity_buffer.push_back(velocity_scale_factor.correction_velocity.linear.x); + smoothing_status->correction_velocity_buffer.push_back(velocity.twist.linear.x); time_buffer_length = std::distance(smoothing_status->time_buffer.begin(), smoothing_status->time_buffer.end()); diff --git a/eagleye_core/navigation/src/trajectory.cpp b/eagleye_core/navigation/src/trajectory.cpp index 2ff0760c..f8b2ac10 100755 --- a/eagleye_core/navigation/src/trajectory.cpp +++ b/eagleye_core/navigation/src/trajectory.cpp @@ -31,36 +31,27 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -void trajectory_estimate(const sensor_msgs::msg::Imu imu, const eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, 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) +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) { rclcpp::Time ros_clock(imu.header.stamp); auto imu_time = ros_clock.seconds(); - if (trajectory_parameter.reverse_imu == false) + if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_velocity_threshold && yawrate_offset_2nd.status.enabled_status == true) { - if (std::abs(velocity_scale_factor.correction_velocity.linear.x) > trajectory_parameter.stop_judgment_velocity_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 - } - 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->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); //Inverted because the coordinate system is reversed } - else if (trajectory_parameter.reverse_imu == true) + else { - if (std::abs(velocity_scale_factor.correction_velocity.linear.x) > trajectory_parameter.stop_judgment_velocity_threshold && yawrate_offset_2nd.status.enabled_status == true) - { - eagleye_twist->twist.angular.z = -1 * (-1 * imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); //Inverted because the coordinate system is reversed - } - else - { - eagleye_twist->twist.angular.z = -1 * (-1 * imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset); //Inverted because the coordinate system is reversed - } + eagleye_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_scale_factor.correction_velocity.linear.x; - if (trajectory_status->estimate_status_count == 0 && velocity_scale_factor.status.enabled_status == true && heading_interpolate_3rd.status.enabled_status == true) + eagleye_twist->twist.linear.x = velocity.twist.linear.x; + + if (trajectory_status->estimate_status_count == 0 && velocity_status.status.enabled_status == true && heading_interpolate_3rd.status.enabled_status == true) { trajectory_status->estimate_status_count = 1; trajectory_status->heading_last = heading_interpolate_3rd.heading_angle; @@ -72,12 +63,12 @@ void trajectory_estimate(const sensor_msgs::msg::Imu imu, const eagleye_msgs::ms if (trajectory_status->estimate_status_count == 2) { - enu_vel->vector.x = sin(heading_interpolate_3rd.heading_angle) * velocity_scale_factor.correction_velocity.linear.x; //vel_e - enu_vel->vector.y = cos(heading_interpolate_3rd.heading_angle) * velocity_scale_factor.correction_velocity.linear.x; //vel_n + enu_vel->vector.x = sin(heading_interpolate_3rd.heading_angle) * velocity.twist.linear.x; //vel_e + enu_vel->vector.y = cos(heading_interpolate_3rd.heading_angle) * velocity.twist.linear.x; //vel_n enu_vel->vector.z = 0; //vel_u } - if (trajectory_status->estimate_status_count == 2 && std::abs(velocity_scale_factor.correction_velocity.linear.x) > 0 && trajectory_status->time_last != 0) + if (trajectory_status->estimate_status_count == 2 && std::abs(velocity.twist.linear.x) > 0 && trajectory_status->time_last != 0) { if(std::abs(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) < trajectory_parameter.stop_judgment_yawrate_threshold) { @@ -87,8 +78,8 @@ void trajectory_estimate(const sensor_msgs::msg::Imu imu, const eagleye_msgs::ms } else if((imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) != 0) { - enu_relative_pos->enu_pos.x = enu_relative_pos->enu_pos.x + velocity_scale_factor.correction_velocity.linear.x/(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) * ( -cos(trajectory_status->heading_last+(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset)*(imu_time - trajectory_status->time_last)) + cos(trajectory_status->heading_last)); - enu_relative_pos->enu_pos.y = enu_relative_pos->enu_pos.y + velocity_scale_factor.correction_velocity.linear.x/(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) * ( sin(trajectory_status->heading_last+(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset)*(imu_time - trajectory_status->time_last)) - sin(trajectory_status->heading_last)); + enu_relative_pos->enu_pos.x = enu_relative_pos->enu_pos.x + velocity.twist.linear.x/(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) * ( -cos(trajectory_status->heading_last+(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset)*(imu_time - trajectory_status->time_last)) + cos(trajectory_status->heading_last)); + enu_relative_pos->enu_pos.y = enu_relative_pos->enu_pos.y + velocity.twist.linear.x/(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) * ( sin(trajectory_status->heading_last+(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset)*(imu_time - trajectory_status->time_last)) - sin(trajectory_status->heading_last)); enu_relative_pos->enu_pos.z = 0; } else{ @@ -104,36 +95,26 @@ void trajectory_estimate(const sensor_msgs::msg::Imu imu, const eagleye_msgs::ms trajectory_status->time_last = imu_time; } -void trajectory3d_estimate(const sensor_msgs::msg::Imu imu, const eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, 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) +void trajectory3d_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 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) { rclcpp::Time ros_clock(imu.header.stamp); auto imu_time = ros_clock.seconds(); - if (trajectory_parameter.reverse_imu == false) + if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_velocity_threshold && yawrate_offset_2nd.status.enabled_status == true) { - if (std::abs(velocity_scale_factor.correction_velocity.linear.x) > trajectory_parameter.stop_judgment_velocity_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 - } - 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->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); //Inverted because the coordinate system is reversed } - else if (trajectory_parameter.reverse_imu == true) + else { - if (std::abs(velocity_scale_factor.correction_velocity.linear.x) > trajectory_parameter.stop_judgment_velocity_threshold && yawrate_offset_2nd.status.enabled_status == true) - { - eagleye_twist->twist.angular.z = -1 * (-1 * imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); //Inverted because the coordinate system is reversed - } - else - { - eagleye_twist->twist.angular.z = -1 * (-1 * imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset); //Inverted because the coordinate system is reversed - } + eagleye_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_scale_factor.correction_velocity.linear.x; + eagleye_twist->twist.linear.x = velocity.twist.linear.x; - if (trajectory_status->estimate_status_count == 0 && velocity_scale_factor.status.enabled_status == true && heading_interpolate_3rd.status.enabled_status == true) + if (trajectory_status->estimate_status_count == 0 && velocity_status.status.enabled_status == true && heading_interpolate_3rd.status.enabled_status == true) { trajectory_status->estimate_status_count = 1; trajectory_status->heading_last = heading_interpolate_3rd.heading_angle; @@ -145,12 +126,12 @@ void trajectory3d_estimate(const sensor_msgs::msg::Imu imu, const eagleye_msgs:: if (trajectory_status->estimate_status_count == 2) { - enu_vel->vector.x = sin(heading_interpolate_3rd.heading_angle) * cos(pitching.pitching_angle) * velocity_scale_factor.correction_velocity.linear.x; //vel_e - enu_vel->vector.y = cos(heading_interpolate_3rd.heading_angle) * cos(pitching.pitching_angle) * velocity_scale_factor.correction_velocity.linear.x; //vel_n - enu_vel->vector.z = sin(pitching.pitching_angle) * velocity_scale_factor.correction_velocity.linear.x; //vel_u + enu_vel->vector.x = sin(heading_interpolate_3rd.heading_angle) * cos(pitching.pitching_angle) * velocity.twist.linear.x; //vel_e + enu_vel->vector.y = cos(heading_interpolate_3rd.heading_angle) * cos(pitching.pitching_angle) * velocity.twist.linear.x; //vel_n + enu_vel->vector.z = sin(pitching.pitching_angle) * velocity.twist.linear.x; //vel_u } - if (trajectory_status->estimate_status_count == 2 && std::abs(velocity_scale_factor.correction_velocity.linear.x) > 0 && trajectory_status->time_last != 0) + if (trajectory_status->estimate_status_count == 2 && std::abs(velocity.twist.linear.x) > 0 && trajectory_status->time_last != 0) { if(std::abs(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) < trajectory_parameter.stop_judgment_yawrate_threshold) { @@ -160,8 +141,8 @@ void trajectory3d_estimate(const sensor_msgs::msg::Imu imu, const eagleye_msgs:: } else { - enu_relative_pos->enu_pos.x = enu_relative_pos->enu_pos.x + velocity_scale_factor.correction_velocity.linear.x/(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) * ( -cos(trajectory_status->heading_last+(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset)*(imu_time - trajectory_status->time_last)) + cos(trajectory_status->heading_last)); - enu_relative_pos->enu_pos.y = enu_relative_pos->enu_pos.y + velocity_scale_factor.correction_velocity.linear.x/(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) * ( sin(trajectory_status->heading_last+(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset)*(imu_time - trajectory_status->time_last)) - sin(trajectory_status->heading_last)); + enu_relative_pos->enu_pos.x = enu_relative_pos->enu_pos.x + velocity.twist.linear.x/(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) * ( -cos(trajectory_status->heading_last+(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset)*(imu_time - trajectory_status->time_last)) + cos(trajectory_status->heading_last)); + enu_relative_pos->enu_pos.y = enu_relative_pos->enu_pos.y + velocity.twist.linear.x/(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset) * ( sin(trajectory_status->heading_last+(imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset)*(imu_time - trajectory_status->time_last)) - sin(trajectory_status->heading_last)); enu_relative_pos->enu_pos.z = enu_relative_pos->enu_pos.z + enu_vel->vector.z * (imu_time - trajectory_status->time_last); } diff --git a/eagleye_core/navigation/src/velocity_estimator.cpp b/eagleye_core/navigation/src/velocity_estimator.cpp new file mode 100644 index 00000000..d131f86b --- /dev/null +++ b/eagleye_core/navigation/src/velocity_estimator.cpp @@ -0,0 +1,714 @@ +// 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. + +/* + * velocity_estimator.cpp + * Author MapIV Takanose + */ + +#include "eagleye_coordinate/eagleye_coordinate.hpp" +#include "eagleye_navigation/eagleye_navigation.hpp" + +#define STATUS_FIX 4 +#define g 9.80665 + +//---PitchrateOffsetStopEstimator---------------------------------------------------------------------------------------------------------- +VelocityEstimator::PitchrateOffsetStopEstimator::PitchrateOffsetStopEstimator() +{ + stop_count = 0; + pitchrate_offset = 0; + pitchrate_offset_status.enabled_status = false; +} + +void VelocityEstimator::PitchrateOffsetStopEstimator::setParam(std::string yaml_file) +{ + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + param.buffer_count_max = conf["/**"]["ros__parameters"]["velocity_estimator"]["pitchrate_offset"]["buffer_count_max"].as(); + // std::cout<< "buffer_count_max "< param.buffer_count_max) + { + double accumulation_pitchrate = 0.0; + for (int i = 0; i < param.buffer_count_max/2; i++) + { + accumulation_pitchrate += pitchrate_buffer[i]; + } + + pitchrate_offset = -1 * accumulation_pitchrate / (param.buffer_count_max/2); + pitchrate_offset_status.enabled_status = true; + pitchrate_offset_status.estimate_status = true; + } + + return pitchrate_offset_status.enabled_status; +} + +//---PitchingEstimator--------------------------------------------------------------------------------------------------------------------- +VelocityEstimator::PitchingEstimator::PitchingEstimator() +{ + pitching = 0; + pitching_status.enabled_status = false; +} + +void VelocityEstimator::PitchingEstimator::setParam(std::string yaml_file) +{ + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + param.buffer_max = conf["/**"]["ros__parameters"]["velocity_estimator"]["pitching"]["buffer_max"].as(); + param.outlier_threshold = conf["/**"]["ros__parameters"]["velocity_estimator"]["pitching"]["outlier_threshold"].as(); + param.estimated_velocity_threshold = conf["/**"]["ros__parameters"]["velocity_estimator"]["pitching"]["estimated_velocity_threshold"].as(); + param.estimated_gnss_coefficient = conf["/**"]["ros__parameters"]["velocity_estimator"]["pitching"]["estimated_gnss_coefficient"].as(); + param.estimated_coefficient = conf["/**"]["ros__parameters"]["velocity_estimator"]["pitching"]["estimated_coefficient"].as(); + // std::cout<< "buffer_max "< param.estimated_velocity_threshold || + rtkfix_velocity > param.estimated_velocity_threshold) + { + use_gnss_status = true; + } + + // data buffer generate + time_buffer.push_back(imu_time_last); + corrected_pitchrate_buffer.push_back(pitchrate + pitchrate_offset); + rtkfix_pitching_buffer.push_back(rtkfix_pitching); + navsat_update_status_buffer.push_back(navsat_update_status); + use_gnss_status_buffer.push_back(use_gnss_status); + + std::size_t time_buffer_length = std::distance(time_buffer.begin(), time_buffer.end()); + if (time_buffer_length <= param.buffer_max) return pitching_status.enabled_status; + + time_buffer.erase(time_buffer.begin()); + corrected_pitchrate_buffer.erase(corrected_pitchrate_buffer.begin()); + rtkfix_pitching_buffer.erase(rtkfix_pitching_buffer.begin()); + navsat_update_status_buffer.erase(navsat_update_status_buffer.begin()); + use_gnss_status_buffer.erase(use_gnss_status_buffer.begin()); + time_buffer_length = std::distance(time_buffer.begin(), time_buffer.end()); + + // setup for estimation + double accumulated_pitchrate; + std::vector accumulated_pitchrate_buffer(time_buffer_length); + std::vector gnss_index; + std::size_t gnss_index_length; + pitching_status.estimate_status = false; + + for (int i = 0; i < time_buffer_length; i++) + { + if (navsat_update_status_buffer[i] && use_gnss_status_buffer[i]) gnss_index.push_back(i); //TODO Velocity judgment + if (i == 0) continue; + accumulated_pitchrate += corrected_pitchrate_buffer[i] * (time_buffer[i]-time_buffer[i-1]); + accumulated_pitchrate_buffer[i] = accumulated_pitchrate; + } + + gnss_index_length = std::distance(gnss_index.begin(), gnss_index.end()); + + if (gnss_index_length < time_buffer_length * param.estimated_gnss_coefficient) return pitching_status.enabled_status; + + // pitching estimation by the least-squares method + std::vector init_accumulated_pitchrate_buffer; + std::vector residual_error; + + while (1) + { + gnss_index_length = std::distance(gnss_index.begin(), gnss_index.end()); + + for (int i = 0; i < time_buffer_length; i++) + { + init_accumulated_pitchrate_buffer.push_back(accumulated_pitchrate_buffer[i] + (rtkfix_pitching_buffer[gnss_index[0]] - accumulated_pitchrate_buffer[gnss_index[0]])); + } + + for (int i = 0; i < gnss_index_length; i++) + { + residual_error.push_back(init_accumulated_pitchrate_buffer[gnss_index[i]] - rtkfix_pitching_buffer[gnss_index[i]]); + } + + double residual_error_mean = std::accumulate(residual_error.begin(), residual_error.end(), 0.0) / gnss_index_length; + double init_pitching = rtkfix_pitching_buffer[gnss_index[0]] - residual_error_mean; + + init_accumulated_pitchrate_buffer.clear(); + for (int i = 0; i < time_buffer_length; i++) + { + init_accumulated_pitchrate_buffer.push_back(accumulated_pitchrate_buffer[i] + (init_pitching - accumulated_pitchrate_buffer[gnss_index[0]])); + } + + residual_error.clear(); + for (int i = 0; i < gnss_index_length; i++) + { + residual_error.push_back(init_accumulated_pitchrate_buffer[gnss_index[i]] - rtkfix_pitching_buffer[gnss_index[i]]); + } + + std::vector::iterator residual_error_max = std::max_element(residual_error.begin(), residual_error.end()); + int residual_error_max_index = std::distance(residual_error.begin(), residual_error_max); + + if (residual_error[residual_error_max_index] < param.outlier_threshold) break; + gnss_index.erase(gnss_index.begin() + residual_error_max_index); + gnss_index_length = std::distance(gnss_index.begin(), gnss_index.end()); + if (gnss_index_length < param.buffer_max * param.estimated_coefficient) break; + + } + + pitching = init_accumulated_pitchrate_buffer[time_buffer_length-1]; + pitching_status.enabled_status = true; + pitching_status.estimate_status = true; + + return pitching_status.enabled_status; +} + +//---AccelerationOffsetEstimator----------------------------------------------------------------------------------------------------------- +VelocityEstimator::AccelerationOffsetEstimator::AccelerationOffsetEstimator() +{ + acceleration_offset = 0; + acceleration_offset_status.enabled_status = false; +} + +void VelocityEstimator::AccelerationOffsetEstimator::setParam(std::string yaml_file) +{ + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + param.buffer_min = conf["/**"]["ros__parameters"]["velocity_estimator"]["acceleration_offset"]["buffer_min"].as(); + param.buffer_max = conf["/**"]["ros__parameters"]["velocity_estimator"]["acceleration_offset"]["buffer_max"].as(); + param.filter_process_noise = conf["/**"]["ros__parameters"]["velocity_estimator"]["acceleration_offset"]["filter_process_noise"].as(); + param.filter_observation_noise = conf["/**"]["ros__parameters"]["velocity_estimator"]["acceleration_offset"]["filter_observation_noise"].as(); + // std::cout<< "buffer_min "< param.buffer_max) + { + time_buffer.erase(time_buffer.begin()); + pitching_buffer.erase(pitching_buffer.begin()); + filtered_acceleration_buffer.erase(filtered_acceleration_buffer.begin()); + rtkfix_velocity_buffer.erase(rtkfix_velocity_buffer.begin()); + navsat_update_status_buffer.erase(navsat_update_status_buffer.begin()); + time_buffer_length = std::distance(time_buffer.begin(), time_buffer.end()); + } + + // Acceleration error estimation by the least-squares method + double accumulated_acceleration; + std::vector accumulated_acceleration_buffer; + std::vector init_accumulated_acceleration_buffer; + std::vector gnss_index; + std::size_t gnss_index_length; + + for (int i = 0; i < time_buffer_length; i++) + { + if (navsat_update_status_buffer[i]) gnss_index.push_back(i); //TODO Velocity judgment + if (i == 0) + { + accumulated_acceleration_buffer.push_back(0); + continue; + } + else + { + accumulated_acceleration += (filtered_acceleration_buffer[i] - g*std::sin(pitching_buffer[i]))* (time_buffer[i]-time_buffer[i-1]); + accumulated_acceleration_buffer.push_back(accumulated_acceleration); + } + } + + gnss_index_length = std::distance(gnss_index.begin(), gnss_index.end()); + + for (int i = 0; i < time_buffer_length; i++) + { + init_accumulated_acceleration_buffer.push_back(accumulated_acceleration_buffer[i] + + (rtkfix_velocity_buffer[gnss_index[0]] - accumulated_acceleration_buffer[gnss_index[0]])); + } + + double dt; // x + double residual_error; // y + double sum_xy = 0.0, sum_x = 0.0, sum_y = 0.0, sum_xx = 0.0; + + for (int i = 0; i < gnss_index_length; i++) + { + residual_error = init_accumulated_acceleration_buffer[gnss_index[i]] - rtkfix_velocity_buffer[gnss_index[i]]; + dt = time_buffer[gnss_index[i]]-time_buffer[gnss_index[0]]; + + sum_xy += dt * residual_error; + sum_x += dt; + sum_y += residual_error; + sum_xx += dt *dt; + } + + acceleration_offset = -1 * (gnss_index_length*sum_xy - sum_x*sum_y)/(gnss_index_length*sum_xx - sum_x*sum_x); + acceleration_offset_status.enabled_status = true; + acceleration_offset_status.estimate_status = true; + + return acceleration_offset_status.enabled_status; +} + +//---VelocityEstimator--------------------------------------------------------------------------------------------------------------------- +VelocityEstimator::VelocityEstimator() +{ + acceleration = 0; + pitchrate = 0; + doppler_velocity = 0; + rtkfix_velocity = 0; + rtkfix_pitching = 0; + gga_time_last = 0; + pitchrate_offset = 0; + pitching = 0; + acceleration_offset = 0; + filtered_acceleration = 0; + ecef_base_position_status = false; + velocity_status.enabled_status = false; + +} + +void VelocityEstimator::setParam(std::string yaml_file) +{ + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + param.ecef_base_pos_x = conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as(); + param.ecef_base_pos_y = conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as(); + param.ecef_base_pos_z = conf["/**"]["ros__parameters"]["ecef_base_pos"]["z"].as(); + param.use_ecef_base_position = conf["/**"]["ros__parameters"]["ecef_base_pos"]["use_ecef_base_position"].as(); + + param.gga_downsample_time = conf["/**"]["ros__parameters"]["velocity_estimator"]["gga_downsample_time"].as(); + param.stop_judgment_velocity_threshold = conf["/**"]["ros__parameters"]["velocity_estimator"]["stop_judgment_velocity_threshold"].as(); + param.stop_judgment_buffer_maxnum = conf["/**"]["ros__parameters"]["velocity_estimator"]["stop_judgment_buffer_maxnum"].as(); + param.variance_threshold = conf["/**"]["ros__parameters"]["velocity_estimator"]["variance_threshold"].as(); + + // doppler fusion parameter + param.buffer_max = conf["/**"]["ros__parameters"]["velocity_estimator"]["doppler_fusion"]["buffer_max"].as(); + param.estimated_gnss_coefficient = conf["/**"]["ros__parameters"]["velocity_estimator"]["doppler_fusion"]["estimated_gnss_coefficient"].as(); + param.estimated_coefficient = conf["/**"]["ros__parameters"]["velocity_estimator"]["doppler_fusion"]["estimated_coefficient"].as(); + param.outlier_threshold = conf["/**"]["ros__parameters"]["velocity_estimator"]["doppler_fusion"]["outlier_threshold"].as(); + + // std::cout<< "ecef_base_pos_x "< param.stop_judgment_velocity_threshold || + rtkfix_velocity > param.stop_judgment_velocity_threshold) + {return false;} + + double sum_x = 0, sum_y = 0, sum_z = 0; + double var_x = 0, var_y = 0, var_z = 0; + + for (int i = 0; i < buffer_length; i++) + { + sum_x = sum_x + std::pow(angular_velocity_x_buffer[i] - std::accumulate(std::begin(angular_velocity_x_buffer), + std::end(angular_velocity_x_buffer), 0.0)/buffer_length,2.0); + sum_y = sum_y + std::pow(angular_velocity_y_buffer[i] - std::accumulate(std::begin(angular_velocity_y_buffer), + std::end(angular_velocity_y_buffer), 0.0)/buffer_length,2.0); + sum_z = sum_z + std::pow(angular_velocity_z_buffer[i] - std::accumulate(std::begin(angular_velocity_z_buffer), + std::end(angular_velocity_z_buffer), 0.0)/buffer_length,2.0); + } + + var_x = sum_x/buffer_length; + var_y = sum_y/buffer_length; + var_z = sum_z/buffer_length; + + if (var_x > param.variance_threshold || var_y > param.variance_threshold || var_z > param.variance_threshold) + {return false;} + + return true; +} + +bool VelocityEstimator::DopplerFusion() +{ + // data buffer generate + time_buffer.push_back(imu_time_last); + doppler_velocity_buffer.push_back(doppler_velocity); + corrected_acceleration_buffer.push_back(filtered_acceleration + acceleration_offset - g*std::sin(pitching)); + rtklib_update_status_buffer.push_back(rtklib_update_status); + + std::size_t time_buffer_length = std::distance(time_buffer.begin(), time_buffer.end()); + if (time_buffer_length <= param.buffer_max) return velocity_status.enabled_status; + + time_buffer.erase(time_buffer.begin()); + doppler_velocity_buffer.erase(doppler_velocity_buffer.begin()); + corrected_acceleration_buffer.erase(corrected_acceleration_buffer.begin()); + rtklib_update_status_buffer.erase(rtklib_update_status_buffer.begin()); + time_buffer_length = std::distance(time_buffer.begin(), time_buffer.end()); + + // stop process + if (stop_status && velocity_status.enabled_status) + { + velocity = 0; + velocity_status.enabled_status = true; + velocity_status.estimate_status = true; + + return velocity_status.enabled_status; + } + + // setup for estimation + double accumulated_acceleration; + std::vector accumulated_acceleration_buffer(time_buffer_length); + std::vector gnss_index; + std::size_t gnss_index_length; + + for (int i = 0; i < time_buffer_length; i++) + { + if (rtklib_update_status_buffer[i] && doppler_velocity_buffer[i] < 33.3) gnss_index.push_back(i); //TODO Velocity judgment + if (i == 0) continue; + accumulated_acceleration += corrected_acceleration_buffer[i] * (time_buffer[i]-time_buffer[i-1]); + accumulated_acceleration_buffer[i] = accumulated_acceleration; + } + + gnss_index_length = std::distance(gnss_index.begin(), gnss_index.end()); + if (gnss_index_length < time_buffer_length * param.estimated_gnss_coefficient) return velocity_status.enabled_status; + + // velocity estimation by the least-squares method + std::vector init_accumulated_acceleration; + std::vector residual_error; + + while (1) + { + gnss_index_length = std::distance(gnss_index.begin(), gnss_index.end()); + + init_accumulated_acceleration.clear(); + for (int i = 0; i < time_buffer_length; i++) + { + init_accumulated_acceleration.push_back(accumulated_acceleration_buffer[i] + + (doppler_velocity_buffer[gnss_index[0]] - accumulated_acceleration_buffer[gnss_index[0]])); + } + + residual_error.clear(); + for (int i = 0; i < gnss_index_length; i++) + { + residual_error.push_back(init_accumulated_acceleration[gnss_index[i]] - doppler_velocity_buffer[gnss_index[i]]); + } + + double residual_error_mean = std::accumulate(residual_error.begin(), residual_error.end(), 0.0) / gnss_index_length; + double init_velocity = doppler_velocity_buffer[gnss_index[0]] - residual_error_mean; + + init_accumulated_acceleration.clear(); + for (int i = 0; i < time_buffer_length; i++) + { + init_accumulated_acceleration.push_back(accumulated_acceleration_buffer[i] + (init_velocity - accumulated_acceleration_buffer[gnss_index[0]])); + } + + residual_error.clear(); + for (int i = 0; i < gnss_index_length; i++) + { + residual_error.push_back(init_accumulated_acceleration[gnss_index[i]] - doppler_velocity_buffer[gnss_index[i]]); + } + + std::vector::iterator residual_error_max = std::max_element(residual_error.begin(), residual_error.end()); + int residual_error_max_index = std::distance(residual_error.begin(), residual_error_max); + + if (residual_error[residual_error_max_index] < param.outlier_threshold) break; + gnss_index.erase(gnss_index.begin() + residual_error_max_index); + gnss_index_length = std::distance(gnss_index.begin(), gnss_index.end()); + if (gnss_index_length < param.buffer_max * param.estimated_coefficient) break; + + } + + gnss_index_length = std::distance(gnss_index.begin(), gnss_index.end()); + + velocity = init_accumulated_acceleration[time_buffer_length-1]; + velocity_status.enabled_status = true; + velocity_status.estimate_status = true; + + return velocity_status.enabled_status; + +} + +void VelocityEstimator::VelocityEstimate(sensor_msgs::msg::Imu imu_msg, rtklib_msgs::msg::RtklibNav rtklib_nav_msg, nmea_msgs::msg::Gpgga gga_msg, geometry_msgs::msg::TwistStamped* velocity_msg) +{ + // imu msgs setting + updateImu(imu_msg); + + // rtklib_nav msgs setting + rtklib_update_status = updateRtklibNav(rtklib_nav_msg); + + // gga msgs setting + navsat_update_status = updateGGA(gga_msg); + + // Stop Judgment + stop_status = StopJudgment(imu_msg); + velocity_status.estimate_status = false; + + // pitchrate offset estimation during stop + pitchrate_offset_stop_estimator.PitchrateOffsetStopEstimate(pitchrate, stop_status); + pitchrate_offset = pitchrate_offset_stop_estimator.pitchrate_offset; + + if(!pitching_estimator.PitchingEstimate + (imu_time_last, doppler_velocity, rtkfix_velocity, + pitchrate, pitchrate_offset, rtkfix_pitching, + navsat_update_status, stop_status) + ) return; + pitching = pitching_estimator.pitching; + + // Acceleration Error Estimation + if(!acceleration_offset_estimator.AccelerationOffsetEstimate + (imu_time_last, rtkfix_velocity, pitching, + acceleration, navsat_update_status) + ) return ; + acceleration_offset = acceleration_offset_estimator.acceleration_offset; + filtered_acceleration = acceleration_offset_estimator.filtered_acceleration; + + // Velocity estimation by fusion of acceleration accumulation and GNSS Doppler + if(!DopplerFusion()) return; + + velocity_msg->header = imu_msg.header; + velocity_msg->header.frame_id = "base_link"; + velocity_msg->twist.linear.x = velocity; +} \ No newline at end of file diff --git a/eagleye_core/navigation/src/velocity_scale_factor.cpp b/eagleye_core/navigation/src/velocity_scale_factor.cpp index 1a793739..f5f2b1bd 100755 --- a/eagleye_core/navigation/src/velocity_scale_factor.cpp +++ b/eagleye_core/navigation/src/velocity_scale_factor.cpp @@ -34,7 +34,8 @@ #define knot2mps 0.51477 void velocity_scale_factor_estimate_(const geometry_msgs::msg::TwistStamped velocity, const VelocityScaleFactorParameter velocity_scale_factor_parameter, - VelocityScaleFactorStatus* velocity_scale_factor_status, eagleye_msgs::msg::VelocityScaleFactor* velocity_scale_factor) + VelocityScaleFactorStatus* velocity_scale_factor_status, geometry_msgs::msg::TwistStamped* correction_velocity, + eagleye_msgs::msg::VelocityScaleFactor* velocity_scale_factor) { int i; double initial_velocity_scale_factor = 1.0; @@ -146,20 +147,22 @@ void velocity_scale_factor_estimate_(const geometry_msgs::msg::TwistStamped velo if (velocity_scale_factor_status->estimate_start_status == true) { velocity_scale_factor->status.enabled_status = true; - velocity_scale_factor->correction_velocity.linear.x = velocity.twist.linear.x * velocity_scale_factor->scale_factor; + correction_velocity->twist.linear.x = velocity.twist.linear.x * velocity_scale_factor->scale_factor; } else { velocity_scale_factor->status.enabled_status = false; velocity_scale_factor->scale_factor = initial_velocity_scale_factor; - velocity_scale_factor->correction_velocity.linear.x = velocity.twist.linear.x * initial_velocity_scale_factor; + correction_velocity->twist.linear.x = velocity.twist.linear.x * initial_velocity_scale_factor; } velocity_scale_factor_status->velocity_scale_factor_last = velocity_scale_factor->scale_factor; } -void velocity_scale_factor_estimate(const rtklib_msgs::msg::RtklibNav rtklib_nav, const geometry_msgs::msg::TwistStamped velocity, const VelocityScaleFactorParameter velocity_scale_factor_parameter, VelocityScaleFactorStatus* velocity_scale_factor_status,eagleye_msgs::msg::VelocityScaleFactor* velocity_scale_factor) +void velocity_scale_factor_estimate(const rtklib_msgs::msg::RtklibNav rtklib_nav, const geometry_msgs::msg::TwistStamped velocity, + const VelocityScaleFactorParameter velocity_scale_factor_parameter, VelocityScaleFactorStatus* velocity_scale_factor_status, + geometry_msgs::msg::TwistStamped* correction_velocity, eagleye_msgs::msg::VelocityScaleFactor* velocity_scale_factor) { double ecef_vel[3]; @@ -209,10 +212,12 @@ void velocity_scale_factor_estimate(const rtklib_msgs::msg::RtklibNav rtklib_nav velocity_scale_factor_status->doppler_velocity_buffer.push_back(doppler_velocity); velocity_scale_factor_status->velocity_buffer.push_back(velocity.twist.linear.x); - velocity_scale_factor_estimate_(velocity, velocity_scale_factor_parameter, velocity_scale_factor_status, velocity_scale_factor); + velocity_scale_factor_estimate_(velocity, velocity_scale_factor_parameter, velocity_scale_factor_status, correction_velocity, velocity_scale_factor); } -void velocity_scale_factor_estimate(const nmea_msgs::msg::Gprmc nmea_rmc, const geometry_msgs::msg::TwistStamped velocity, const VelocityScaleFactorParameter velocity_scale_factor_parameter, VelocityScaleFactorStatus* velocity_scale_factor_status,eagleye_msgs::msg::VelocityScaleFactor* velocity_scale_factor) +void velocity_scale_factor_estimate(const nmea_msgs::msg::Gprmc nmea_rmc, const geometry_msgs::msg::TwistStamped velocity, + const VelocityScaleFactorParameter velocity_scale_factor_parameter, VelocityScaleFactorStatus* velocity_scale_factor_status, + geometry_msgs::msg::TwistStamped* correction_velocity, eagleye_msgs::msg::VelocityScaleFactor* velocity_scale_factor) { bool gnss_status; double doppler_velocity = 0.0; @@ -234,5 +239,5 @@ void velocity_scale_factor_estimate(const nmea_msgs::msg::Gprmc nmea_rmc, const velocity_scale_factor_status->doppler_velocity_buffer.push_back(doppler_velocity); velocity_scale_factor_status->velocity_buffer.push_back(velocity.twist.linear.x); - velocity_scale_factor_estimate_(velocity, velocity_scale_factor_parameter, velocity_scale_factor_status, velocity_scale_factor); + velocity_scale_factor_estimate_(velocity, velocity_scale_factor_parameter, velocity_scale_factor_status, correction_velocity, velocity_scale_factor); } diff --git a/eagleye_core/navigation/src/yawrate_offset.cpp b/eagleye_core/navigation/src/yawrate_offset.cpp index 994b0f96..4d6cf942 100755 --- a/eagleye_core/navigation/src/yawrate_offset.cpp +++ b/eagleye_core/navigation/src/yawrate_offset.cpp @@ -31,7 +31,9 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -void yawrate_offset_estimate(const eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor, const eagleye_msgs::msg::YawrateOffset yawrate_offset_stop,const eagleye_msgs::msg::Heading heading_interpolate,const sensor_msgs::msg::Imu imu, const YawrateOffsetParameter yawrate_offset_parameter, YawrateOffsetStatus* yawrate_offset_status, eagleye_msgs::msg::YawrateOffset* yawrate_offset) +void yawrate_offset_estimate(const geometry_msgs::msg::TwistStamped velocity, const eagleye_msgs::msg::YawrateOffset yawrate_offset_stop, + const eagleye_msgs::msg::Heading heading_interpolate,const sensor_msgs::msg::Imu imu, const YawrateOffsetParameter yawrate_offset_parameter, + YawrateOffsetStatus* yawrate_offset_status, eagleye_msgs::msg::YawrateOffset* yawrate_offset) { int i; double yawrate = 0.0; @@ -65,20 +67,13 @@ void yawrate_offset_estimate(const eagleye_msgs::msg::VelocityScaleFactor veloci yawrate_offset_status->estimated_number = estimated_number_cur; } - if (yawrate_offset_parameter.reverse_imu == false) - { - yawrate = imu.angular_velocity.z; - } - else if (yawrate_offset_parameter.reverse_imu == true) - { - yawrate = -1 * imu.angular_velocity.z; - } + yawrate = imu.angular_velocity.z; // data buffer generate yawrate_offset_status->time_buffer.push_back(imu_time); yawrate_offset_status->yawrate_buffer.push_back(yawrate); yawrate_offset_status->heading_angle_buffer.push_back(heading_interpolate.heading_angle); - yawrate_offset_status->correction_velocity_buffer.push_back(velocity_scale_factor.correction_velocity.linear.x); + yawrate_offset_status->correction_velocity_buffer.push_back(velocity.twist.linear.x); yawrate_offset_status->heading_estimate_status_buffer.push_back(heading_interpolate.status.estimate_status); yawrate_offset_status->yawrate_offset_stop_buffer.push_back(yawrate_offset_stop.yawrate_offset); @@ -94,7 +89,8 @@ void yawrate_offset_estimate(const eagleye_msgs::msg::VelocityScaleFactor veloci yawrate_offset_status->yawrate_offset_stop_buffer.erase(yawrate_offset_status->yawrate_offset_stop_buffer.begin()); } - if (yawrate_offset_status->estimated_preparation_conditions == 0 && yawrate_offset_status->heading_estimate_status_buffer[yawrate_offset_status->estimated_number - 1] == true) + if (yawrate_offset_status->estimated_preparation_conditions == 0 && + yawrate_offset_status->heading_estimate_status_buffer[yawrate_offset_status->estimated_number - 1] == true) { yawrate_offset_status->estimated_preparation_conditions = 1; } @@ -110,7 +106,8 @@ void yawrate_offset_estimate(const eagleye_msgs::msg::VelocityScaleFactor veloci } } - if (yawrate_offset_status->estimated_preparation_conditions == 2 && yawrate_offset_status->correction_velocity_buffer[yawrate_offset_status->estimated_number-1] > yawrate_offset_parameter.estimated_velocity_threshold && yawrate_offset_status->heading_estimate_status_buffer[yawrate_offset_status->estimated_number-1] == true) + if (yawrate_offset_status->estimated_preparation_conditions == 2 && yawrate_offset_status->correction_velocity_buffer[yawrate_offset_status->estimated_number-1] > + yawrate_offset_parameter.estimated_velocity_threshold && yawrate_offset_status->heading_estimate_status_buffer[yawrate_offset_status->estimated_number-1] == true) { estimated_condition_status = true; } @@ -150,7 +147,8 @@ void yawrate_offset_estimate(const eagleye_msgs::msg::VelocityScaleFactor veloci { if (i > 0) { - provisional_heading_angle_buffer[i] = provisional_heading_angle_buffer[i-1] + yawrate_offset_status->yawrate_buffer[i] * (yawrate_offset_status->time_buffer[i] - yawrate_offset_status->time_buffer[i-1]); + provisional_heading_angle_buffer[i] = provisional_heading_angle_buffer[i-1] + + yawrate_offset_status->yawrate_buffer[i] * (yawrate_offset_status->time_buffer[i] - yawrate_offset_status->time_buffer[i-1]); } } @@ -165,15 +163,16 @@ void yawrate_offset_estimate(const eagleye_msgs::msg::VelocityScaleFactor veloci //base_heading_angle_buffer.clear(); for (i = 0; i < yawrate_offset_status->estimated_number; i++) { - base_heading_angle_buffer.push_back(yawrate_offset_status->heading_angle_buffer[index[index_length-1]] - provisional_heading_angle_buffer[index[index_length-1]] + provisional_heading_angle_buffer[i]); + base_heading_angle_buffer.push_back(yawrate_offset_status->heading_angle_buffer[index[index_length-1]] - + provisional_heading_angle_buffer[index[index_length-1]] + provisional_heading_angle_buffer[i]); } //diff_buffer.clear(); for (i = 0; i < index_length; i++) { // diff_buffer.push_back(base_heading_angle_buffer[index[i]] - heading_angle_buffer[index[i]]); - diff_buffer.push_back(yawrate_offset_status->heading_angle_buffer[index[index_length-1]] - provisional_heading_angle_buffer[index[index_length-1]] + provisional_heading_angle_buffer[index[i]] - - yawrate_offset_status->heading_angle_buffer[index[i]]); + diff_buffer.push_back(yawrate_offset_status->heading_angle_buffer[index[index_length-1]] - provisional_heading_angle_buffer[index[index_length-1]] + + provisional_heading_angle_buffer[index[i]] - yawrate_offset_status->heading_angle_buffer[index[i]]); } time_buffer2.clear(); diff --git a/eagleye_core/navigation/src/yawrate_offset_stop.cpp b/eagleye_core/navigation/src/yawrate_offset_stop.cpp index cda51dbf..42900a37 100755 --- a/eagleye_core/navigation/src/yawrate_offset_stop.cpp +++ b/eagleye_core/navigation/src/yawrate_offset_stop.cpp @@ -43,25 +43,11 @@ void yawrate_offset_stop_estimate(const geometry_msgs::msg::TwistStamped velocit // data buffer generate if (yawrate_offset_stop_status->estimate_start_status == false) { - if (yawrate_offset_stop_parameter.reverse_imu == false) - { - yawrate_offset_stop_status->yawrate_buffer.push_back(imu.angular_velocity.z); - } - else if (yawrate_offset_stop_parameter.reverse_imu == true) - { - yawrate_offset_stop_status->yawrate_buffer.push_back(-1 * imu.angular_velocity.z); - } + yawrate_offset_stop_status->yawrate_buffer.push_back(imu.angular_velocity.z); } else if ( std::fabs(std::fabs(yawrate_offset_stop_status->yawrate_offset_stop_last) - std::fabs(imu.angular_velocity.z)) < yawrate_offset_stop_parameter.outlier_threshold && yawrate_offset_stop_status->estimate_start_status == true) { - if (yawrate_offset_stop_parameter.reverse_imu == false) - { - yawrate_offset_stop_status->yawrate_buffer.push_back(imu.angular_velocity.z); - } - else if (yawrate_offset_stop_parameter.reverse_imu == true) - { - yawrate_offset_stop_status->yawrate_buffer.push_back(-1 * imu.angular_velocity.z); - } + yawrate_offset_stop_status->yawrate_buffer.push_back(imu.angular_velocity.z); } yawrate_buffer_length = std::distance(yawrate_offset_stop_status->yawrate_buffer.begin(), yawrate_offset_stop_status->yawrate_buffer.end()); diff --git a/eagleye_msgs/CMakeLists.txt b/eagleye_msgs/CMakeLists.txt index d631916b..33b14ee1 100644 --- a/eagleye_msgs/CMakeLists.txt +++ b/eagleye_msgs/CMakeLists.txt @@ -35,6 +35,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Rolling.msg" "msg/SlipAngle.msg" "msg/Status.msg" + "msg/StatusStamped.msg" "msg/VelocityScaleFactor.msg" "msg/YawrateOffset.msg" DEPENDENCIES std_msgs diff --git a/eagleye_msgs/msg/StatusStamped.msg b/eagleye_msgs/msg/StatusStamped.msg new file mode 100644 index 00000000..5e311eb7 --- /dev/null +++ b/eagleye_msgs/msg/StatusStamped.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +eagleye_msgs/Status status \ No newline at end of file diff --git a/eagleye_msgs/msg/VelocityScaleFactor.msg b/eagleye_msgs/msg/VelocityScaleFactor.msg index 1744264c..884ae04c 100644 --- a/eagleye_msgs/msg/VelocityScaleFactor.msg +++ b/eagleye_msgs/msg/VelocityScaleFactor.msg @@ -1,5 +1,4 @@ std_msgs/Header header float64 scale_factor -geometry_msgs/Twist correction_velocity eagleye_msgs/Status status diff --git a/eagleye_rt/CMakeLists.txt b/eagleye_rt/CMakeLists.txt index 740ad181..08450d9b 100644 --- a/eagleye_rt/CMakeLists.txt +++ b/eagleye_rt/CMakeLists.txt @@ -12,11 +12,16 @@ endif() # find dependencies find_package(ament_cmake_auto REQUIRED) +find_package(PkgConfig REQUIRED) ament_auto_find_build_dependencies() include_directories(include) +ament_auto_add_executable(tf_converted_imu + src/tf_converted_imu.cpp +) + ament_auto_add_executable(correction_imu src/correction_imu.cpp ) @@ -97,7 +102,17 @@ 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 heading_interpolate diff --git a/eagleye_rt/config/eagleye_config.yaml b/eagleye_rt/config/eagleye_config.yaml index 2c6bf879..fe0bebf0 100644 --- a/eagleye_rt/config/eagleye_config.yaml +++ b/eagleye_rt/config/eagleye_config.yaml @@ -10,13 +10,9 @@ rtklib_nav_topic: /rtklib_nav ## If RTK positioning is not used, the following topic settings are not necessary. nmea_sentence_topic: /navsat/nmea_sentence + gga_topic: /navsat/gga localization_pose_topic: /localization/pose # This parameter is used to input external poses such as map matching to eagleye. (Normally it is not used.) - #Parameter settings for Eagleye - reverse_imu: false # reverse imu angular_velocity.z - reverse_imu_angular_velocity_x: true # reverse imu angular_velocity.x - reverse_imu_linear_acceleration_y: false # reverse imu linear_acceleration.y - tf_gnss_flame: parent: "base_link" child: "gnss" @@ -95,6 +91,8 @@ trajectory: stop_judgment_velocity_threshold: 0.01 #Speed threshold for judgment at stop. (default:0.01 m/s) + timer_updata_rate: 10.0 #Period of timer to detect the absence of input twist (default:10 hz) + th_deadlock_time: 1.0 #Time to allow input twist not to come (default:1s) position: estimated_distance: 300.0 #Vehicle trajectory length used for estimation. (default:300 m) @@ -138,5 +136,30 @@ filter_process_noise: 0.01 filter_observation_noise: 1.0 + velocity_estimator: + gga_downsample_time: 1.0 + stop_judgment_velocity_threshold: 0.2 + stop_judgment_buffer_maxnum: 50.0 + variance_threshold: 0.000025 + pitchrate_offset: + buffer_count_max: 400 + pitching: + buffer_max: 250 + outlier_threshold: 0.0174 + estimated_velocity_threshold: 0.3 + estimated_gnss_coefficient: 0.02 + estimated_coefficient: 0.0125 + acceleration_offset: + buffer_min: 1500.0 + buffer_max: 25000.0 + filter_process_noise: 0.01 + filter_observation_noise: 1.0 + doppler_fusion: + buffer_max: 200.0 + estimated_gnss_coefficient: 0.02 + estimated_coefficient: 0.01 + outlier_threshold: 0.1 + monitor: print_status: true + log_output_status: false diff --git a/eagleye_rt/launch/eagleye_calibration.launch.xml b/eagleye_rt/launch/eagleye_calibration.launch.xml index 5d160806..85f60965 100644 --- a/eagleye_rt/launch/eagleye_calibration.launch.xml +++ b/eagleye_rt/launch/eagleye_calibration.launch.xml @@ -1,7 +1,7 @@ - + @@ -10,6 +10,9 @@ + + + diff --git a/eagleye_rt/launch/eagleye_rt.launch.xml b/eagleye_rt/launch/eagleye_rt.launch.xml index c10ac3b7..25ea0276 100644 --- a/eagleye_rt/launch/eagleye_rt.launch.xml +++ b/eagleye_rt/launch/eagleye_rt.launch.xml @@ -1,7 +1,7 @@ - + @@ -11,6 +11,9 @@ + + + diff --git a/eagleye_rt/launch/eagleye_rt_canless.launch.xml b/eagleye_rt/launch/eagleye_rt_canless.launch.xml new file mode 100644 index 00000000..3f5a5902 --- /dev/null +++ b/eagleye_rt/launch/eagleye_rt_canless.launch.xml @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/eagleye_rt/launch/navpvt_rt.launch.xml b/eagleye_rt/launch/navpvt_rt.launch.xml new file mode 100644 index 00000000..a1a13eb7 --- /dev/null +++ b/eagleye_rt/launch/navpvt_rt.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/eagleye_rt/log/.gitignore b/eagleye_rt/log/.gitignore new file mode 100644 index 00000000..e69de29b diff --git a/eagleye_rt/package.xml b/eagleye_rt/package.xml index 92228c18..d2a03089 100644 --- a/eagleye_rt/package.xml +++ b/eagleye_rt/package.xml @@ -10,6 +10,8 @@ BSD ament_cmake + yaml_cpp_vendor + tf2_eigen rclcpp std_msgs geometry_msgs @@ -25,6 +27,7 @@ tf2 tf2_ros tf2_geometry_msgs + ublox_msgs rclcpp std_msgs geometry_msgs @@ -40,6 +43,7 @@ tf2 tf2_ros tf2_geometry_msgs + ublox_msgs rclcpp std_msgs geometry_msgs @@ -55,6 +59,7 @@ tf2 tf2_ros tf2_geometry_msgs + ublox_msgs ament_lint_auto ament_lint_common diff --git a/eagleye_rt/src/angular_velocity_offset_stop_node.cpp b/eagleye_rt/src/angular_velocity_offset_stop_node.cpp index 155e5510..adfb3b25 100644 --- a/eagleye_rt/src/angular_velocity_offset_stop_node.cpp +++ b/eagleye_rt/src/angular_velocity_offset_stop_node.cpp @@ -65,31 +65,24 @@ int main(int argc, char** argv) auto node = rclcpp::Node::make_shared("angular_velocity_offset_stop"); std::string subscribe_twist_topic_name = "/can_twist"; - std::string subscribe_imu_topic_name = "/imu/data_raw"; node->declare_parameter("twist_topic",subscribe_twist_topic_name); - node->declare_parameter("imu_topic",subscribe_imu_topic_name); - node->declare_parameter("reverse_imu", angular_velocity_offset_stop_parameter.reverse_imu); node->declare_parameter("angular_velocity_offset_stop.stop_judgment_velocity_threshold",angular_velocity_offset_stop_parameter.stop_judgment_velocity_threshold); node->declare_parameter("angular_velocity_offset_stop.estimated_number",angular_velocity_offset_stop_parameter.estimated_number); node->declare_parameter("angular_velocity_offset_stop.outlier_threshold",angular_velocity_offset_stop_parameter.outlier_threshold); node->get_parameter("twist_topic",subscribe_twist_topic_name); - node->get_parameter("imu_topic",subscribe_imu_topic_name); - node->get_parameter("reverse_imu", angular_velocity_offset_stop_parameter.reverse_imu); node->get_parameter("angular_velocity_offset_stop.stop_judgment_velocity_threshold",angular_velocity_offset_stop_parameter.stop_judgment_velocity_threshold); node->get_parameter("angular_velocity_offset_stop.estimated_number",angular_velocity_offset_stop_parameter.estimated_number); node->get_parameter("angular_velocity_offset_stop.outlier_threshold",angular_velocity_offset_stop_parameter.outlier_threshold); std::cout<< "subscribe_twist_topic_name "<create_subscription(subscribe_twist_topic_name, 1000, velocity_callback); - auto sub2 = node->create_subscription(subscribe_imu_topic_name, 1000, imu_callback); + auto sub2 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); pub = node->create_publisher("angular_velocity_offset_stop", 1000); rclcpp::spin(node); diff --git a/eagleye_rt/src/correction_imu.cpp b/eagleye_rt/src/correction_imu.cpp index 51957ba9..31df5ca9 100644 --- a/eagleye_rt/src/correction_imu.cpp +++ b/eagleye_rt/src/correction_imu.cpp @@ -32,8 +32,6 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -static bool reverse_imu; - rclcpp::Publisher::SharedPtr pub; static eagleye_msgs::msg::YawrateOffset yawrate_offset; static eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop; @@ -87,18 +85,9 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) correction_imu.linear_acceleration.z = imu.linear_acceleration.z; } - if (reverse_imu == false) - { - correction_imu.angular_velocity.x = imu.angular_velocity.x + angular_velocity_offset_stop.angular_velocity_offset.x; - correction_imu.angular_velocity.y = imu.angular_velocity.y + angular_velocity_offset_stop.angular_velocity_offset.y; - correction_imu.angular_velocity.z = -1 * (imu.angular_velocity.z + angular_velocity_offset_stop.angular_velocity_offset.z); - } - else if (reverse_imu == true) - { - correction_imu.angular_velocity.x = imu.angular_velocity.x + angular_velocity_offset_stop.angular_velocity_offset.x; - correction_imu.angular_velocity.y = imu.angular_velocity.y + angular_velocity_offset_stop.angular_velocity_offset.y; - correction_imu.angular_velocity.z = -1 * (-1 * (imu.angular_velocity.z + angular_velocity_offset_stop.angular_velocity_offset.z)); - } + correction_imu.angular_velocity.x = imu.angular_velocity.x + angular_velocity_offset_stop.angular_velocity_offset.x; + correction_imu.angular_velocity.y = imu.angular_velocity.y + angular_velocity_offset_stop.angular_velocity_offset.y; + correction_imu.angular_velocity.z = -1 * (imu.angular_velocity.z + angular_velocity_offset_stop.angular_velocity_offset.z); pub->publish(correction_imu); } @@ -108,21 +97,11 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("correction_imu"); - std::string subscribe_imu_topic_name = "/imu/data_raw"; - - node->declare_parameter("imu_topic",subscribe_imu_topic_name); - node->declare_parameter("reverse_imu", reverse_imu); - - node->get_parameter("imu_topic",subscribe_imu_topic_name); - node->get_parameter("reverse_imu", reverse_imu); - std::cout<< "subscribe_imu_topic_name "<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() auto sub3 = node->create_subscription("acc_x_offset", rclcpp::QoS(10), acc_x_offset_callback); //ros::TransportHints().tcpNoDelay() auto sub4 = node->create_subscription("acc_x_scale_factor", rclcpp::QoS(10), acc_x_scale_factor_callback); //ros::TransportHints().tcpNoDelay() - auto sub5 = node->create_subscription(subscribe_imu_topic_name, 1000, imu_callback); //ros::TransportHints().tcpNoDelay() + auto sub5 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); //ros::TransportHints().tcpNoDelay() pub = node->create_publisher("imu/data_corrected", rclcpp::QoS(10)); rclcpp::spin(node); diff --git a/eagleye_rt/src/distance_node.cpp b/eagleye_rt/src/distance_node.cpp index 46e0d172..b07a9325 100644 --- a/eagleye_rt/src/distance_node.cpp +++ b/eagleye_rt/src/distance_node.cpp @@ -32,22 +32,32 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -rclcpp::Publisher::SharedPtr pub; -static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; -static eagleye_msgs::msg::Distance distance; +rclcpp::Publisher::SharedPtr _pub; +static geometry_msgs::msg::TwistStamped _velocity; +static eagleye_msgs::msg::StatusStamped _velocity_status; +static eagleye_msgs::msg::Distance _distance; -struct DistanceStatus distance_status; +struct DistanceStatus _distance_status; -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) +static bool _use_canless_mode; + +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) +{ + _velocity_status = *msg; +} + +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - velocity_scale_factor = *msg; - distance.header = msg->header; - distance.header.frame_id = "base_link"; - distance_estimate(velocity_scale_factor,&distance_status,&distance); + if(_use_canless_mode && !_velocity_status.status.enabled_status) return; + + _velocity = *msg; + _distance.header = msg->header; + _distance.header.frame_id = "base_link"; + distance_estimate(_velocity, &_distance_status, &_distance); - if (distance_status.time_last != 0) + if (_distance_status.time_last != 0) { - pub->publish(distance); + _pub->publish(_distance); } } @@ -56,8 +66,12 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("distance"); - auto sub1 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); - pub = node->create_publisher("distance", rclcpp::QoS(10)); + node->declare_parameter("use_canless_mode",_use_canless_mode); + node->get_parameter("use_canless_mode",_use_canless_mode); + + auto sub1 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); + auto sub2 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); + _pub = node->create_publisher("distance", rclcpp::QoS(10)); rclcpp::spin(node); diff --git a/eagleye_rt/src/enable_additional_rolling_node.cpp b/eagleye_rt/src/enable_additional_rolling_node.cpp index 4bf1961f..57274a06 100644 --- a/eagleye_rt/src/enable_additional_rolling_node.cpp +++ b/eagleye_rt/src/enable_additional_rolling_node.cpp @@ -35,6 +35,8 @@ rclcpp::Publisher::SharedPtr _pub1; rclcpp::Publisher::SharedPtr _pub2; static eagleye_msgs::msg::VelocityScaleFactor _velocity_scale_factor; +static geometry_msgs::msg::TwistStamped _velocity; +static eagleye_msgs::msg::StatusStamped _velocity_status; static eagleye_msgs::msg::YawrateOffset _yawrate_offset_2nd; static eagleye_msgs::msg::YawrateOffset _yawrate_offset_stop; static eagleye_msgs::msg::Distance _distance; @@ -48,44 +50,69 @@ static eagleye_msgs::msg::AccYOffset _acc_y_offset; struct EnableAdditionalRollingParameter _rolling_parameter; struct EnableAdditionalRollingStatus _rolling_status; -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstPtr &msg) +static bool _use_canless_mode; + +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + _velocity = *msg; +} + +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) +{ + _velocity_status = *msg; +} + +void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) { _velocity_scale_factor = *msg; } -void distance_callback(const eagleye_msgs::msg::Distance::ConstPtr& msg) +void distance_callback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) { _distance = *msg; } -void yawrate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstPtr &msg) +void yawrate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { _yawrate_offset_2nd = *msg; } -void yawrate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstPtr &msg) +void yawrate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { _yawrate_offset_stop = *msg; } -void localization_pose_callback(const geometry_msgs::msg::PoseStamped::ConstPtr &msg) +void localization_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) { _localization_pose = *msg; } -void angular_velocity_offset_stop_callback(const eagleye_msgs::msg::AngularVelocityOffset::ConstPtr &msg) +void angular_velocity_offset_stop_callback(const eagleye_msgs::msg::AngularVelocityOffset::ConstSharedPtr msg) { _angular_velocity_offset_stop = *msg; } -void imu_callback(const sensor_msgs::msg::Imu::ConstPtr &msg) +void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { + if(_use_canless_mode && !_velocity_status.status.enabled_status) return; + + eagleye_msgs::msg::StatusStamped velocity_enable_status; + if(_use_canless_mode) + { + velocity_enable_status = _velocity_status; + } + else + { + velocity_enable_status.header = _velocity_scale_factor.header; + velocity_enable_status.status = _velocity_scale_factor.status; + } + _imu = *msg; _acc_y_offset.header = msg->header; _acc_y_offset.header.frame_id = "imu"; _rolling_angle.header = msg->header; _rolling_angle.header.frame_id = "base_link"; - enable_additional_rolling_estimate(_velocity_scale_factor, _yawrate_offset_2nd, _yawrate_offset_stop, _distance, _imu, + enable_additional_rolling_estimate(_velocity, velocity_enable_status, _yawrate_offset_2nd, _yawrate_offset_stop, _distance, _imu, _localization_pose, _angular_velocity_offset_stop, _rolling_parameter, &_rolling_status, &_rolling_angle, &_acc_y_offset); _pub1->publish(_acc_y_offset); _pub2->publish(_rolling_angle); @@ -96,36 +123,25 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("enable_additional_rolling"); - std::string subscribe_imu_topic_name; std::string subscribe_localization_pose_topic_name; node->declare_parameter("localization_pose_topic",subscribe_localization_pose_topic_name); - node->declare_parameter("imu_topic",subscribe_imu_topic_name); - node->declare_parameter("reverse_imu",_rolling_parameter.reverse_imu); - node->declare_parameter("reverse_imu_angular_velocity_x",_rolling_parameter.reverse_imu_angular_velocity_x); - node->declare_parameter("reverse_imu_linear_acceleration_y",_rolling_parameter.reverse_imu_linear_acceleration_y); node->declare_parameter("enable_additional_rolling.matching_update_distance",_rolling_parameter.matching_update_distance); node->declare_parameter("enable_additional_rolling.stop_judgment_velocity_threshold",_rolling_parameter.stop_judgment_velocity_threshold); node->declare_parameter("enable_additional_rolling.rolling_buffer_num",_rolling_parameter.rolling_buffer_num); node->declare_parameter("enable_additional_rolling.link_Time_stamp_parameter",_rolling_parameter.link_Time_stamp_parameter); node->declare_parameter("enable_additional_rolling.imu_buffer_num",_rolling_parameter.imu_buffer_num); + node->declare_parameter("use_canless_mode",_use_canless_mode); node->get_parameter("localization_pose_topic",subscribe_localization_pose_topic_name); - node->get_parameter("imu_topic",subscribe_imu_topic_name); - node->get_parameter("reverse_imu",_rolling_parameter.reverse_imu); - node->get_parameter("reverse_imu_angular_velocity_x",_rolling_parameter.reverse_imu_angular_velocity_x); - node->get_parameter("reverse_imu_linear_acceleration_y",_rolling_parameter.reverse_imu_linear_acceleration_y); node->get_parameter("enable_additional_rolling.matching_update_distance",_rolling_parameter.matching_update_distance); node->get_parameter("enable_additional_rolling.stop_judgment_velocity_threshold",_rolling_parameter.stop_judgment_velocity_threshold); node->get_parameter("enable_additional_rolling.rolling_buffer_num",_rolling_parameter.rolling_buffer_num); node->get_parameter("enable_additional_rolling.link_Time_stamp_parameter",_rolling_parameter.link_Time_stamp_parameter); node->get_parameter("enable_additional_rolling.imu_buffer_num",_rolling_parameter.imu_buffer_num); + node->get_parameter("use_canless_modeum",_use_canless_mode); std::cout<< "subscribe_localization_pose_topic_name: " << subscribe_localization_pose_topic_name << std::endl; - std::cout<< "subscribe_imu_topic_name: " << subscribe_imu_topic_name << std::endl; - std::cout<< "reverse_imu: " << _rolling_parameter.reverse_imu << std::endl; - std::cout<< "reverse_imu_angular_velocity_x: " << _rolling_parameter.reverse_imu_angular_velocity_x << std::endl; - std::cout<< "reverse_imu_linear_acceleration_y: " << _rolling_parameter.reverse_imu_linear_acceleration_y << std::endl; std::cout<< "matching_update_distance: " << _rolling_parameter.matching_update_distance << std::endl; std::cout<< "stop_judgment_velocity_threshold: " << _rolling_parameter.stop_judgment_velocity_threshold << std::endl; std::cout<< "rolling_buffer_num: " << _rolling_parameter.rolling_buffer_num << std::endl; @@ -138,7 +154,7 @@ int main(int argc, char** argv) auto sub4 = node->create_subscription("distance", 1000, distance_callback); auto sub5 = node->create_subscription(subscribe_localization_pose_topic_name, 1000, localization_pose_callback); auto sub6 = node->create_subscription("angular_velocity_offset_stop", 1000, angular_velocity_offset_stop_callback); - auto sub7 = node->create_subscription(subscribe_imu_topic_name, 1000, imu_callback); + auto sub7 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); _pub1 = node->create_publisher("acc_y_offset_additional_rolling", 1000); _pub2 = node->create_publisher("enable_additional_rolling", 1000); diff --git a/eagleye_rt/src/heading_interpolate_node.cpp b/eagleye_rt/src/heading_interpolate_node.cpp index 810ae3bb..96ba76c9 100644 --- a/eagleye_rt/src/heading_interpolate_node.cpp +++ b/eagleye_rt/src/heading_interpolate_node.cpp @@ -33,7 +33,8 @@ #include "eagleye_navigation/eagleye_navigation.hpp" static sensor_msgs::msg::Imu imu; -static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; +static geometry_msgs::msg::TwistStamped velocity; +static eagleye_msgs::msg::StatusStamped velocity_status; static eagleye_msgs::msg::YawrateOffset yawrate_offset_stop; static eagleye_msgs::msg::YawrateOffset yawrate_offset; static eagleye_msgs::msg::Heading heading; @@ -45,9 +46,16 @@ static eagleye_msgs::msg::Heading heading_interpolate; struct HeadingInterpolateParameter heading_interpolate_parameter; struct HeadingInterpolateStatus heading_interpolate_status; -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) +static bool _use_canless_mode; + +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + velocity = *msg; +} + +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { - velocity_scale_factor = *msg; + velocity_status = *msg; } void yawrate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) @@ -72,10 +80,13 @@ void slip_angle_callback(const eagleye_msgs::msg::SlipAngle::ConstSharedPtr msg) void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { + if(_use_canless_mode && !velocity_status.status.enabled_status) return; + imu = *msg; heading_interpolate.header = msg->header; heading_interpolate.header.frame_id = "base_link"; - heading_interpolate_estimate(imu,velocity_scale_factor,yawrate_offset_stop,yawrate_offset,heading,slip_angle,heading_interpolate_parameter,&heading_interpolate_status,&heading_interpolate); + heading_interpolate_estimate(imu,velocity,yawrate_offset_stop,yawrate_offset,heading,slip_angle,heading_interpolate_parameter, + &heading_interpolate_status,&heading_interpolate); pub->publish(heading_interpolate); } @@ -84,19 +95,12 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("heading_interpolate"); - std::string subscribe_imu_topic_name = "/imu/data_raw"; - - node->declare_parameter("imu_topic",subscribe_imu_topic_name); - node->declare_parameter("reverse_imu", heading_interpolate_parameter.reverse_imu); node->declare_parameter("heading_interpolate.stop_judgment_velocity_threshold", heading_interpolate_parameter.stop_judgment_velocity_threshold); node->declare_parameter("heading_interpolate.number_buffer_max", heading_interpolate_parameter.number_buffer_max); - node->get_parameter("imu_topic",subscribe_imu_topic_name); - node->get_parameter("reverse_imu", heading_interpolate_parameter.reverse_imu); node->get_parameter("heading_interpolate.stop_judgment_velocity_threshold", heading_interpolate_parameter.stop_judgment_velocity_threshold); node->get_parameter("heading_interpolate.number_buffer_max", heading_interpolate_parameter.number_buffer_max); - std::cout<< "subscribe_imu_topic_name "<create_subscription(subscribe_imu_topic_name, 1000, imu_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); //ros::TransportHints().tcpNoDelay() - auto sub3 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); //ros::TransportHints().tcpNoDelay() - auto sub4 = node->create_subscription(subscribe_topic_name_1, 1000, yawrate_offset_callback); //ros::TransportHints().tcpNoDelay() - auto sub5 = node->create_subscription(subscribe_topic_name_2, 1000, heading_callback); //ros::TransportHints().tcpNoDelay() - auto sub6 = node->create_subscription("slip_angle", rclcpp::QoS(10), slip_angle_callback); //ros::TransportHints().tcpNoDelay() + auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); + auto sub2 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); + auto sub3 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); + auto sub4 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); + auto sub5 = node->create_subscription(subscribe_topic_name_1, 1000, yawrate_offset_callback); + auto sub6 = node->create_subscription(subscribe_topic_name_2, 1000, heading_callback); + auto sub7 = node->create_subscription("slip_angle", rclcpp::QoS(10), slip_angle_callback); pub = node->create_publisher(publish_topic_name, rclcpp::QoS(10)); rclcpp::spin(node); diff --git a/eagleye_rt/src/heading_node.cpp b/eagleye_rt/src/heading_node.cpp index 205f6b9c..22ec9023 100644 --- a/eagleye_rt/src/heading_node.cpp +++ b/eagleye_rt/src/heading_node.cpp @@ -35,7 +35,8 @@ static rtklib_msgs::msg::RtklibNav rtklib_nav; static nmea_msgs::msg::Gprmc nmea_rmc; static sensor_msgs::msg::Imu imu; -static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; +static geometry_msgs::msg::TwistStamped velocity; +static eagleye_msgs::msg::StatusStamped velocity_status; static eagleye_msgs::msg::YawrateOffset yawrate_offset_stop; static eagleye_msgs::msg::YawrateOffset yawrate_offset; static eagleye_msgs::msg::SlipAngle slip_angle; @@ -48,6 +49,7 @@ struct HeadingParameter heading_parameter; struct HeadingStatus heading_status; std::string use_gnss_mode; +static bool use_canless_mode; bool is_first_correction_velocity = false; @@ -56,15 +58,20 @@ void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) rtklib_nav = *msg; } -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - velocity_scale_factor = *msg; - if (is_first_correction_velocity == false && msg->correction_velocity.linear.x > heading_parameter.estimated_velocity_threshold) + velocity = *msg; + if (is_first_correction_velocity == false && msg->twist.linear.x > heading_parameter.estimated_velocity_threshold) { is_first_correction_velocity = true; } } +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) +{ + velocity_status = *msg; +} + void yawrate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { yawrate_offset_stop = *msg; @@ -92,17 +99,16 @@ void rmc_callback(const nmea_msgs::msg::Gprmc::ConstSharedPtr msg) void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - if (is_first_correction_velocity == false) - { - return; - } + if (!is_first_correction_velocity) return; + if(use_canless_mode && !velocity_status.status.enabled_status) return; + imu = *msg; heading.header = msg->header; heading.header.frame_id = "base_link"; if (use_gnss_mode == "rtklib" || use_gnss_mode == "RTKLIB") // use RTKLIB mode - heading_estimate(rtklib_nav,imu,velocity_scale_factor,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,&heading_status,&heading); + 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 - heading_estimate(nmea_rmc,imu,velocity_scale_factor,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,&heading_status,&heading); + 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) { pub->publish(heading); @@ -115,13 +121,12 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("heading"); - std::string subscribe_imu_topic_name = "/imu/data_raw"; + std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; std::string subscribe_rmc_topic_name = "/navsat/rmc"; - node->declare_parameter("imu_topic",subscribe_imu_topic_name); + node->declare_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - node->declare_parameter("reverse_imu", heading_parameter.reverse_imu); node->declare_parameter("heading.estimated_number_min",heading_parameter.estimated_number_min); node->declare_parameter("heading.estimated_number_max",heading_parameter.estimated_number_max); node->declare_parameter("heading.estimated_gnss_coefficient",heading_parameter.estimated_gnss_coefficient); @@ -131,10 +136,9 @@ int main(int argc, char** argv) node->declare_parameter("heading.stop_judgment_velocity_threshold",heading_parameter.stop_judgment_velocity_threshold); node->declare_parameter("heading.estimated_yawrate_threshold",heading_parameter.estimated_yawrate_threshold); node->declare_parameter("use_gnss_mode",use_gnss_mode); + node->declare_parameter("use_canless_mode",use_canless_mode); - node->get_parameter("imu_topic",subscribe_imu_topic_name); node->get_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - node->get_parameter("reverse_imu", heading_parameter.reverse_imu); node->get_parameter("heading.estimated_number_min",heading_parameter.estimated_number_min); node->get_parameter("heading.estimated_number_max",heading_parameter.estimated_number_max); node->get_parameter("heading.estimated_gnss_coefficient",heading_parameter.estimated_gnss_coefficient); @@ -144,10 +148,9 @@ int main(int argc, char** argv) node->get_parameter("heading.stop_judgment_velocity_threshold",heading_parameter.stop_judgment_velocity_threshold); node->get_parameter("heading.estimated_yawrate_threshold",heading_parameter.estimated_yawrate_threshold); node->get_parameter("use_gnss_mode",use_gnss_mode); + node->get_parameter("use_canless_mode",use_canless_mode); - std::cout<< "subscribe_imu_topic_name "<create_subscription(subscribe_imu_topic_name, 1000, imu_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); //ros::TransportHints().tcpNoDelay() + 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_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); //ros::TransportHints().tcpNoDelay() - auto sub5 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); //ros::TransportHints().tcpNoDelay() - auto sub6 = node->create_subscription(subscribe_topic_name, 1000, yawrate_offset_callback); //ros::TransportHints().tcpNoDelay() - auto sub7 = node->create_subscription("slip_angle", rclcpp::QoS(10), slip_angle_callback); //ros::TransportHints().tcpNoDelay() - auto sub8 = node->create_subscription(subscribe_topic_name2 , 1000, heading_interpolate_callback); //ros::TransportHints().tcpNoDelay() + 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); 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 7084e51a..13ee8669 100644 --- a/eagleye_rt/src/height_node.cpp +++ b/eagleye_rt/src/height_node.cpp @@ -34,7 +34,8 @@ static sensor_msgs::msg::Imu imu; static nmea_msgs::msg::Gpgga gga; - static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; + static geometry_msgs::msg::TwistStamped velocity; + static eagleye_msgs::msg::StatusStamped velocity_status; static eagleye_msgs::msg::Distance distance; rclcpp::Publisher::SharedPtr pub1; @@ -50,14 +51,21 @@ struct HeightParameter height_parameter; struct HeightStatus height_status; + static bool use_canless_mode; + void gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) { gga = *msg; } -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + velocity = *msg; +} + +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { - velocity_scale_factor = *msg; + velocity_status = *msg; } void distance_callback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) @@ -67,6 +75,8 @@ void distance_callback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { + if(use_canless_mode && !velocity_status.status.enabled_status) return; + imu = *msg; height.header = msg->header; height.header.frame_id = "base_link"; @@ -74,7 +84,7 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) pitching.header.frame_id = "base_link"; acc_x_offset.header = msg->header; acc_x_scale_factor.header = msg->header; - pitching_estimate(imu,gga,velocity_scale_factor,distance,height_parameter,&height_status,&height,&pitching,&acc_x_offset,&acc_x_scale_factor); + pitching_estimate(imu,gga,velocity,distance,height_parameter,&height_status,&height,&pitching,&acc_x_offset,&acc_x_scale_factor); pub1->publish(height); pub2->publish(pitching); pub3->publish(acc_x_offset); @@ -98,10 +108,9 @@ int main(int argc, char** argv) auto node = rclcpp::Node::make_shared("height"); std::string subscribe_gga_topic_name = "/navsat/gga"; - std::string subscribe_imu_topic_name = "/imu/data_raw"; + node->declare_parameter("gga_topic",subscribe_gga_topic_name); - node->declare_parameter("imu_topic",subscribe_imu_topic_name); node->declare_parameter("height.estimated_distance",height_parameter.estimated_distance); node->declare_parameter("height.estimated_distance_max",height_parameter.estimated_distance_max); node->declare_parameter("height.separation_distance",height_parameter.separation_distance); @@ -110,9 +119,9 @@ int main(int argc, char** argv) node->declare_parameter("height.estimated_height_coefficient",height_parameter.estimated_height_coefficient); node->declare_parameter("height.outlier_threshold",height_parameter.outlier_threshold); node->declare_parameter("height.average_num",height_parameter.average_num); + node->declare_parameter("use_canless_mode",use_canless_mode); node->get_parameter("gga_topic",subscribe_gga_topic_name); - node->get_parameter("imu_topic",subscribe_imu_topic_name); node->get_parameter("height.estimated_distance",height_parameter.estimated_distance); node->get_parameter("height.estimated_distance_max",height_parameter.estimated_distance_max); node->get_parameter("height.separation_distance",height_parameter.separation_distance); @@ -121,9 +130,9 @@ int main(int argc, char** argv) node->get_parameter("height.estimated_height_coefficient",height_parameter.estimated_height_coefficient); node->get_parameter("height.outlier_threshold",height_parameter.outlier_threshold); node->get_parameter("height.average_num",height_parameter.average_num); + node->get_parameter("use_canless_mode",use_canless_mode); std::cout<< "subscribe_gga_topic_name "<create_subscription(subscribe_imu_topic_name, 1000, imu_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); //ros::TransportHints().tcpNoDelay() - auto sub3 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); //ros::TransportHints().tcpNoDelay() - auto sub4 = node->create_subscription("distance", rclcpp::QoS(10), distance_callback); //ros::TransportHints().tcpNoDelay() + auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); + auto sub2 = node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); + auto sub3 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); + auto sub4 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); + auto sub5 = node->create_subscription("distance", rclcpp::QoS(10), distance_callback); std::string publish_height_topic_name = "height"; std::string publish_pitching_topic_name = "pitching"; diff --git a/eagleye_rt/src/monitor_node.cpp b/eagleye_rt/src/monitor_node.cpp index 436f1200..4f37d399 100755 --- a/eagleye_rt/src/monitor_node.cpp +++ b/eagleye_rt/src/monitor_node.cpp @@ -33,677 +33,698 @@ #include "eagleye_navigation/eagleye_navigation.hpp" #include - -static sensor_msgs::msg::Imu imu; -static rtklib_msgs::msg::RtklibNav rtklib_nav; -static sensor_msgs::msg::NavSatFix fix; -static nmea_msgs::msg::Gpgga gga; -static geometry_msgs::msg::TwistStamped velocity; -static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; -static eagleye_msgs::msg::Distance distance; -static eagleye_msgs::msg::Heading heading_1st; -static eagleye_msgs::msg::Heading heading_interpolate_1st; -static eagleye_msgs::msg::Heading heading_2nd; -static eagleye_msgs::msg::Heading heading_interpolate_2nd; -static eagleye_msgs::msg::Heading heading_3rd; -static eagleye_msgs::msg::Heading heading_interpolate_3rd; -static eagleye_msgs::msg::YawrateOffset yawrate_offset_stop; -static eagleye_msgs::msg::YawrateOffset yawrate_offset_1st; -static eagleye_msgs::msg::YawrateOffset yawrate_offset_2nd; -static eagleye_msgs::msg::SlipAngle slip_angle; -static eagleye_msgs::msg::Height height; -static eagleye_msgs::msg::Pitching pitching; -static eagleye_msgs::msg::Position enu_relative_pos; -static geometry_msgs::msg::Vector3Stamped enu_vel; -static eagleye_msgs::msg::Position enu_absolute_pos; -static eagleye_msgs::msg::Position enu_absolute_pos_interpolate; -static sensor_msgs::msg::NavSatFix eagleye_fix; -static geometry_msgs::msg::TwistStamped eagleye_twist; - -static bool gga_sub_status; -static bool print_status; - -static double imu_time_last; -static double rtklib_nav_time_last; -static double navsat_gga_time_last; -static double velocity_time_last; -static double velocity_scale_factor_time_last; -static double distance_time_last; -static double heading_1st_time_last; -static double heading_interpolate_1st_time_last; -static double heading_2nd_time_last; -static double heading_interpolate_2nd_time_last; -static double heading_3rd_time_last; -static double heading_interpolate_3rd_time_last; -static double yawrate_offset_stop_time_last; -static double yawrate_offset_1st_time_last; -static double yawrate_offset_2nd_time_last; -static double slip_angle_time_last; -static double height_time_last; -static double pitching_time_last; -static double enu_vel_time_last; -static double enu_absolute_pos_time_last; -static double enu_absolute_pos_interpolate_time_last; -static double eagleye_twist_time_last; - -static double update_rate = 10; -static double th_gnss_deadrock_time = 10; +#include + +static sensor_msgs::msg::Imu _imu; +static rtklib_msgs::msg::RtklibNav _rtklib_nav; +static sensor_msgs::msg::NavSatFix _rtklib_fix; +static nmea_msgs::msg::Gpgga _gga; +static geometry_msgs::msg::TwistStamped _velocity; +static geometry_msgs::msg::TwistStamped _correction_velocity; +static eagleye_msgs::msg::VelocityScaleFactor _velocity_scale_factor; +static eagleye_msgs::msg::Distance _distance; +static eagleye_msgs::msg::Heading _heading_1st; +static eagleye_msgs::msg::Heading _heading_interpolate_1st; +static eagleye_msgs::msg::Heading _heading_2nd; +static eagleye_msgs::msg::Heading _heading_interpolate_2nd; +static eagleye_msgs::msg::Heading _heading_3rd; +static eagleye_msgs::msg::Heading _heading_interpolate_3rd; +static eagleye_msgs::msg::YawrateOffset _yawrate_offset_stop; +static eagleye_msgs::msg::YawrateOffset _yawrate_offset_1st; +static eagleye_msgs::msg::YawrateOffset _yawrate_offset_2nd; +static eagleye_msgs::msg::SlipAngle _slip_angle; +static eagleye_msgs::msg::Height _height; +static eagleye_msgs::msg::Pitching _pitching; +static eagleye_msgs::msg::Rolling _rolling; +static eagleye_msgs::msg::Position _enu_relative_pos; +static geometry_msgs::msg::Vector3Stamped _enu_vel; +static eagleye_msgs::msg::Position _enu_absolute_pos; +static eagleye_msgs::msg::Position _enu_absolute_pos_interpolate; +static sensor_msgs::msg::NavSatFix _eagleye_fix; +static geometry_msgs::msg::TwistStamped _eagleye_twist; + +static geometry_msgs::msg::TwistStamped::ConstSharedPtr _comparison_velocity_ptr; +static sensor_msgs::msg::Imu _corrected_imu; + +static bool _gga_sub_status; +static bool _print_status, _log_output_status, _log_header_make = false; +static std::string _output_log_dir; + +static double _imu_time_last; +static double _rtklib_nav_time_last; +static double _navsat_gga_time_last; +static double _velocity_time_last; +static double _velocity_scale_factor_time_last; +static double _distance_time_last; +static double _heading_1st_time_last; +static double _heading_interpolate_1st_time_last; +static double _heading_2nd_time_last; +static double _heading_interpolate_2nd_time_last; +static double _heading_3rd_time_last; +static double _heading_interpolate_3rd_time_last; +static double _yawrate_offset_stop_time_last; +static double _yawrate_offset_1st_time_last; +static double _yawrate_offset_2nd_time_last; +static double _slip_angle_time_last; +static double _height_time_last; +static double _pitching_time_last; +static double _enu_vel_time_last; +static double _enu_absolute_pos_time_last; +static double _enu_absolute_pos_interpolate_time_last; +static double _eagleye_twist_time_last; + +bool _use_compare_yawrate = false; +double _update_rate = 10.0; +double _th_gnss_deadrock_time = 10; +double _th_velocity_scale_factor_percent = 20; +double _th_diff_rad_per_sec = 0.17453; +int _num_continuous_abnormal_yawrate = 0; +int _th_num_continuous_abnormal_yawrate = 10; std::shared_ptr updater_; void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { - rtklib_nav = *msg; + _rtklib_nav = *msg; } -void fix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) +void rtklib_fix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) { - fix = *msg; + _rtklib_fix = *msg; } void navsatfix_gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) { - gga = *msg; - gga_sub_status = true; + _gga = *msg; + _gga_sub_status = true; } void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - velocity = *msg; + _velocity = *msg; +} + +void correction_velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + _correction_velocity = *msg; } void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) { - velocity_scale_factor = *msg; + _velocity_scale_factor = *msg; } void distance_callback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) { - distance = *msg; + _distance = *msg; } void heading_1st_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - heading_1st = *msg; + _heading_1st = *msg; } void heading_interpolate_1st_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - heading_interpolate_1st = *msg; + _heading_interpolate_1st = *msg; } void heading_2nd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - heading_2nd = *msg; + _heading_2nd = *msg; } void heading_interpolate_2nd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - heading_interpolate_2nd = *msg; + _heading_interpolate_2nd = *msg; } void heading_3rd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - heading_3rd = *msg; + _heading_3rd = *msg; } void heading_interpolate_3rd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - heading_interpolate_3rd = *msg; + _heading_interpolate_3rd = *msg; } void yawrate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - yawrate_offset_stop = *msg; + _yawrate_offset_stop = *msg; } void yawrate_offset_1st_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - yawrate_offset_1st = *msg; + _yawrate_offset_1st = *msg; } void yawrate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - yawrate_offset_2nd = *msg; + _yawrate_offset_2nd = *msg; } void slip_angle_callback(const eagleye_msgs::msg::SlipAngle::ConstSharedPtr msg) { - slip_angle = *msg; + _slip_angle = *msg; } void enu_relative_pos_callback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) { - enu_relative_pos = *msg; + _enu_relative_pos = *msg; } void enu_vel_callback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr msg) { - enu_vel = *msg; + _enu_vel = *msg; } void enu_absolute_pos_callback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) { - enu_absolute_pos = *msg; + _enu_absolute_pos = *msg; } void height_callback(const eagleye_msgs::msg::Height::ConstSharedPtr msg) { - height = *msg; + _height = *msg; } void pitching_callback(const eagleye_msgs::msg::Pitching::ConstSharedPtr msg) { - pitching = *msg; + _pitching = *msg; } +void rolling_callback(const eagleye_msgs::msg::Rolling::ConstSharedPtr msg) +{ + _rolling = *msg; +} void enu_absolute_pos_interpolate_callback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) { - enu_absolute_pos_interpolate = *msg; + _enu_absolute_pos_interpolate = *msg; } void eagleye_fix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) { - eagleye_fix = *msg; + _eagleye_fix = *msg; } void eagleye_twist_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - eagleye_twist = *msg; + _eagleye_twist = *msg; } void imu_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(imu.header.stamp); + rclcpp::Time ros_clock(_imu.header.stamp); auto imu_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (imu_time_last == imu_time) { + if (_imu_time_last == imu_time) { level = diagnostic_msgs::msg::DiagnosticStatus::STALE; msg = "not subscribed to topic"; } - imu_time_last = imu_time; + _imu_time_last = imu_time; stat.summary(level, msg); } void rtklib_nav_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(rtklib_nav.header.stamp); + rclcpp::Time ros_clock(_rtklib_nav.header.stamp); auto rtklib_nav_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (rtklib_nav_time_last - rtklib_nav_time > th_gnss_deadrock_time) { + if (_rtklib_nav_time_last - rtklib_nav_time > _th_gnss_deadrock_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed or deadlock of more than 10 seconds"; } - rtklib_nav_time_last = rtklib_nav_time; + _rtklib_nav_time_last = rtklib_nav_time; stat.summary(level, msg); } void navsat_gga_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(gga.header.stamp); + rclcpp::Time ros_clock(_gga.header.stamp); auto navsat_gga_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (navsat_gga_time_last - navsat_gga_time > th_gnss_deadrock_time || !gga_sub_status) { + if (_navsat_gga_time_last - navsat_gga_time > _th_gnss_deadrock_time || !_gga_sub_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - navsat_gga_time_last = navsat_gga_time; + _navsat_gga_time_last = navsat_gga_time; stat.summary(level, msg); } void velocity_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(velocity.header.stamp); + rclcpp::Time ros_clock(_velocity.header.stamp); auto velocity_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (velocity_time_last == velocity_time) { + if (_velocity_time_last == velocity_time) { level = diagnostic_msgs::msg::DiagnosticStatus::STALE; msg = "not subscribed to topic"; } - velocity_time_last = velocity_time; + _velocity_time_last = velocity_time; stat.summary(level, msg); } void velocity_scale_factor_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(velocity_scale_factor.header.stamp); + rclcpp::Time ros_clock(_velocity_scale_factor.header.stamp); auto velocity_scale_factor_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (velocity_scale_factor_time_last == velocity_scale_factor_time) { + if (_velocity_scale_factor_time_last == velocity_scale_factor_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(velocity_scale_factor.scale_factor)) { + else if (!std::isfinite(_velocity_scale_factor.scale_factor)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (!velocity_scale_factor.status.enabled_status) { + else if (!_velocity_scale_factor.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - velocity_scale_factor_time_last = velocity_scale_factor_time; + _velocity_scale_factor_time_last = velocity_scale_factor_time; stat.summary(level, msg); } void distance_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(distance.header.stamp); + rclcpp::Time ros_clock(_distance.header.stamp); auto distance_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (distance_time_last == distance_time) { + if (_distance_time_last == distance_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(distance.distance)) { + else if (!std::isfinite(_distance.distance)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (!distance.status.enabled_status) { + else if (!_distance.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - distance_time_last = distance_time; + _distance_time_last = distance_time; stat.summary(level, msg); } void heading_1st_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(heading_1st.header.stamp); + rclcpp::Time ros_clock(_heading_1st.header.stamp); auto heading_1st_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (!std::isfinite(heading_1st.heading_angle)) { + if (!std::isfinite(_heading_1st.heading_angle)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (heading_1st_time_last - heading_1st_time > th_gnss_deadrock_time) { + else if (_heading_1st_time_last - heading_1st_time > _th_gnss_deadrock_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed or deadlock of more than 10 seconds"; } - else if (!heading_1st.status.enabled_status) { + else if (!_heading_1st.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - heading_1st_time_last = heading_1st_time; + _heading_1st_time_last = heading_1st_time; stat.summary(level, msg); } void heading_interpolate_1st_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(heading_interpolate_1st.header.stamp); + rclcpp::Time ros_clock(_heading_interpolate_1st.header.stamp); auto heading_interpolate_1st_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (heading_interpolate_1st_time_last == heading_interpolate_1st_time) { + if (_heading_interpolate_1st_time_last == heading_interpolate_1st_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(heading_interpolate_1st.heading_angle)) { + else if (!std::isfinite(_heading_interpolate_1st.heading_angle)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (!heading_interpolate_1st.status.enabled_status) { + else if (!_heading_interpolate_1st.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - heading_interpolate_1st_time_last = heading_interpolate_1st_time; + _heading_interpolate_1st_time_last = heading_interpolate_1st_time; stat.summary(level, msg); } void heading_2nd_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(heading_2nd.header.stamp); + rclcpp::Time ros_clock(_heading_2nd.header.stamp); auto heading_2nd_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (!std::isfinite(heading_2nd.heading_angle)) { + if (!std::isfinite(_heading_2nd.heading_angle)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (heading_2nd_time_last - heading_2nd_time > th_gnss_deadrock_time) { + else if (_heading_2nd_time_last - heading_2nd_time > _th_gnss_deadrock_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed or deadlock of more than 10 seconds"; } - else if (!heading_2nd.status.enabled_status) { + else if (!_heading_2nd.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - heading_2nd_time_last = heading_2nd_time; + _heading_2nd_time_last = heading_2nd_time; stat.summary(level, msg); } void heading_interpolate_2nd_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(heading_interpolate_2nd.header.stamp); + rclcpp::Time ros_clock(_heading_interpolate_2nd.header.stamp); auto heading_interpolate_2nd_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (heading_interpolate_2nd_time_last == heading_interpolate_2nd_time) { + if (_heading_interpolate_2nd_time_last == heading_interpolate_2nd_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(heading_interpolate_2nd.heading_angle)) { + else if (!std::isfinite(_heading_interpolate_2nd.heading_angle)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (!heading_interpolate_2nd.status.enabled_status) { + else if (!_heading_interpolate_2nd.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - heading_interpolate_2nd_time_last = heading_interpolate_2nd_time; + _heading_interpolate_2nd_time_last = heading_interpolate_2nd_time; stat.summary(level, msg); } void heading_3rd_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(heading_3rd.header.stamp); + rclcpp::Time ros_clock(_heading_3rd.header.stamp); auto heading_3rd_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (!std::isfinite(heading_3rd.heading_angle)) { + if (!std::isfinite(_heading_3rd.heading_angle)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (heading_3rd_time_last - heading_3rd_time > th_gnss_deadrock_time) { + else if (_heading_3rd_time_last - heading_3rd_time > _th_gnss_deadrock_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed or deadlock of more than 10 seconds"; } - else if (!heading_3rd.status.enabled_status) { + else if (!_heading_3rd.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - heading_3rd_time_last = heading_3rd_time; + _heading_3rd_time_last = heading_3rd_time; stat.summary(level, msg); } void heading_interpolate_3rd_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(heading_interpolate_3rd.header.stamp); + rclcpp::Time ros_clock(_heading_interpolate_3rd.header.stamp); auto heading_interpolate_3rd_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (heading_interpolate_3rd_time_last == heading_interpolate_3rd_time) { + if (_heading_interpolate_3rd_time_last == heading_interpolate_3rd_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(heading_interpolate_3rd.heading_angle)) { + else if (!std::isfinite(_heading_interpolate_3rd.heading_angle)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (!heading_interpolate_3rd.status.enabled_status) { + else if (!_heading_interpolate_3rd.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - heading_interpolate_3rd_time_last = heading_interpolate_3rd_time; + _heading_interpolate_3rd_time_last = heading_interpolate_3rd_time; stat.summary(level, msg); } void yawrate_offset_stop_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(yawrate_offset_stop.header.stamp); + rclcpp::Time ros_clock(_yawrate_offset_stop.header.stamp); auto yawrate_offset_stop_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (yawrate_offset_stop_time_last == yawrate_offset_stop_time) { + if (_yawrate_offset_stop_time_last == yawrate_offset_stop_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(yawrate_offset_stop.yawrate_offset)) { + else if (!std::isfinite(_yawrate_offset_stop.yawrate_offset)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (!yawrate_offset_stop.status.enabled_status) { + else if (!_yawrate_offset_stop.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - yawrate_offset_stop_time_last = yawrate_offset_stop_time; + _yawrate_offset_stop_time_last = yawrate_offset_stop_time; stat.summary(level, msg); } void yawrate_offset_1st_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(yawrate_offset_1st.header.stamp); + rclcpp::Time ros_clock(_yawrate_offset_1st.header.stamp); auto yawrate_offset_1st_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (yawrate_offset_1st_time_last == yawrate_offset_1st_time) { + if (_yawrate_offset_1st_time_last == yawrate_offset_1st_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(yawrate_offset_1st.yawrate_offset)) { + else if (!std::isfinite(_yawrate_offset_1st.yawrate_offset)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (!yawrate_offset_1st.status.enabled_status) { + else if (!_yawrate_offset_1st.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - yawrate_offset_1st_time_last = yawrate_offset_1st_time; + _yawrate_offset_1st_time_last = yawrate_offset_1st_time; stat.summary(level, msg); } void yawrate_offset_2nd_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(yawrate_offset_2nd.header.stamp); + rclcpp::Time ros_clock(_yawrate_offset_2nd.header.stamp); auto yawrate_offset_2nd_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (yawrate_offset_2nd_time_last == yawrate_offset_2nd_time) { + if (_yawrate_offset_2nd_time_last == yawrate_offset_2nd_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(yawrate_offset_2nd.yawrate_offset)) { + else if (!std::isfinite(_yawrate_offset_2nd.yawrate_offset)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (!yawrate_offset_2nd.status.enabled_status) { + else if (!_yawrate_offset_2nd.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - yawrate_offset_2nd_time_last = yawrate_offset_2nd_time; + _yawrate_offset_2nd_time_last = yawrate_offset_2nd_time; stat.summary(level, msg); } void slip_angle_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(slip_angle.header.stamp); + rclcpp::Time ros_clock(_slip_angle.header.stamp); auto slip_angle_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (slip_angle_time_last == slip_angle_time) { + if (_slip_angle_time_last == slip_angle_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(slip_angle.slip_angle)) { + else if (!std::isfinite(_slip_angle.slip_angle)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (slip_angle.coefficient == 0) { + else if (_slip_angle.coefficient == 0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "/slip_angle/manual_coefficient is not set"; } - else if (!slip_angle.status.enabled_status) { + else if (!_slip_angle.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - slip_angle_time_last = slip_angle_time; + _slip_angle_time_last = slip_angle_time; stat.summary(level, msg); } void enu_vel_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(enu_vel.header.stamp); + rclcpp::Time ros_clock(_enu_vel.header.stamp); auto enu_vel_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (!std::isfinite(enu_vel.vector.x)||!std::isfinite(enu_vel.vector.y)||!std::isfinite(enu_vel.vector.z)) { + if (!std::isfinite(_enu_vel.vector.x)||!std::isfinite(_enu_vel.vector.y)||!std::isfinite(_enu_vel.vector.z)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (enu_vel_time_last == enu_vel_time) { + else if (_enu_vel_time_last == enu_vel_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - enu_vel_time_last = enu_vel_time; + _enu_vel_time_last = enu_vel_time; stat.summary(level, msg); } void height_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(height.header.stamp); + rclcpp::Time ros_clock(_height.header.stamp); auto height_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (height_time_last == height_time) { + if (_height_time_last == height_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(height.height)) { + else if (!std::isfinite(_height.height)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (!height.status.enabled_status) { + else if (!_height.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - height_time_last = height_time; + _height_time_last = height_time; stat.summary(level, msg); } void pitching_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(pitching.header.stamp); + rclcpp::Time ros_clock(_pitching.header.stamp); auto pitching_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (pitching_time_last == pitching_time) { + if (_pitching_time_last == pitching_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed to topic"; } - else if (!std::isfinite(pitching.pitching_angle)) { + else if (!std::isfinite(_pitching.pitching_angle)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (!pitching.status.enabled_status) { + else if (!_pitching.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - pitching_time_last = pitching_time; + _pitching_time_last = pitching_time; stat.summary(level, msg); } void enu_absolute_pos_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(enu_absolute_pos.header.stamp); + rclcpp::Time ros_clock(_enu_absolute_pos.header.stamp); auto enu_absolute_pos_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (!std::isfinite(enu_absolute_pos.enu_pos.x)||!std::isfinite(enu_absolute_pos.enu_pos.y)||!std::isfinite(enu_absolute_pos.enu_pos.z)) { + if (!std::isfinite(_enu_absolute_pos.enu_pos.x)||!std::isfinite(_enu_absolute_pos.enu_pos.y)||!std::isfinite(_enu_absolute_pos.enu_pos.z)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (enu_absolute_pos_time_last - enu_absolute_pos_time > th_gnss_deadrock_time) { + else if (_enu_absolute_pos_time_last - enu_absolute_pos_time > _th_gnss_deadrock_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed or deadlock of more than 10 seconds"; } - else if (!enu_absolute_pos.status.enabled_status) { + else if (!_enu_absolute_pos.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - enu_absolute_pos_time_last = enu_absolute_pos_time; + _enu_absolute_pos_time_last = enu_absolute_pos_time; stat.summary(level, msg); } void enu_absolute_pos_interpolate_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(enu_absolute_pos_interpolate.header.stamp); + rclcpp::Time ros_clock(_enu_absolute_pos_interpolate.header.stamp); auto enu_absolute_pos_interpolate_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (!std::isfinite(enu_absolute_pos_interpolate.enu_pos.x)||!std::isfinite(enu_absolute_pos_interpolate.enu_pos.y)||!std::isfinite(enu_absolute_pos_interpolate.enu_pos.z)) { + if (!std::isfinite(_enu_absolute_pos_interpolate.enu_pos.x)||!std::isfinite(_enu_absolute_pos_interpolate.enu_pos.y)||!std::isfinite(_enu_absolute_pos_interpolate.enu_pos.z)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - else if (enu_absolute_pos_interpolate_time_last == enu_absolute_pos_interpolate_time) { + else if (_enu_absolute_pos_interpolate_time_last == enu_absolute_pos_interpolate_time) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "not subscribed or deadlock of more than 10 seconds"; } - else if (!enu_absolute_pos_interpolate.status.enabled_status) { + else if (!_enu_absolute_pos_interpolate.status.enabled_status) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; msg = "estimates have not started yet"; } - enu_absolute_pos_interpolate_time_last = enu_absolute_pos_interpolate_time; + _enu_absolute_pos_interpolate_time_last = enu_absolute_pos_interpolate_time; stat.summary(level, msg); } void twist_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) { - rclcpp::Time ros_clock(eagleye_twist.header.stamp); + rclcpp::Time ros_clock(_eagleye_twist.header.stamp); auto eagleye_twist_time = ros_clock.seconds(); int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string msg = "OK"; - if (eagleye_twist_time_last == eagleye_twist_time) { + if (_eagleye_twist_time_last == eagleye_twist_time) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "not subscribed or deadlock of more than 10 seconds"; } - else if (!std::isfinite(eagleye_twist.twist.linear.x)||!std::isfinite(eagleye_twist.twist.linear.y)||!std::isfinite(eagleye_twist.twist.linear.z) - ||!std::isfinite(eagleye_twist.twist.angular.x)||!std::isfinite(eagleye_twist.twist.angular.y)||!std::isfinite(eagleye_twist.twist.angular.z)) { + else if (!std::isfinite(_eagleye_twist.twist.linear.x)||!std::isfinite(_eagleye_twist.twist.linear.y)||!std::isfinite(_eagleye_twist.twist.linear.z) + ||!std::isfinite(_eagleye_twist.twist.angular.x)||!std::isfinite(_eagleye_twist.twist.angular.y)||!std::isfinite(_eagleye_twist.twist.angular.z)) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; msg = "invalid number"; } - eagleye_twist_time_last = eagleye_twist_time; + _eagleye_twist_time_last = eagleye_twist_time; stat.summary(level, msg); } @@ -715,34 +736,34 @@ void printStatus(void) std::cout << std::fixed; std::cout << "--- \033[1;34m imu(input)\033[m ------------------------------"<< std::endl; - std::cout<<"\033[1m linear_acceleration \033[mx "<::max_digits10) << 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 << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _imu.linear_acceleration.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _imu.linear_acceleration.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _imu.linear_acceleration.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.tow << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_pos.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_vel.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_vel.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_vel.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << int(_rtklib_nav.status.status.status) << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.status.status.service << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.status.latitude << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.status.longitude << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.status.altitude << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.linear.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.linear.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.linear.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.angular.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.angular.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.angular.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity_scale_factor.scale_factor << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.linear.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.linear.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.linear.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.angular.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.angular.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.angular.z << ","; + output_log_file << (_velocity_scale_factor.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_velocity_scale_factor.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _distance.distance << ","; + output_log_file << (_distance.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_distance.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_1st.heading_angle << ","; + output_log_file << (_heading_1st.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_heading_1st.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_interpolate_1st.heading_angle << ","; + output_log_file << (_heading_interpolate_1st.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_heading_interpolate_1st.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_2nd.heading_angle << ","; + output_log_file << (_heading_2nd.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_heading_2nd.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_interpolate_2nd.heading_angle << ","; + output_log_file << (_heading_interpolate_2nd.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_heading_interpolate_2nd.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_3rd.heading_angle << ","; + output_log_file << (_heading_3rd.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_heading_3rd.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_interpolate_3rd.heading_angle << ","; + output_log_file << (_heading_interpolate_3rd.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_heading_interpolate_3rd.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _yawrate_offset_stop.yawrate_offset << ","; + output_log_file << (_yawrate_offset_stop.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_yawrate_offset_stop.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _yawrate_offset_1st.yawrate_offset << ","; + output_log_file << (_yawrate_offset_1st.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_yawrate_offset_1st.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _yawrate_offset_2nd.yawrate_offset << ","; + output_log_file << (_yawrate_offset_2nd.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_yawrate_offset_2nd.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _slip_angle.coefficient << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _slip_angle.slip_angle << ","; + output_log_file << (_slip_angle.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_slip_angle.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_vel.vector.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_vel.vector.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_vel.vector.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.enu_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.enu_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.enu_pos.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.ecef_base_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.ecef_base_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.ecef_base_pos.z << ","; + output_log_file << (_enu_absolute_pos.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_enu_absolute_pos.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.enu_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.enu_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.enu_pos.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.ecef_base_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.ecef_base_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.ecef_base_pos.z << ","; + output_log_file << (_enu_absolute_pos_interpolate.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_enu_absolute_pos_interpolate.status.estimate_status ? "1" : "0") << ","; + // output_log_file << std::setprecision(std::numeric_limits::max_digits10) << angular_velocity_offset_stop.rollrate_offset << ","; + // output_log_file << std::setprecision(std::numeric_limits::max_digits10) << angular_velocity_offset_stop.pitchrate_offset << ","; + // output_log_file << std::setprecision(std::numeric_limits::max_digits10) << angular_velocity_offset_stop.yawrate_offset << ","; + // output_log_file << (angular_velocity_offset_stop.status.enabled_status ? "1" : "0") << ","; + // output_log_file << (angular_velocity_offset_stop.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _height.height << ","; + output_log_file << (_height.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_height.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _pitching.pitching_angle << ","; + output_log_file << (_pitching.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_pitching.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; // acc_x_offset + output_log_file << 0 << ","; // acc_x_offset.status.enabled_status + output_log_file << 0 << ","; // acc_x_offset.status.estimate_status + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; // acc_x_scale_factor.acc_x_scale_factor + output_log_file << 0 << ","; // acc_x_scale_factor.status.enabled_status + output_log_file << 0 << ","; // acc_x_scale_factor.status.estimate_status + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rolling.rolling_angle << ","; + output_log_file << (_rolling.status.enabled_status ? "1" : "0") << ","; + output_log_file << (_rolling.status.estimate_status ? "1" : "0") << ","; + rclcpp::Time gga_clock(_gga.header.stamp); + double gga_time = gga_clock.seconds(); + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << gga_time << ","; //timestamp + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga.lat << ","; //gga_llh.latitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga.lon << ","; //gga_llh.longitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga.alt + _gga.undulation<< ","; //gga_llh.altitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << int(_gga.gps_qual) << ","; //gga_llh.gps_qual + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _eagleye_fix.latitude << ","; //eagleye_pp_llh.latitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _eagleye_fix.longitude << ","; //eagleye_pp_llh.longitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _eagleye_fix.altitude << ","; //eagleye_pp_llh.altitude + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[0] + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[1] + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[2] + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[3] + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[4] + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[5] + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[6] + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[7] + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[8] + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.status + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.status + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_relative_pos.enu_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_relative_pos.enu_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_relative_pos.enu_pos.z << ","; + output_log_file << (_enu_relative_pos.status.enabled_status ? "1" : "0"); + output_log_file << "\n"; + } + return; +} + void on_timer() { // Diagnostic Updater @@ -807,19 +1011,24 @@ void on_timer() void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - imu.header = msg->header; - imu.orientation = msg->orientation; - imu.orientation_covariance = msg->orientation_covariance; - imu.angular_velocity = msg->angular_velocity; - imu.angular_velocity_covariance = msg->angular_velocity_covariance; - imu.linear_acceleration = msg->linear_acceleration; - imu.linear_acceleration_covariance = msg->linear_acceleration_covariance; - - if (print_status) + _imu.header = msg->header; + _imu.orientation = msg->orientation; + _imu.orientation_covariance = msg->orientation_covariance; + _imu.angular_velocity = msg->angular_velocity; + _imu.angular_velocity_covariance = msg->angular_velocity_covariance; + _imu.linear_acceleration = msg->linear_acceleration; + _imu.linear_acceleration_covariance = msg->linear_acceleration_covariance; + + if (_print_status) { printStatus(); } + if(_log_output_status) + { + outputLog(); + } + } int main(int argc, char** argv) @@ -829,27 +1038,28 @@ int main(int argc, char** argv) updater_ = std::make_shared(node); std::string subscribe_twist_topic_name = "/can_twist"; - std::string subscribe_imu_topic_name = "/imu/data_raw"; + std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; std::string subscribe_gga_topic_name = "/navsat/gga"; node->declare_parameter("twist_topic",subscribe_twist_topic_name); - node->declare_parameter("imu_topic",subscribe_imu_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.print_status",print_status); + node->declare_parameter("monitor.print_status",_print_status); + node->declare_parameter("monitor.log_output_status",_log_output_status); node->get_parameter("twist_topic",subscribe_twist_topic_name); - node->get_parameter("imu_topic",subscribe_imu_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.print_status",print_status); + node->get_parameter("monitor.print_status",_print_status); + node->get_parameter("monitor.log_output_status",_log_output_status); std::cout<< "subscribe_twist_topic_name "<setHardwareID("eagleye_topic_checker"); @@ -876,33 +1086,43 @@ int main(int argc, char** argv) updater_->add("eagleye_enu_absolute_pos_interpolate", enu_absolute_pos_interpolate_topic_checker); updater_->add("eagleye_twist", twist_topic_checker); - auto sub1 = node->create_subscription(subscribe_imu_topic_name, 1000, imu_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); //ros::TransportHints().tcpNoDelay() - auto sub3 = node->create_subscription("fix", rclcpp::QoS(10), fix_callback); //ros::TransportHints().tcpNoDelay() - auto sub4 = node->create_subscription(subscribe_gga_topic_name, 1000, navsatfix_gga_callback); //ros::TransportHints().tcpNoDelay() - auto sub5 = node->create_subscription(subscribe_twist_topic_name, 1000, velocity_callback); //ros::TransportHints().tcpNoDelay() - auto sub6 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); //ros::TransportHints().tcpNoDelay() - auto sub7 = node->create_subscription("distance", rclcpp::QoS(10), distance_callback); //ros::TransportHints().tcpNoDelay() - auto sub8 = node->create_subscription("heading_1st", rclcpp::QoS(10), heading_1st_callback); //ros::TransportHints().tcpNoDelay() - auto sub9 = node->create_subscription("heading_interpolate_1st", rclcpp::QoS(10), heading_interpolate_1st_callback); //ros::TransportHints().tcpNoDelay() - auto sub10 = node->create_subscription("heading_2nd", rclcpp::QoS(10), heading_2nd_callback); //ros::TransportHints().tcpNoDelay() - auto sub11 = node->create_subscription("heading_interpolate_2nd", rclcpp::QoS(10), heading_interpolate_2nd_callback); //ros::TransportHints().tcpNoDelay() - auto sub12 = node->create_subscription("heading_3rd", rclcpp::QoS(10), heading_3rd_callback); //ros::TransportHints().tcpNoDelay() - auto sub13 = node->create_subscription("heading_interpolate_3rd", rclcpp::QoS(10), heading_interpolate_3rd_callback); //ros::TransportHints().tcpNoDelay() - auto sub14 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); //ros::TransportHints().tcpNoDelay() - auto sub15 = node->create_subscription("yawrate_offset_1st", rclcpp::QoS(10), yawrate_offset_1st_callback); //ros::TransportHints().tcpNoDelay() - auto sub16 = node->create_subscription("yawrate_offset_2nd", rclcpp::QoS(10), yawrate_offset_2nd_callback); //ros::TransportHints().tcpNoDelay() - auto sub17 = node->create_subscription("slip_angle", rclcpp::QoS(10), slip_angle_callback); //ros::TransportHints().tcpNoDelay() - auto sub18 = node->create_subscription("enu_relative_pos", rclcpp::QoS(10), enu_relative_pos_callback); //ros::TransportHints().tcpNoDelay() - auto sub19 = node->create_subscription("enu_vel", rclcpp::QoS(10), enu_vel_callback); //ros::TransportHints().tcpNoDelay() - auto sub20 = node->create_subscription("height", rclcpp::QoS(10), height_callback); //ros::TransportHints().tcpNoDelay() - auto sub21 = node->create_subscription("pitching", rclcpp::QoS(10), pitching_callback); //ros::TransportHints().tcpNoDelay() - auto sub22 = node->create_subscription("enu_absolute_pos", rclcpp::QoS(10), enu_absolute_pos_callback); //ros::TransportHints().tcpNoDelay() - auto sub23 = node->create_subscription("enu_absolute_pos_interpolate", rclcpp::QoS(10), enu_absolute_pos_interpolate_callback); //ros::TransportHints().tcpNoDelay() - auto sub24 = node->create_subscription("fix", rclcpp::QoS(10), eagleye_fix_callback); //ros::TransportHints().tcpNoDelay() - auto sub25 = node->create_subscription("twist", rclcpp::QoS(10), eagleye_twist_callback); //ros::TransportHints().tcpNoDelay() - - double delta_time = 1.0 / static_cast(update_rate); + time_t time_; + time_ = time(NULL); + std::stringstream time_ss; + time_ss << time_; + std::string time_str = time_ss.str(); + _output_log_dir = ament_index_cpp::get_package_share_directory("eagleye_rt") + "/log/eagleye_log_" + time_str + ".csv"; + if(_log_output_status) std::cout << _output_log_dir << std::endl; + + 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("rtklib/fix", rclcpp::QoS(10), rtklib_fix_callback); + auto sub4 = node->create_subscription(subscribe_gga_topic_name, 1000, navsatfix_gga_callback); + auto sub5 = node->create_subscription(subscribe_twist_topic_name, 1000, velocity_callback); + auto sub6 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); + auto sub7 = node->create_subscription("distance", rclcpp::QoS(10), distance_callback); + auto sub8 = node->create_subscription("heading_1st", rclcpp::QoS(10), heading_1st_callback); + auto sub9 = node->create_subscription("heading_interpolate_1st", rclcpp::QoS(10), heading_interpolate_1st_callback); + auto sub10 = node->create_subscription("heading_2nd", rclcpp::QoS(10), heading_2nd_callback); + auto sub11 = node->create_subscription("heading_interpolate_2nd", rclcpp::QoS(10), heading_interpolate_2nd_callback); + auto sub12 = node->create_subscription("heading_3rd", rclcpp::QoS(10), heading_3rd_callback); + auto sub13 = node->create_subscription("heading_interpolate_3rd", rclcpp::QoS(10), heading_interpolate_3rd_callback); + auto sub14 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); + auto sub15 = node->create_subscription("yawrate_offset_1st", rclcpp::QoS(10), yawrate_offset_1st_callback); + auto sub16 = node->create_subscription("yawrate_offset_2nd", rclcpp::QoS(10), yawrate_offset_2nd_callback); + auto sub17 = node->create_subscription("slip_angle", rclcpp::QoS(10), slip_angle_callback); + auto sub18 = node->create_subscription("enu_relative_pos", rclcpp::QoS(10), enu_relative_pos_callback); + auto sub19 = node->create_subscription("enu_vel", rclcpp::QoS(10), enu_vel_callback); + auto sub20 = node->create_subscription("height", rclcpp::QoS(10), height_callback); + auto sub21 = node->create_subscription("pitching", rclcpp::QoS(10), pitching_callback); + auto sub22 = node->create_subscription("enu_absolute_pos", rclcpp::QoS(10), enu_absolute_pos_callback); + auto sub23 = node->create_subscription("enu_absolute_pos_interpolate", rclcpp::QoS(10), enu_absolute_pos_interpolate_callback); + auto sub24 = node->create_subscription("fix", rclcpp::QoS(10), eagleye_fix_callback); + auto sub25 = node->create_subscription("twist", rclcpp::QoS(10), eagleye_twist_callback); + auto sub26 = node->create_subscription("rolling", rclcpp::QoS(10), rolling_callback); + + double delta_time = 1.0 / static_cast(_update_rate); + auto timer_callback = std::bind(on_timer); const auto period_ns = std::chrono::duration_cast(std::chrono::duration(delta_time)); diff --git a/eagleye_rt/src/navpvt2rtk_node.cpp b/eagleye_rt/src/navpvt2rtk_node.cpp new file mode 100644 index 00000000..cf988afa --- /dev/null +++ b/eagleye_rt/src/navpvt2rtk_node.cpp @@ -0,0 +1,97 @@ +// 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_node.cpp b/eagleye_rt/src/position_node.cpp index 5efdad9f..d5e1f4fe 100644 --- a/eagleye_rt/src/position_node.cpp +++ b/eagleye_rt/src/position_node.cpp @@ -37,6 +37,8 @@ #include static rtklib_msgs::msg::RtklibNav rtklib_nav; +static geometry_msgs::msg::TwistStamped velocity; +static eagleye_msgs::msg::StatusStamped velocity_status; static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; static eagleye_msgs::msg::Distance distance; static eagleye_msgs::msg::Heading heading_interpolate_3rd; @@ -49,6 +51,7 @@ struct PositionParameter position_parameter; struct PositionStatus position_status; std::string use_gnss_mode; +static bool use_canless_mode; rclcpp::Clock clock_(RCL_ROS_TIME); tf2_ros::Buffer tfBuffer_(std::make_shared(clock_)); @@ -58,6 +61,16 @@ void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) rtklib_nav = *msg; } +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + velocity = *msg; +} + +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) +{ + velocity_status = *msg; +} + void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) { velocity_scale_factor = *msg; @@ -103,13 +116,28 @@ void on_timer() void enu_vel_callback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr msg) { + if(use_canless_mode && !velocity_status.status.enabled_status) return; + + eagleye_msgs::msg::StatusStamped velocity_enable_status; + if(use_canless_mode) + { + velocity_enable_status = velocity_status; + } + else + { + velocity_enable_status.header = velocity_scale_factor.header; + velocity_enable_status.status = velocity_scale_factor.status; + } + enu_vel = *msg; enu_absolute_pos.header = msg->header; enu_absolute_pos.header.frame_id = "base_link"; if (use_gnss_mode == "rtklib" || use_gnss_mode == "RTKLIB") // use RTKLIB mode - position_estimate(rtklib_nav, velocity_scale_factor, distance, heading_interpolate_3rd, enu_vel, position_parameter, &position_status, &enu_absolute_pos); + position_estimate(rtklib_nav, velocity, velocity_enable_status, distance, heading_interpolate_3rd, enu_vel, + position_parameter, &position_status, &enu_absolute_pos); else if (use_gnss_mode == "nmea" || use_gnss_mode == "NMEA") // use NMEA mode - position_estimate(gga, velocity_scale_factor, distance, heading_interpolate_3rd, enu_vel, position_parameter, &position_status, &enu_absolute_pos); + position_estimate(gga, velocity, velocity_enable_status, distance, heading_interpolate_3rd, enu_vel, + position_parameter, &position_status, &enu_absolute_pos); if (enu_absolute_pos.status.estimate_status == true) { pub->publish(enu_absolute_pos); @@ -141,6 +169,7 @@ int main(int argc, char** argv) node->declare_parameter("tf_gnss_flame.parent", position_parameter.tf_gnss_parent_flame); node->declare_parameter("tf_gnss_flame.child", position_parameter.tf_gnss_child_flame); node->declare_parameter("use_gnss_mode",use_gnss_mode); + node->declare_parameter("use_canless_mode",use_canless_mode); node->get_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); node->get_parameter("gga_topic",subscribe_gga_topic_name); @@ -156,6 +185,7 @@ int main(int argc, char** argv) node->get_parameter("tf_gnss_flame.parent", position_parameter.tf_gnss_parent_flame); node->get_parameter("tf_gnss_flame.child", position_parameter.tf_gnss_child_flame); node->get_parameter("use_gnss_mode",use_gnss_mode); + node->get_parameter("use_canless_mode",use_canless_mode); std::cout<< "subscribe_rtklib_nav_topic_name "<create_subscription("enu_vel", 1000, enu_vel_callback); auto sub2 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); - auto sub3 = node->create_subscription("velocity_scale_factor", 1000, velocity_scale_factor_callback); - auto sub4 = node->create_subscription("distance", 1000, distance_callback); - auto sub5 = node->create_subscription("heading_interpolate_3rd", 1000, heading_interpolate_3rd_callback); - auto sub6 = node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); - + auto sub3 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); + auto sub4 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); + auto sub5 = node->create_subscription("velocity_scale_factor", 1000, velocity_scale_factor_callback); + auto sub6 = node->create_subscription("distance", 1000, distance_callback); + auto sub7 = node->create_subscription("heading_interpolate_3rd", 1000, heading_interpolate_3rd_callback); + auto sub8 = node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); + pub = node->create_publisher("enu_absolute_pos", 1000); const auto period_ns = diff --git a/eagleye_rt/src/rolling_node.cpp b/eagleye_rt/src/rolling_node.cpp index 3fb29b7a..9b1b696c 100644 --- a/eagleye_rt/src/rolling_node.cpp +++ b/eagleye_rt/src/rolling_node.cpp @@ -33,7 +33,8 @@ rclcpp::Publisher::SharedPtr _rolling_pub; -static eagleye_msgs::msg::VelocityScaleFactor _velocity_scale_factor_msg; +static geometry_msgs::msg::TwistStamped _velocity_msg; +static eagleye_msgs::msg::StatusStamped _velocity_status_msg; static eagleye_msgs::msg::YawrateOffset _yawrate_offset_2nd_msg; static eagleye_msgs::msg::YawrateOffset _yawrate_offset_stop_msg; static sensor_msgs::msg::Imu _imu_msg; @@ -43,11 +44,16 @@ static eagleye_msgs::msg::Rolling _rolling_msg; struct RollingParameter _rolling_parameter; struct RollingStatus _rolling_status; -static std::string _subscribe_imu_topic_name; +static bool _use_canless_mode; -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstPtr msg) +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg) { - _velocity_scale_factor_msg = *msg; + _velocity_msg = *msg; +} + +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +{ + _velocity_status_msg = *msg; } void yawrate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstPtr msg) @@ -62,28 +68,25 @@ void yawrate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstPt void imu_callback(const sensor_msgs::msg::Imu::ConstPtr msg) { + if(_use_canless_mode && !_velocity_status_msg.status.enabled_status) return; _imu_msg = *msg; - rolling_estimate(_imu_msg, _velocity_scale_factor_msg, _yawrate_offset_stop_msg, _yawrate_offset_2nd_msg, + rolling_estimate(_imu_msg, _velocity_msg, _yawrate_offset_stop_msg, _yawrate_offset_2nd_msg, _rolling_parameter, &_rolling_status, &_rolling_msg); _rolling_pub->publish(_rolling_msg); } void setParam(rclcpp::Node::SharedPtr node) { - node->declare_parameter("imu_topic",_subscribe_imu_topic_name); - node->declare_parameter("reverse_imu",_rolling_parameter.reverse_imu); node->declare_parameter("rolling.stop_judgment_velocity_threshold",_rolling_parameter.stop_judgment_velocity_threshold); node->declare_parameter("rolling.filter_process_noise",_rolling_parameter.filter_process_noise); node->declare_parameter("rolling.filter_observation_noise",_rolling_parameter.filter_observation_noise); + node->declare_parameter("use_canless_mode",_use_canless_mode); - node->get_parameter("imu_topic",_subscribe_imu_topic_name); - node->get_parameter("reverse_imu",_rolling_parameter.reverse_imu); node->get_parameter("rolling.stop_judgment_velocity_threshold",_rolling_parameter.stop_judgment_velocity_threshold); node->get_parameter("rolling.filter_process_noise",_rolling_parameter.filter_process_noise); node->get_parameter("rolling.filter_observation_noise",_rolling_parameter.filter_observation_noise); + node->get_parameter("use_canless_mode",_use_canless_mode); - std::cout << "subscribe_imu_topic_name " << _subscribe_imu_topic_name << std::endl; - std::cout << "reverse_imu " << _rolling_parameter.reverse_imu << std::endl; std::cout << "stop_judgment_velocity_threshold " << _rolling_parameter.stop_judgment_velocity_threshold << std::endl; std::cout << "filter_process_noise " << _rolling_parameter.filter_process_noise << std::endl; std::cout << "filter_observation_noise " << _rolling_parameter.filter_observation_noise << std::endl; @@ -94,9 +97,11 @@ void rolling_node(rclcpp::Node::SharedPtr node) setParam(node); auto imu_sub = - node->create_subscription(_subscribe_imu_topic_name, 1000, imu_callback); - auto velocity_scale_factor_sub = - node->create_subscription("velocity_scale_factor", 1000, velocity_scale_factor_callback); + node->create_subscription("imu/data_tf_converted", 1000, imu_callback); + auto velocity_sub = + node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); + auto velocity_status_sub = + node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); auto yawrate_offset_2nd_sub = node->create_subscription("yawrate_offset_2nd", 1000, yawrate_offset_2nd_callback); auto yawrate_offset_stop_sub = diff --git a/eagleye_rt/src/rtk_heading_node.cpp b/eagleye_rt/src/rtk_heading_node.cpp index dbce29d4..e342a6e7 100644 --- a/eagleye_rt/src/rtk_heading_node.cpp +++ b/eagleye_rt/src/rtk_heading_node.cpp @@ -34,7 +34,8 @@ static nmea_msgs::msg::Gpgga gga; static sensor_msgs::msg::Imu imu; -static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; +static geometry_msgs::msg::TwistStamped velocity; +static eagleye_msgs::msg::StatusStamped velocity_status; static eagleye_msgs::msg::Distance distance; static eagleye_msgs::msg::YawrateOffset yawrate_offset_stop; static eagleye_msgs::msg::YawrateOffset yawrate_offset; @@ -47,14 +48,21 @@ static eagleye_msgs::msg::Heading heading; struct RtkHeadingParameter heading_parameter; struct RtkHeadingStatus heading_status; +static bool use_canless_mode; + +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg) +{ + velocity = *msg; +} + void gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) { gga = *msg; } -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) { - velocity_scale_factor = *msg; + velocity_status = *msg; } void yawrate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) @@ -84,10 +92,12 @@ void distance_callback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { + if(use_canless_mode && !velocity_status.status.enabled_status) return; + imu = *msg; heading.header = msg->header; heading.header.frame_id = "base_link"; - rtk_heading_estimate(gga,imu,velocity_scale_factor,distance,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,&heading_status,&heading); + rtk_heading_estimate(gga,imu,velocity,distance,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,&heading_status,&heading); if (heading.status.estimate_status == true) { @@ -101,12 +111,11 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("rtk_heading"); - std::string subscribe_imu_topic_name = "/imu/data_raw"; + std::string subscribe_gga_topic_name = "/navsat/gga"; - node->declare_parameter("imu_topic",subscribe_imu_topic_name); + node->declare_parameter("gga_topic",subscribe_gga_topic_name); - node->declare_parameter("reverse_imu", heading_parameter.reverse_imu); node->declare_parameter("rtk_heading.estimated_distance",heading_parameter.estimated_distance); node->declare_parameter("rtk_heading.estimated_heading_buffer_min",heading_parameter.estimated_heading_buffer_min); node->declare_parameter("rtk_heading.estimated_number_min",heading_parameter.estimated_number_min); @@ -117,10 +126,9 @@ int main(int argc, char** argv) node->declare_parameter("rtk_heading.estimated_velocity_threshold",heading_parameter.estimated_velocity_threshold); node->declare_parameter("rtk_heading.stop_judgment_velocity_threshold",heading_parameter.stop_judgment_velocity_threshold); node->declare_parameter("rtk_heading.estimated_yawrate_threshold",heading_parameter.estimated_yawrate_threshold); + node->declare_parameter("use_canless_mode",use_canless_mode); - node->get_parameter("imu_topic",subscribe_imu_topic_name); node->get_parameter("gga_topic",subscribe_gga_topic_name); - node->get_parameter("reverse_imu", heading_parameter.reverse_imu); node->get_parameter("rtk_heading.estimated_distance",heading_parameter.estimated_distance); node->get_parameter("rtk_heading.estimated_heading_buffer_min",heading_parameter.estimated_heading_buffer_min); node->get_parameter("rtk_heading.estimated_number_min",heading_parameter.estimated_number_min); @@ -131,10 +139,9 @@ int main(int argc, char** argv) node->get_parameter("rtk_heading.estimated_velocity_threshold",heading_parameter.estimated_velocity_threshold); node->get_parameter("rtk_heading.stop_judgment_velocity_threshold",heading_parameter.stop_judgment_velocity_threshold); node->get_parameter("rtk_heading.estimated_yawrate_threshold",heading_parameter.estimated_yawrate_threshold); + node->get_parameter("use_canless_moded",use_canless_mode); - std::cout<< "subscribe_imu_topic_name "<create_subscription(subscribe_imu_topic_name, 1000, imu_callback); + auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); auto sub2 = node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); - auto sub3 = node->create_subscription("velocity_scale_factor", 1000, velocity_scale_factor_callback); - auto sub4 = node->create_subscription("yawrate_offset_stop", 1000, yawrate_offset_stop_callback); - auto sub5 = node->create_subscription(subscribe_topic_name, 1000, yawrate_offset_callback); - auto sub6 = node->create_subscription("slip_angle", 1000, slip_angle_callback); - auto sub7 = node->create_subscription(subscribe_topic_name2, 1000, heading_interpolate_callback); - auto sub8 = node->create_subscription("distance", 1000, distance_callback); + auto sub3 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); + auto sub4 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); + auto sub5 = node->create_subscription("yawrate_offset_stop", 1000, yawrate_offset_stop_callback); + auto sub6 = node->create_subscription(subscribe_topic_name, 1000, yawrate_offset_callback); + auto sub7 = node->create_subscription("slip_angle", 1000, slip_angle_callback); + auto sub8 = node->create_subscription(subscribe_topic_name2, 1000, heading_interpolate_callback); + auto sub9 = node->create_subscription("distance", 1000, distance_callback); pub = node->create_publisher(publish_topic_name, 1000); diff --git a/eagleye_rt/src/slip_angle_node.cpp b/eagleye_rt/src/slip_angle_node.cpp index 027eefa5..db9639e9 100644 --- a/eagleye_rt/src/slip_angle_node.cpp +++ b/eagleye_rt/src/slip_angle_node.cpp @@ -33,6 +33,8 @@ #include "eagleye_navigation/eagleye_navigation.hpp" static sensor_msgs::msg::Imu imu; +static geometry_msgs::msg::TwistStamped velocity; +static eagleye_msgs::msg::StatusStamped velocity_status; static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; static eagleye_msgs::msg::YawrateOffset yawrate_offset_stop; static eagleye_msgs::msg::YawrateOffset yawrate_offset_2nd; @@ -42,6 +44,17 @@ static eagleye_msgs::msg::SlipAngle slip_angle; struct SlipangleParameter slip_angle_parameter; +static bool use_canless_mode; + +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg) +{ + velocity = *msg; +} + +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +{ + velocity_status = *msg; +} void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) { @@ -60,10 +73,23 @@ void yawrate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstSh void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { + if(use_canless_mode && !velocity_status.status.enabled_status) return; + + eagleye_msgs::msg::StatusStamped velocity_enable_status; + if(use_canless_mode) + { + velocity_enable_status = velocity_status; + } + else + { + velocity_enable_status.header = velocity_scale_factor.header; + velocity_enable_status.status = velocity_scale_factor.status; + } + imu = *msg; slip_angle.header = msg->header; slip_angle.header.frame_id = "base_link"; - slip_angle_estimate(imu,velocity_scale_factor,yawrate_offset_stop,yawrate_offset_2nd,slip_angle_parameter,&slip_angle); + slip_angle_estimate(imu,velocity,velocity_enable_status,yawrate_offset_stop,yawrate_offset_2nd,slip_angle_parameter,&slip_angle); pub->publish(slip_angle); slip_angle.status.estimate_status = false; } @@ -73,23 +99,19 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("slip_angle"); - std::string subscribe_imu_topic_name = "/imu/data_raw"; - node->declare_parameter("imu_topic",subscribe_imu_topic_name); - node->declare_parameter("reverse_imu", slip_angle_parameter.reverse_imu); + + node->declare_parameter("slip_angle.manual_coefficient", slip_angle_parameter.manual_coefficient); node->declare_parameter("slip_angle.stop_judgment_velocity_threshold", slip_angle_parameter.stop_judgment_velocity_threshold); - node->get_parameter("imu_topic",subscribe_imu_topic_name); - node->get_parameter("reverse_imu", slip_angle_parameter.reverse_imu); node->get_parameter("slip_angle.manual_coefficient", slip_angle_parameter.manual_coefficient); node->get_parameter("slip_angle.stop_judgment_velocity_threshold", slip_angle_parameter.stop_judgment_velocity_threshold); - std::cout<< "subscribe_imu_topic_name "<create_subscription(subscribe_imu_topic_name, rclcpp::QoS(10), imu_callback); //ros::TransportHints().tcpNoDelay() + auto sub1 = node->create_subscription("imu/data_tf_converted", rclcpp::QoS(10), imu_callback); //ros::TransportHints().tcpNoDelay() auto sub2 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); //ros::TransportHints().tcpNoDelay() auto sub3 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); //ros::TransportHints().tcpNoDelay() auto sub4 = node->create_subscription("yawrate_offset_2nd", rclcpp::QoS(10), yawrate_offset_2nd_callback); //ros::TransportHints().tcpNoDelay() diff --git a/eagleye_rt/src/slip_coefficient_node.cpp b/eagleye_rt/src/slip_coefficient_node.cpp index 2817c478..f77dbc22 100644 --- a/eagleye_rt/src/slip_coefficient_node.cpp +++ b/eagleye_rt/src/slip_coefficient_node.cpp @@ -38,7 +38,8 @@ static rtklib_msgs::msg::RtklibNav rtklib_nav; static sensor_msgs::msg::Imu imu; -static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; +static geometry_msgs::msg::TwistStamped velocity; +static eagleye_msgs::msg::StatusStamped velocity_status; static eagleye_msgs::msg::YawrateOffset yawrate_offset_stop; static eagleye_msgs::msg::YawrateOffset yawrate_offset_2nd; static eagleye_msgs::msg::Heading heading_interpolate_3rd; @@ -49,21 +50,27 @@ struct SlipCoefficientStatus slip_coefficient_status; static double estimate_coefficient; bool is_first_correction_velocity = false; +static bool use_canless_mode; void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { rtklib_nav = *msg; } -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - velocity_scale_factor = *msg; - if (is_first_correction_velocity == false && msg->correction_velocity.linear.x > slip_coefficient_parameter.estimated_velocity_threshold) + velocity = *msg; + if (is_first_correction_velocity == false && msg->twist.linear.x > slip_coefficient_parameter.estimated_velocity_threshold) { is_first_correction_velocity = true; } } +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +{ + velocity_status = *msg; +} + void yawrate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { yawrate_offset_stop = *msg; @@ -81,12 +88,11 @@ void heading_interpolate_3rd_callback(const eagleye_msgs::msg::Heading::ConstSha void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - if (is_first_correction_velocity == false) - { - return; - } + if (is_first_correction_velocity == false) return; + if (use_canless_mode && !velocity_status.status.enabled_status) return; + imu = *msg; - slip_coefficient_estimate(imu,rtklib_nav,velocity_scale_factor,yawrate_offset_stop,yawrate_offset_2nd,heading_interpolate_3rd,slip_coefficient_parameter,&slip_coefficient_status,&estimate_coefficient); + slip_coefficient_estimate(imu,rtklib_nav,velocity,yawrate_offset_stop,yawrate_offset_2nd,heading_interpolate_3rd,slip_coefficient_parameter,&slip_coefficient_status,&estimate_coefficient); std::cout << "--- \033[1;34m slip_coefficient \033[m ------------------------------"<< std::endl; std::cout<<"\033[1m estimate_coefficient \033[m "<declare_parameter("imu_topic",subscribe_imu_topic_name); + node->declare_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - node->declare_parameter("reverse_imu", slip_coefficient_parameter.reverse_imu); node->declare_parameter("slip_coefficient.estimated_number_min", slip_coefficient_parameter.estimated_number_min); node->declare_parameter("slip_coefficient.estimated_number_max", slip_coefficient_parameter.estimated_number_max); node->declare_parameter("slip_coefficient.estimated_velocity_threshold", slip_coefficient_parameter.estimated_velocity_threshold); node->declare_parameter("slip_coefficient.estimated_yawrate_threshold", slip_coefficient_parameter.estimated_yawrate_threshold); node->declare_parameter("slip_coefficient.lever_arm", slip_coefficient_parameter.lever_arm); node->declare_parameter("slip_coefficient.stop_judgment_velocity_threshold", slip_coefficient_parameter.stop_judgment_velocity_threshold); + node->declare_parameter("use_canless_mode", use_canless_mode); - node->get_parameter("imu_topic",subscribe_imu_topic_name); node->get_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - node->get_parameter("reverse_imu", slip_coefficient_parameter.reverse_imu); node->get_parameter("slip_coefficient.estimated_number_min", slip_coefficient_parameter.estimated_number_min); node->get_parameter("slip_coefficient.estimated_number_max", slip_coefficient_parameter.estimated_number_max); node->get_parameter("slip_coefficient.estimated_velocity_threshold", slip_coefficient_parameter.estimated_velocity_threshold); node->get_parameter("slip_coefficient.estimated_yawrate_threshold", slip_coefficient_parameter.estimated_yawrate_threshold); node->get_parameter("slip_coefficient.lever_arm", slip_coefficient_parameter.lever_arm); node->get_parameter("slip_coefficient.stop_judgment_velocity_threshold", slip_coefficient_parameter.stop_judgment_velocity_threshold); + node->get_parameter("use_canless_mode", use_canless_mode); - std::cout<< "subscribe_imu_topic_name "<create_subscription(subscribe_imu_topic_name, 1000, imu_callback); + 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("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); - auto sub4 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); - auto sub5 = node->create_subscription("yawrate_offset_2nd", rclcpp::QoS(10), yawrate_offset_2nd_callback); - auto sub6 = node->create_subscription("heading_interpolate_3rd", rclcpp::QoS(10), heading_interpolate_3rd_callback); + auto sub3 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); + auto sub4 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); + auto sub5 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); + auto sub6 = node->create_subscription("yawrate_offset_2nd", rclcpp::QoS(10), yawrate_offset_2nd_callback); + auto sub7 = node->create_subscription("heading_interpolate_3rd", rclcpp::QoS(10), heading_interpolate_3rd_callback); rclcpp::spin(node); diff --git a/eagleye_rt/src/smoothing_node.cpp b/eagleye_rt/src/smoothing_node.cpp index 5ea098d5..cdf6fd4f 100644 --- a/eagleye_rt/src/smoothing_node.cpp +++ b/eagleye_rt/src/smoothing_node.cpp @@ -34,23 +34,33 @@ static rtklib_msgs::msg::RtklibNav rtklib_nav; static eagleye_msgs::msg::Position enu_absolute_pos,gnss_smooth_pos_enu; -static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; +static geometry_msgs::msg::TwistStamped velocity; +static eagleye_msgs::msg::StatusStamped velocity_status; rclcpp::Publisher::SharedPtr pub; struct SmoothingParameter smoothing_parameter; struct SmoothingStatus smoothing_status; -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) +static bool use_canless_mode; + +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - velocity_scale_factor = *msg; + velocity = *msg; +} + +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +{ + velocity_status = *msg; } void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { + if(use_canless_mode && !velocity_status.status.enabled_status) return; + rtklib_nav = *msg; gnss_smooth_pos_enu.header = msg->header; gnss_smooth_pos_enu.header.frame_id = "base_link"; - smoothing_estimate(rtklib_nav,velocity_scale_factor,smoothing_parameter,&smoothing_status,&gnss_smooth_pos_enu); + smoothing_estimate(rtklib_nav,velocity,smoothing_parameter,&smoothing_status,&gnss_smooth_pos_enu); pub->publish(gnss_smooth_pos_enu); } @@ -68,6 +78,7 @@ int main(int argc, char** argv) node->declare_parameter("smoothing.estimated_number_max",smoothing_parameter.estimated_number_max); node->declare_parameter("smoothing.estimated_velocity_threshold",smoothing_parameter.estimated_velocity_threshold); node->declare_parameter("smoothing.estimated_threshold",smoothing_parameter.estimated_threshold); + node->declare_parameter("use_canless_mode",use_canless_mode); node->get_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); node->get_parameter("ecef_base_pos_x",smoothing_parameter.ecef_base_pos_x); @@ -76,6 +87,7 @@ int main(int argc, char** argv) node->get_parameter("smoothing.estimated_number_max",smoothing_parameter.estimated_number_max); node->get_parameter("smoothing.estimated_velocity_threshold",smoothing_parameter.estimated_velocity_threshold); node->get_parameter("smoothing.estimated_threshold",smoothing_parameter.estimated_threshold); + node->get_parameter("use_canless_mode",use_canless_mode); std::cout<< "subscribe_rtklib_nav_topic_name "<create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); //ros::TransportHints().tcpNoDelay() + auto sub1 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); + auto sub2 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); + auto sub3 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); pub = node->create_publisher("gnss_smooth_pos_enu", rclcpp::QoS(10)); rclcpp::spin(node); diff --git a/eagleye_rt/src/tf_converted_imu.cpp b/eagleye_rt/src/tf_converted_imu.cpp new file mode 100644 index 00000000..dae6d66e --- /dev/null +++ b/eagleye_rt/src/tf_converted_imu.cpp @@ -0,0 +1,134 @@ +// Copyright (c) 2022, Map IV, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of the Map IV, Inc. nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +/* + * correction_imu.cpp + * Author MapIV Sasaki + */ + +#include "rclcpp/rclcpp.hpp" +#include "eagleye_coordinate/eagleye_coordinate.hpp" +#include "eagleye_navigation/eagleye_navigation.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +class TFConvertedIMU: public rclcpp::Node +{ +public: + TFConvertedIMU(); + ~TFConvertedIMU(); + +private: + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_; + sensor_msgs::msg::Imu imu_; + sensor_msgs::msg::Imu tf_converted_imu_; + + rclcpp::Logger logger_; + rclcpp::Clock clock_; + tf2_ros::Buffer tfbuffer_; + tf2_ros::TransformListener tflistener_; + + std::string tf_base_link_frame_; + + void imu_callback(const sensor_msgs::msg::Imu::ConstPtr msg); + +}; + +TFConvertedIMU::TFConvertedIMU() : Node("tf_converted_imu"), + clock_(RCL_ROS_TIME), + tfbuffer_(std::make_shared(clock_)), + tflistener_(tfbuffer_), + logger_(get_logger()) +{ + std::string subscribe_imu_topic_name = "/imu/data_raw"; + std::string publish_imu_topic_name = "imu/data_tf_converted"; + + declare_parameter("imu_topic", subscribe_imu_topic_name); + declare_parameter("publish_imu_topic", publish_imu_topic_name); + declare_parameter("tf_gnss_flame.parent", tf_base_link_frame_); + + get_parameter("imu_topic", subscribe_imu_topic_name); + get_parameter("publish_imu_topic", publish_imu_topic_name); + get_parameter("tf_gnss_flame.parent", tf_base_link_frame_); + + 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; + + 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)); +}; + +TFConvertedIMU::~TFConvertedIMU(){}; + +void TFConvertedIMU::imu_callback(const sensor_msgs::msg::Imu::ConstPtr msg) +{ + imu_ = *msg; + tf_converted_imu_.header = imu_.header; + + try { + const geometry_msgs::msg::TransformStamped transform = tfbuffer_.lookupTransform( + tf_base_link_frame_, msg->header.frame_id, tf2::TimePointZero); + + geometry_msgs::msg::Vector3Stamped angular_velocity, linear_acceleration, transformed_angular_velocity, transformed_linear_acceleration; + geometry_msgs::msg::Quaternion transformed_quaternion; + + angular_velocity.header = imu_.header; + angular_velocity.vector = imu_.angular_velocity; + linear_acceleration.header = imu_.header; + linear_acceleration.vector = imu_.linear_acceleration; + + tf2::doTransform(angular_velocity, transformed_angular_velocity, transform); + tf2::doTransform(linear_acceleration, transformed_linear_acceleration, transform); + + tf_converted_imu_.angular_velocity = transformed_angular_velocity.vector; + tf_converted_imu_.linear_acceleration = transformed_linear_acceleration.vector; + tf_converted_imu_.orientation = transformed_quaternion; + + } + catch (tf2::TransformException& ex) + { + std::cout << "Failed to lookup transform" << std::endl; + return; + } + pub_->publish(tf_converted_imu_); +}; + +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/trajectory_node.cpp b/eagleye_rt/src/trajectory_node.cpp index 7d582b6e..f76807e6 100644 --- a/eagleye_rt/src/trajectory_node.cpp +++ b/eagleye_rt/src/trajectory_node.cpp @@ -33,13 +33,15 @@ #include "eagleye_navigation/eagleye_navigation.hpp" static sensor_msgs::msg::Imu imu; +static geometry_msgs::msg::TwistStamped velocity; +static eagleye_msgs::msg::StatusStamped velocity_status; +static geometry_msgs::msg::TwistStamped correction_velocity; static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; static eagleye_msgs::msg::Heading heading_interpolate_3rd; static eagleye_msgs::msg::YawrateOffset yawrate_offset_stop; static eagleye_msgs::msg::YawrateOffset yawrate_offset_2nd; 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; @@ -51,6 +53,24 @@ rclcpp::Publisher::SharedPtr pub struct TrajectoryParameter trajectory_parameter; struct TrajectoryStatus trajectory_status; +static double timer_updata_rate = 10; +static double th_deadlock_time = 1; + +static double imu_time_last,velocity_time_last; +static bool input_status; + +static bool use_canless_mode; + +void correction_velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + correction_velocity = *msg; +} + +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +{ + velocity_status = *msg; +} + void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) { velocity_scale_factor = *msg; @@ -76,38 +96,82 @@ void pitching_callback(const eagleye_msgs::msg::Pitching::ConstSharedPtr msg) pitching = *msg; } +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + velocity = *msg; +} + +void on_timer() +{ + rclcpp::Time imu_clock(imu.header.stamp); + double imu_time = imu_clock.seconds(); + rclcpp::Time velocity_clock(velocity.header.stamp); + double velocity_time = velocity_clock.seconds(); + if (std::abs(imu_time - imu_time_last) < th_deadlock_time && + std::abs(velocity_time - velocity_time_last) < th_deadlock_time && + std::abs(velocity_time - imu_time) < th_deadlock_time) + { + input_status = true; + } + else + { + input_status = false; + RCLCPP_WARN(rclcpp::get_logger("trajectory"), "Twist is missing the required input topics."); + } + + if (imu_time != imu_time_last) imu_time_last = imu_time; + if (velocity_time != velocity_time_last) velocity_time_last = velocity_time; +} + void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { + if(use_canless_mode && !velocity_status.status.enabled_status) return; + + eagleye_msgs::msg::StatusStamped velocity_enable_status; + if(use_canless_mode) + { + velocity_enable_status = velocity_status; + } + else + { + velocity_enable_status.header = velocity_scale_factor.header; + velocity_enable_status.status = velocity_scale_factor.status; + } + imu = *msg; - enu_vel.header = msg->header; - enu_vel.header.frame_id = "gnss"; - enu_relative_pos.header = msg->header; - enu_relative_pos.header.frame_id = "base_link"; - eagleye_twist.header = msg->header; - eagleye_twist.header.frame_id = "base_link"; - trajectory3d_estimate(imu,velocity_scale_factor,heading_interpolate_3rd,yawrate_offset_stop,yawrate_offset_2nd,pitching,trajectory_parameter,&trajectory_status,&enu_vel,&enu_relative_pos,&eagleye_twist); - - if (heading_interpolate_3rd.status.enabled_status == true) + if(input_status) { - pub1->publish(enu_vel); - pub2->publish(enu_relative_pos); + enu_vel.header = msg->header; + enu_vel.header.frame_id = "gnss"; + enu_relative_pos.header = msg->header; + enu_relative_pos.header.frame_id = "base_link"; + eagleye_twist.header = msg->header; + eagleye_twist.header.frame_id = "base_link"; + trajectory3d_estimate(imu,correction_velocity,velocity_enable_status,heading_interpolate_3rd,yawrate_offset_stop,yawrate_offset_2nd,pitching, + trajectory_parameter,&trajectory_status,&enu_vel,&enu_relative_pos,&eagleye_twist); + + if (heading_interpolate_3rd.status.enabled_status) + { + pub1->publish(enu_vel); + 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); } - 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); } int main(int argc, char** argv) @@ -115,33 +179,46 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("trajectory"); - std::string subscribe_imu_topic_name = "/imu/data_raw"; - - node->declare_parameter("imu_topic",subscribe_imu_topic_name); - node->declare_parameter("reverse_imu", trajectory_parameter.reverse_imu); node->declare_parameter("trajectory.stop_judgment_velocity_threshold",trajectory_parameter.stop_judgment_velocity_threshold); node->declare_parameter("trajectory.stop_judgment_yawrate_threshold",trajectory_parameter.stop_judgment_yawrate_threshold); + node->declare_parameter("timer_updata_rate",timer_updata_rate); + node->declare_parameter("th_deadlock_time",th_deadlock_time); + node->declare_parameter("use_canless_mode",use_canless_mode); - node->get_parameter("imu_topic",subscribe_imu_topic_name); - node->get_parameter("reverse_imu", trajectory_parameter.reverse_imu); node->get_parameter("trajectory.stop_judgment_velocity_threshold",trajectory_parameter.stop_judgment_velocity_threshold); node->get_parameter("trajectory.stop_judgment_yawrate_threshold",trajectory_parameter.stop_judgment_yawrate_threshold); - std::cout<< "subscribe_imu_topic_name "<get_parameter("timer_updata_rate",timer_updata_rate); + node->get_parameter("th_deadlock_time",th_deadlock_time); + node->get_parameter("use_canless_mode",use_canless_mode); + std::cout<< "stop_judgment_velocity_threshold "<create_subscription(subscribe_imu_topic_name, 1000, imu_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); //ros::TransportHints().tcpNoDelay() - auto sub3 = node->create_subscription("heading_interpolate_3rd", rclcpp::QoS(10), heading_interpolate_3rd_callback); //ros::TransportHints().tcpNoDelay() - auto sub4 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); //ros::TransportHints().tcpNoDelay() - auto sub5 = node->create_subscription("yawrate_offset_2nd", rclcpp::QoS(10), yawrate_offset_2nd_callback); //ros::TransportHints().tcpNoDelay() - auto sub6 = node->create_subscription("pitching", rclcpp::QoS(10), pitching_callback); //ros::TransportHints().tcpNoDelay() + std::cout<< "timer_updata_rate "<create_subscription("imu/data_tf_converted", 1000, imu_callback); //ros::TransportHints().tcpNoDelay() + auto sub2 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); + auto sub3 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); + auto sub4 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); //ros::TransportHints().tcpNoDelay() + auto sub5 = node->create_subscription("heading_interpolate_3rd", rclcpp::QoS(10), heading_interpolate_3rd_callback); //ros::TransportHints().tcpNoDelay() + auto sub6 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); //ros::TransportHints().tcpNoDelay() + auto sub7 = node->create_subscription("yawrate_offset_2nd", rclcpp::QoS(10), yawrate_offset_2nd_callback); //ros::TransportHints().tcpNoDelay() + auto sub8 = node->create_subscription("pitching", rclcpp::QoS(10), pitching_callback); //ros::TransportHints().tcpNoDelay() pub1 = node->create_publisher("enu_vel", 1000); pub2 = node->create_publisher("enu_relative_pos", 1000); pub3 = node->create_publisher("twist", 1000); pub4 = node->create_publisher("twist_with_covariance", 1000); + double delta_time = 1.0 / static_cast(timer_updata_rate); + auto timer_callback = std::bind(on_timer); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(delta_time)); + auto timer = std::make_shared>( + node->get_clock(), period_ns, std::move(timer_callback), + node->get_node_base_interface()->get_context()); + node->get_node_timers_interface()->add_timer(timer, nullptr); + rclcpp::spin(node); return 0; diff --git a/eagleye_rt/src/velocity_estimator_node.cpp b/eagleye_rt/src/velocity_estimator_node.cpp new file mode 100644 index 00000000..76fd0208 --- /dev/null +++ b/eagleye_rt/src/velocity_estimator_node.cpp @@ -0,0 +1,126 @@ +// 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. + +/* + * velocity_estimator_node.cpp + * Author MapIV Takanose + */ + +#include "rclcpp/rclcpp.hpp" +#include + +#include "eagleye_coordinate/eagleye_coordinate.hpp" +#include "eagleye_navigation/eagleye_navigation.hpp" + +rclcpp::Publisher::SharedPtr velocity_pub; +rclcpp::Publisher::SharedPtr velocity_status_pub; + +static rtklib_msgs::msg::RtklibNav rtklib_nav_msg; +static nmea_msgs::msg::Gpgga gga_msg; +static sensor_msgs::msg::Imu imu_msg; + +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 "<header; + velocity_status.status = velocity_estimator.getStatus(); + velocity_status_pub->publish(velocity_status); + + if(velocity_status.status.enabled_status) + { + velocity_pub->publish(velocity_msg); + } + +} + +void velocity_estimator_node(rclcpp::Node::SharedPtr node) +{ + node->declare_parameter("yaml_file",yaml_file); + node->get_parameter("yaml_file",yaml_file); + + setParam(yaml_file); + velocity_estimator.setParam(yaml_file); + + auto rtklib_sub = + node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); + auto gga_sub = + node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); + auto imu_sub = + node->create_subscription("imu/data_tf_converted", 1000, imu_callback); + + velocity_pub = node->create_publisher("velocity", 1000); + velocity_status_pub = node->create_publisher("velocity_status", 1000); + + rclcpp::spin(node); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("velocity_estimator"); + + velocity_estimator_node(node); + + return 0; +} \ No newline at end of file diff --git a/eagleye_rt/src/velocity_scale_factor_node.cpp b/eagleye_rt/src/velocity_scale_factor_node.cpp index 36345cc2..54bece5d 100644 --- a/eagleye_rt/src/velocity_scale_factor_node.cpp +++ b/eagleye_rt/src/velocity_scale_factor_node.cpp @@ -36,9 +36,10 @@ static rtklib_msgs::msg::RtklibNav rtklib_nav; static nmea_msgs::msg::Gprmc nmea_rmc; static geometry_msgs::msg::TwistStamped velocity; static sensor_msgs::msg::Imu imu; +static geometry_msgs::msg::TwistStamped correction_velocity; - -rclcpp::Publisher::SharedPtr pub; +rclcpp::Publisher::SharedPtr pub1; +rclcpp::Publisher::SharedPtr pub2; static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; struct VelocityScaleFactorParameter velocity_scale_factor_parameter; @@ -80,23 +81,26 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) velocity_scale_factor.header = msg->header; velocity_scale_factor.header.frame_id = "base_link"; + correction_velocity.header = msg->header; + correction_velocity.header.frame_id = "base_link"; + if (is_first_move == false) { velocity_scale_factor.scale_factor = initial_velocity_scale_factor; - velocity_scale_factor.correction_velocity = velocity.twist; - pub->publish(velocity_scale_factor); + pub2->publish(velocity_scale_factor); return; } if (use_gnss_mode == "rtklib" || use_gnss_mode == "RTKLIB") // use RTKLIB mode { - velocity_scale_factor_estimate(rtklib_nav,velocity,velocity_scale_factor_parameter,&velocity_scale_factor_status,&velocity_scale_factor); + velocity_scale_factor_estimate(rtklib_nav,velocity,velocity_scale_factor_parameter,&velocity_scale_factor_status,&correction_velocity,&velocity_scale_factor); } else if (use_gnss_mode == "nmea" || use_gnss_mode == "NMEA") // use NMEA mode { - velocity_scale_factor_estimate(nmea_rmc,velocity,velocity_scale_factor_parameter,&velocity_scale_factor_status,&velocity_scale_factor); + velocity_scale_factor_estimate(nmea_rmc,velocity,velocity_scale_factor_parameter,&velocity_scale_factor_status,&correction_velocity,&velocity_scale_factor); } - pub->publish(velocity_scale_factor); + pub1->publish(correction_velocity); + pub2->publish(velocity_scale_factor); } void load_velocity_scale_factor(std::string txt_path) @@ -165,12 +169,12 @@ int main(int argc, char** argv) double velocity_scale_factor_save_duration = 100.0; std::string subscribe_twist_topic_name = "/can_twist"; - std::string subscribe_imu_topic_name = "/imu/data_raw"; + std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; std::string subscribe_rmc_topic_name = "/navsat/rmc"; node->declare_parameter("twist_topic",subscribe_twist_topic_name); - node->declare_parameter("imu_topic",subscribe_imu_topic_name); + node->declare_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); node->declare_parameter("velocity_scale_factor.estimated_number_min",velocity_scale_factor_parameter.estimated_number_min); node->declare_parameter("velocity_scale_factor.estimated_number_max",velocity_scale_factor_parameter.estimated_number_max); @@ -182,7 +186,6 @@ int main(int argc, char** argv) node->declare_parameter("use_gnss_mode",use_gnss_mode); node->get_parameter("twist_topic",subscribe_twist_topic_name); - node->get_parameter("imu_topic",subscribe_imu_topic_name); node->get_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); node->get_parameter("velocity_scale_factor.estimated_number_min",velocity_scale_factor_parameter.estimated_number_min); node->get_parameter("velocity_scale_factor.estimated_number_max",velocity_scale_factor_parameter.estimated_number_max); @@ -194,7 +197,6 @@ int main(int argc, char** argv) node->get_parameter("use_gnss_mode",use_gnss_mode); std::cout<< "subscribe_twist_topic_name "<create_subscription(subscribe_imu_topic_name, 1000, imu_callback); + auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); auto sub2 = node->create_subscription(subscribe_twist_topic_name, 1000, velocity_callback); auto sub3 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); auto sub4 = node->create_subscription(subscribe_rmc_topic_name, 1000, rmc_callback); - pub = node->create_publisher("velocity_scale_factor", rclcpp::QoS(10)); + pub1 = node->create_publisher("velocity", rclcpp::QoS(10)); + pub2 = node->create_publisher("velocity_scale_factor", rclcpp::QoS(10)); double delta_time = static_cast(velocity_scale_factor_save_duration); auto timer_callback = std::bind(on_timer); diff --git a/eagleye_rt/src/yawrate_offset_node.cpp b/eagleye_rt/src/yawrate_offset_node.cpp index 243de511..aa64ce89 100644 --- a/eagleye_rt/src/yawrate_offset_node.cpp +++ b/eagleye_rt/src/yawrate_offset_node.cpp @@ -32,7 +32,8 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; +static geometry_msgs::msg::TwistStamped velocity; +static eagleye_msgs::msg::StatusStamped velocity_status; static eagleye_msgs::msg::YawrateOffset yawrate_offset_stop; static eagleye_msgs::msg::Heading heading_interpolate; static sensor_msgs::msg::Imu imu; @@ -42,11 +43,17 @@ static eagleye_msgs::msg::YawrateOffset yawrate_offset; struct YawrateOffsetParameter yawrate_offset_parameter; struct YawrateOffsetStatus yawrate_offset_status; -bool is_first_heading= false; +bool is_first_heading = false; +static bool use_canless_mode = false; -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) +void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - velocity_scale_factor = *msg; + velocity = *msg; +} + +void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstPtr msg) +{ + velocity_status = *msg; } void yawrate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) @@ -65,13 +72,12 @@ void heading_interpolate_callback(const eagleye_msgs::msg::Heading::ConstSharedP void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - if (is_first_heading == false) - { - return; - } + if (is_first_heading == false) return; + if(use_canless_mode && !velocity_status.status.enabled_status) return; + imu = *msg; yawrate_offset.header = msg->header; - yawrate_offset_estimate(velocity_scale_factor,yawrate_offset_stop,heading_interpolate,imu, yawrate_offset_parameter, &yawrate_offset_status, &yawrate_offset); + yawrate_offset_estimate(velocity,yawrate_offset_stop,heading_interpolate,imu, yawrate_offset_parameter, &yawrate_offset_status, &yawrate_offset); pub->publish(yawrate_offset); yawrate_offset.status.estimate_status = false; } @@ -81,28 +87,26 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("yawrate_offset"); - std::string subscribe_imu_topic_name = "/imu/data_raw"; - node->declare_parameter("imu_topic",subscribe_imu_topic_name); - node->declare_parameter("reverse_imu", yawrate_offset_parameter.reverse_imu); + + node->declare_parameter("yawrate_offset.estimated_number_min",yawrate_offset_parameter.estimated_number_min); node->declare_parameter("yawrate_offset.estimated_coefficient",yawrate_offset_parameter.estimated_coefficient); node->declare_parameter("yawrate_offset.estimated_velocity_threshold",yawrate_offset_parameter.estimated_velocity_threshold); node->declare_parameter("yawrate_offset.outlier_threshold",yawrate_offset_parameter.outlier_threshold); + node->declare_parameter("use_canless_mode",use_canless_mode); - node->get_parameter("imu_topic",subscribe_imu_topic_name); - node->get_parameter("reverse_imu", yawrate_offset_parameter.reverse_imu); node->get_parameter("yawrate_offset.estimated_number_min",yawrate_offset_parameter.estimated_number_min); node->get_parameter("yawrate_offset.estimated_coefficient",yawrate_offset_parameter.estimated_coefficient); node->get_parameter("yawrate_offset.estimated_velocity_threshold",yawrate_offset_parameter.estimated_velocity_threshold); node->get_parameter("yawrate_offset.outlier_threshold",yawrate_offset_parameter.outlier_threshold); + node->get_parameter("use_canless_mode",use_canless_mode); - std::cout<< "subscribe_imu_topic_name "<create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); //ros::TransportHints().tcpNoDelay() + auto sub1 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); //ros::TransportHints().tcpNoDelay() auto sub2 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); //ros::TransportHints().tcpNoDelay() auto sub3 = node->create_subscription(subscribe_topic_name, 1000, heading_interpolate_callback); //ros::TransportHints().tcpNoDelay() - auto sub4 = node->create_subscription(subscribe_imu_topic_name, 1000, imu_callback); //ros::TransportHints().tcpNoDelay() + auto sub4 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); //ros::TransportHints().tcpNoDelay() pub = node->create_publisher(publish_topic_name, rclcpp::QoS(10)); rclcpp::spin(node); diff --git a/eagleye_rt/src/yawrate_offset_stop_node.cpp b/eagleye_rt/src/yawrate_offset_stop_node.cpp index 38d37f92..35c842f3 100644 --- a/eagleye_rt/src/yawrate_offset_stop_node.cpp +++ b/eagleye_rt/src/yawrate_offset_stop_node.cpp @@ -57,33 +57,26 @@ 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"; - std::string subscribe_imu_topic_name = "/imu/data_raw"; + node->declare_parameter("twist_topic",subscribe_twist_topic_name); - node->declare_parameter("imu_topic",subscribe_imu_topic_name); - node->declare_parameter("reverse_imu", yawrate_offset_stop_parameter.reverse_imu); node->declare_parameter("yawrate_offset_stop.stop_judgment_velocity_threshold",yawrate_offset_stop_parameter.stop_judgment_velocity_threshold); node->declare_parameter("yawrate_offset_stop.estimated_number",yawrate_offset_stop_parameter.estimated_number); node->declare_parameter("yawrate_offset_stop.outlier_threshold",yawrate_offset_stop_parameter.outlier_threshold); node->get_parameter("twist_topic",subscribe_twist_topic_name); - node->get_parameter("imu_topic",subscribe_imu_topic_name); - node->get_parameter("reverse_imu", yawrate_offset_stop_parameter.reverse_imu); node->get_parameter("yawrate_offset_stop.stop_judgment_velocity_threshold",yawrate_offset_stop_parameter.stop_judgment_velocity_threshold); node->get_parameter("yawrate_offset_stop.estimated_number",yawrate_offset_stop_parameter.estimated_number); node->get_parameter("yawrate_offset_stop.outlier_threshold",yawrate_offset_stop_parameter.outlier_threshold); std::cout<< "subscribe_twist_topic_name "<create_subscription(subscribe_twist_topic_name, 1000, velocity_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription(subscribe_imu_topic_name, 1000, imu_callback); //ros::TransportHints().tcpNoDelay() + auto sub2 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); //ros::TransportHints().tcpNoDelay() pub = node->create_publisher("yawrate_offset_stop", rclcpp::QoS(10)); rclcpp::spin(node); diff --git a/eagleye_util/fix2kml/include/fix2kml/KmlGenerator.hpp b/eagleye_util/fix2kml/include/fix2kml/KmlGenerator.hpp index da03a6f2..bdf80a1f 100755 --- a/eagleye_util/fix2kml/include/fix2kml/KmlGenerator.hpp +++ b/eagleye_util/fix2kml/include/fix2kml/KmlGenerator.hpp @@ -37,7 +37,7 @@ class KmlGenerator { public: - KmlGenerator(const std::string); + KmlGenerator(const std::string,const std::string); KmlGenerator(const std::string,const std::string,const std::string); @@ -55,7 +55,7 @@ class KmlGenerator std::stringstream footer_; }; -KmlGenerator::KmlGenerator(const std::string kmlname) +KmlGenerator::KmlGenerator(const std::string kmlname,const std::string color="ff0000ff") { header_ << ""<< "\n" @@ -69,7 +69,7 @@ KmlGenerator::KmlGenerator(const std::string kmlname) << "\t\t"<< kmlname <<""<< "\n" << "\t\t" << "\n" diff --git a/eagleye_util/fix2kml/launch/fix2kml.xml b/eagleye_util/fix2kml/launch/fix2kml.xml index 59078894..4a16fe3c 100644 --- a/eagleye_util/fix2kml/launch/fix2kml.xml +++ b/eagleye_util/fix2kml/launch/fix2kml.xml @@ -1,13 +1,15 @@ - + + - + + - + + - diff --git a/eagleye_util/fix2kml/src/fix2kml.cpp b/eagleye_util/fix2kml/src/fix2kml.cpp index 6eafa315..f6b6b47e 100755 --- a/eagleye_util/fix2kml/src/fix2kml.cpp +++ b/eagleye_util/fix2kml/src/fix2kml.cpp @@ -28,26 +28,25 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" #include "eagleye_msgs/msg/distance.hpp" #include "fix2kml/KmlGenerator.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" -static double interval = 0.2; //m +static double interval = 0.2; //m static double driving_distance = 0.0; static double driving_distance_last = 0.0; -static std::string filename, kmlname; +static std::string filename, kmlname, fixname, color = "ff0000ff"; void distance_callback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) { driving_distance = msg->distance; } -void receive_data(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg, KmlGenerator *kmlfile) +void receive_data(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg, KmlGenerator* kmlfile) { - if( (driving_distance - driving_distance_last) > interval) - { - kmlfile->addPoint(msg->longitude,msg->latitude,msg->altitude); + if ((driving_distance - driving_distance_last) > interval) { + kmlfile->addPoint(msg->longitude, msg->latitude, msg->altitude); kmlfile->KmlGenerate(filename); driving_distance_last = driving_distance; } @@ -55,22 +54,28 @@ void receive_data(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg, KmlGene int main(int argc, char** argv) { -rclcpp::init(argc, argv); + rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("fix2kml"); - node->declare_parameter("filename",filename); - node->declare_parameter("kmlname",kmlname); + node->declare_parameter("filename", filename); + node->declare_parameter("kmlname", kmlname); + node->declare_parameter("fixname", fixname); + node->declare_parameter("color", color); - node->get_parameter("filename",filename); - node->get_parameter("kmlname",kmlname); + node->get_parameter("filename", filename); + node->get_parameter("kmlname", kmlname); + node->get_parameter("fixname", fixname); + node->get_parameter("color", color); - std::cout<< "filename: "<)> sub1_fnc = std::bind(&receive_data, std::placeholders::_1, &kmlfile); - auto sub1 = node->create_subscription("/eagleye/fix", rclcpp::QoS(10), sub1_fnc); + auto sub1 = node->create_subscription(fixname, rclcpp::QoS(10), sub1_fnc); auto sub2 = node->create_subscription("/eagleye/distance", rclcpp::QoS(10), distance_callback); rclcpp::spin(node); diff --git a/eagleye_util/fix2pose/src/fix2pose.cpp b/eagleye_util/fix2pose/src/fix2pose.cpp index 73a82fef..d1510382 100644 --- a/eagleye_util/fix2pose/src/fix2pose.cpp +++ b/eagleye_util/fix2pose/src/fix2pose.cpp @@ -31,6 +31,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/point_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "eagleye_msgs/msg/rolling.hpp" #include "eagleye_msgs/msg/pitching.hpp" @@ -47,9 +48,11 @@ static eagleye_msgs::msg::Position eagleye_position; 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; static int convert_height_num = 0; static int plane = 7; @@ -111,7 +114,7 @@ void fix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) if (eagleye_heading.status.enabled_status) { eagleye_heading.heading_angle = fmod(eagleye_heading.heading_angle,2*M_PI); - tf2::Matrix3x3(localization_quat).setRPY(eagleye_rolling.rolling_angle,eagleye_pitching.pitching_angle,(90* M_PI / 180)-eagleye_heading.heading_angle); + localization_quat.setRPY(eagleye_rolling.rolling_angle,eagleye_pitching.pitching_angle,(90* M_PI / 180)-eagleye_heading.heading_angle); } else { @@ -127,6 +130,23 @@ void fix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) 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; + // 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); tf2::Transform transform; tf2::Quaternion q; @@ -171,6 +191,7 @@ int main(int argc, char** argv) 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); rclcpp::spin(node); diff --git a/eagleye_util/nmea2fix/src/nmea2fix_core.cpp b/eagleye_util/nmea2fix/src/nmea2fix_core.cpp index c361bf71..60f42dbb 100644 --- a/eagleye_util/nmea2fix/src/nmea2fix_core.cpp +++ b/eagleye_util/nmea2fix/src/nmea2fix_core.cpp @@ -100,8 +100,16 @@ void nmea2fix_converter(const nmea_msgs::msg::Sentence sentence, sensor_msgs::ms gga->station_id = nmea_data[14].substr(0, nmea_data[14].find("*")); fix->header = sentence.header; - fix->latitude = gga->lat; - fix->longitude = gga->lon; + if (gga->lat_dir == "N") { + fix->latitude = gga->lat; + } else if (gga->lat_dir == "S") { + fix->latitude = -gga->lat; + } + if (gga->lon_dir == "E") { + fix->longitude = gga->lon; + } else if (gga->lon_dir == "W") { + fix->longitude = -gga->lon; + } fix->altitude = gga->alt + gga->undulation; fix->status.service = 1; diff --git a/eagleye_util/trajectory_plot/README.md b/eagleye_util/trajectory_plot/README.md new file mode 100644 index 00000000..803366db --- /dev/null +++ b/eagleye_util/trajectory_plot/README.md @@ -0,0 +1,100 @@ +# eagleye_evaluation + +## Set up +``` +pip3 install scipy +pip3 install open3d +pip3 install mgrs +``` + +## Usage +### eagleye_pp_single_evaluation +``` +python3 eagleye_pp_single_evaluation.py +``` + +ex) +``` +python3 scripts/eagleye_pp_single_evaluation.py eagleye_log.csv trajectory_plot.yaml +``` +Output figures +* 6DoF +* Velocity scale factor/Velocity +* 2D trajectory +* GNSS position solution +* 3D trajectory + +### evaluation_plot +``` +python3 scripts/evaluation_plot.py [ref_data_input_option] [target data input option] -yaml +``` + +ex1)(evaluation eagleye_log.csv) +``` +python3 scripts/evaluation_plot.py [-ref] [-log] -yaml +``` +Output figures +* 6DoF +* 6DoF Error +* X Error/Y Error/Across Error/Along Error +* X-Y Error/Across-Along Error +* Cumulative Error Distribution +* Cumulative Error Distribution(relative position) +* DR trajectory +* Velocity/Velocity Error +* 2D trajectory +* GNSS position solution +* 3D trajectory + + +ex2)(evaluation target.csv) +``` +python3 scripts/evaluation_plot.py [-ref] [-target] -yaml +``` +Output figures +* 6DoF +* 6DoF Error +* X Error/Y Error/Across Error/Along Error +* X-Y Error/Across-Along Error +* Cumulative Error Distribution +* Velocity +* 2D trajectory +* 3D trajectory + +### twist_evaluation +``` +python3 scripts/twist_evaluation.py [ref_data_input_option] [twist data input option] -yaml +``` + +ex1)(evaluation eagleye_log.csv) +``` +python3 scripts/twist_evaluation.py [-ref] [-log] -yaml +``` +Output figures +* Cumulative Error Distribution(relative position) +* DR trajectory + +ex2)(evaluation twist.csv) +``` +python3 scripts/evaluation_plot.py [-ref] [-twist] -yaml +``` +Output figures +* Cumulative Error Distribution(relative position) +* DR trajectory + +#### Options +ref data input options +* [`-ref`]: Import reference data referenced by column count +* [`-ref_log`]: Import eagleye_pp data by reference with the name of the HEADER(eagleye_log.csv) + +target data input options +* [`-log`]: Import eagleye_pp data by reference with the name of the HEADER(eagleye_log.csv) +* [`-target`]: Import target data referenced by column count + +twist data input options +* [`-log`]: Import eagleye_pp data by reference with the name of the HEADER(eagleye_log.csv) +* [`-twist`]: Import target twist data referenced by column count + +Common Options +* [`-yaml`]: Import yaml file + diff --git a/eagleye_util/trajectory_plot/config/trajectory_plot.yaml b/eagleye_util/trajectory_plot/config/trajectory_plot.yaml new file mode 100644 index 00000000..c64d847c --- /dev/null +++ b/eagleye_util/trajectory_plot/config/trajectory_plot.yaml @@ -0,0 +1,97 @@ +ref_index: + separate_time_stamp: False #[True]:Set time_sec and time_nsec / [False]:Set time + time_unit: 1 # 0:[s], 1:[ns] + use_quaternion: True # [True]:Set Quaternion / [False]:Set Euler + use_radian: True # [True]:radian / [False]:degree + use_vel: False # Use enu velocity + use_angular: False # + use_qual: False # Use GNSS quality + time_sec: 1 # separate_time_stamp:[True] + time_nsec: 2 # separate_time_stamp:[True] + time: 2 # separate_time_stamp:[False] + x: 4 + y: 5 + z: 6 + ori_x: 7 + ori_y: 8 + ori_z: 9 + ori_w: 10 + roll: 7 + pitch: 8 + yaw: 9 + vel_x: 0 + vel_y: 0 + vel_z: 0 + angular_x: 0 + angular_y: 0 + angular_z: 0 + qual: 10 + +target_data_index: + separate_time_stamp: False #[True]:Set time_sec and time_nsec / [False]:Set time + time_unit: 0 # 0:[s], 1:[ns] + use_quaternion: True # [True]:Set Quaternion / [False]:Set Euler + use_radian: True # [True]:radian / [False]:degree + use_qual: False # Use GNSS quality + time_sec: 1 # separate_time_stamp:[True] + time_nsec: 2 # separate_time_stamp:[True] + time: 2 # separate_time_stamp:[False] + x: 4 + y: 5 + z: 6 + ori_x: 7 + ori_y: 8 + ori_z: 9 + ori_w: 10 + roll: 7 + pitch: 8 + yaw: 9 + qual: 10 + +twist_index: + separate_time_stamp: False #[True]:Set time_sec and time_nsec / [False]:Set time + time_unit: 0 # 0:[s], 1:[ns] + time_sec: 1 # separate_time_stamp:[True] + time_nsec: 2 # separate_time_stamp:[True] + time: 2 # separate_time_stamp:[False] + linear_x: 4 + linear_y: 5 + linear_z: 6 + angular_x: 7 + angular_y: 8 + angular_z: 9 + +eagleye_log: + tf_num: 0 # 0:plane, 1:mgrs + time_unit: 1 # 0:[s], 1:[ns] + ros_reverse_imu: False # [True]:Use reverse_imu in ros + +param: + reverse_imu_flag: False + plane_num: 7 # Plane Cartesian coordinate system number (default:7 = 7) + sync_threshold_time_data_param: 0.01 # Time threshold for judgment at synchronized. (default:0.01 = 0.01[s]) + leap_time_param: 0.0 # Offset correction for time synchronization. (default:0.0 = 0.0[s]) + tf_x_param: 0.0 # Correction amount in East-West direction[m] + tf_y_param: 0.0 # Correction amount in North-South direction[m] + tf_across_param: 0.0 # Correction amount in vehicle orthogonal direction[m] + tf_along_param: 0.0 # Correction amount in the direction of vehicle travel[m] + tf_height_param: 0.0 # Correction amount in height direction[m] + tf_yaw_param: 0.0 # Correction amount in yaw[deg] + based_heaing_angle: False # [True]:The heading angle is based on North /[False]:The heading angle is based on East (ros data) + distance_length_param: 100 # Distance to calculate relative trajectory. (default:100 = 100[m]) + distance_step_param: 50 # Calculate relative trajectories step (default:50 = 50[m]) + 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') + +evaluation_plot: + dr_error_ylim: 3.0 # ylabel data lim + 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]) + +twist_evaluation: + dr_error_ylim: 3.0 # ylabel data lim + 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 new file mode 100755 index 00000000..91c931e3 --- /dev/null +++ b/eagleye_util/trajectory_plot/scripts/eagleye_pp_single_evaluation.py @@ -0,0 +1,111 @@ +# 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. + +# eagleye_pp_single_evaluation.py +# Author MapIV Hoda + +import argparse +import yaml +from re import X +from typing import List +import pandas as pd +import sys +import matplotlib.pyplot as plt + +import util.preprocess as util_prepro +import util.calc as util_calc +import util.plot as util_plot + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("input_file", action="store") + parser.add_argument("input_yaml", action="store") + args= parser.parse_args() + print(args) + input_ref_path=sys.argv[1] + yaml_path=sys.argv[2] + + yaml_path_str: str = yaml_path + with open(yaml_path_str,"r") as yml: + config = yaml.safe_load(yml) + + # set param + reverse_imu = config["param"]["reverse_imu_flag"] + plane = config["param"]["plane_num"] + data_name = config["param"]["data_name_param"] + + print('plane',plane) + + eagleye_df,raw_df = util_prepro.set_log_df(input_ref_path,plane,config) + print("set eagleye_data") + + eagleye_ecef_base = pd.concat([eagleye_df['ecef_base_x'],eagleye_df['ecef_base_y'],eagleye_df['ecef_base_z']],axis=1) + for i in range(len(eagleye_ecef_base)): + if not eagleye_ecef_base['ecef_base_x'][i] == 0 and not eagleye_ecef_base['ecef_base_y'][i] == 0: + org_x = eagleye_ecef_base['ecef_base_x'][i] + org_y = eagleye_ecef_base['ecef_base_y'][i] + org_z = eagleye_ecef_base['ecef_base_z'][i] + org_xyz = [org_x,org_y,org_z] + break + + eagleye_xyz = pd.concat([eagleye_df['x'],eagleye_df['y'],eagleye_df['z']],axis=1) + + raw_llh = pd.concat([raw_df['latitude'],raw_df['longitude'],raw_df['altitude']],axis=1) + raw_xyz = util_prepro.latlon_to_19(raw_llh,plane) + + rtk_llh_tmp = pd.concat([raw_df['rtk_latitude'],raw_df['rtk_longitude'],raw_df['rtk_altitude']],axis=1) + rtk_llh = rtk_llh_tmp.rename(columns={'rtk_latitude': 'latitude', 'rtk_longitude': 'longitude','rtk_altitude': 'altitude'}) + rtk_xyz = util_prepro.latlon_to_19(rtk_llh,plane) + + raw_xyz_vel = pd.concat([raw_df['vel_x'],raw_df['vel_y'],raw_df['vel_z']],axis=1) + vel = util_calc.xyz2enu_vel(raw_xyz_vel,org_xyz) + dopplor_heading = util_calc.calc_dopplor_heading(vel) + 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) + + 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) + + ax_vel = fig2.add_subplot(2, 1, 2) + ax_vel.set_title('Velocity') + ax_vel.plot(dopplor['elapsed_time'] , dopplor['velocity'] , marker="s",linestyle="None",markersize=1 , color = "green", label="dopplor") + ax_vel.plot(raw_df['elapsed_time'] , raw_df['velocity'] , marker=".",linestyle="None",markersize=1, color = "red", label="can velocity") + ax_vel.plot(eagleye_df['elapsed_time'] , eagleye_df['velocity'] , marker="s",linestyle="None",markersize=1,alpha=0.3 , color = "blue", label="eagleye") + ax_vel.set_xlabel('Time [s]') + ax_vel.set_ylabel('Velocity [m/s]') + 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 new file mode 100644 index 00000000..4111cae9 --- /dev/null +++ b/eagleye_util/trajectory_plot/scripts/evaluation_plot.py @@ -0,0 +1,250 @@ +# 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. + +# evaluation_plot.py +# Author MapIV Hoda + +import csv +import argparse +import math +import yaml +from typing import List +import pandas as pd +import numpy as np + +import matplotlib.pyplot as plt +import matplotlib.ticker + +import util.preprocess as util_prepro +import util.calc as util_calc +import util.plot as util_plot + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-ref", "--ref", help="Path to the ref data(POSLV csv)") + parser.add_argument("-target", "--input_target", help="Path to the csv") + parser.add_argument("-log", "--input_log_csv", help="Path to the csv by df") + parser.add_argument("-ref_log", "--input_ref_log", help="Path to the csv by df") + parser.add_argument("-yaml", "--yaml_path",help="yaml name") + args = parser.parse_args() + + yaml_path_str: str = args.yaml_path + with open(yaml_path_str,"r") as yml: + config = yaml.safe_load(yml) + + # set param + reverse_imu = config["param"]["reverse_imu_flag"] + plane = config["param"]["plane_num"] + sync_threshold_time = config["param"]["sync_threshold_time_data_param"] + leap_time = config["param"]["leap_time_param"] + tf_x = config["param"]["tf_x_param"] + tf_y = config["param"]["tf_y_param"] + tf_across = config["param"]["tf_across_param"] + tf_along = config["param"]["tf_along_param"] + tf_height = config["param"]["tf_height_param"] + tf_yaw = config["param"]["tf_yaw_param"] + based_heaing_angle = config["param"]["based_heaing_angle"] + distance_length = config["param"]["distance_length_param"] + distance_step = config["param"]["distance_step_param"] + eval_step_max = config["param"]["eval_step_max_param"] + dr_error_ylim = config["evaluation_plot"]["dr_error_ylim"] + plot_text_data = config["evaluation_plot"]["plot_text_data"] + 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"] + + print('plane',plane) + print('sync_threshold_time',sync_threshold_time) + print('leap_time',leap_time) + print('tf_x',tf_x) + print('tf_y',tf_y) + print('tf_across',tf_across) + print('tf_along',tf_along) + print('tf_height',tf_height) + print('tf_yaw',tf_yaw) + print('based_heaing_angle',based_heaing_angle) + print('reverse_imu',reverse_imu) + print('distance_length',distance_length) + print('distance_step',distance_step) + print('eval_step_max',eval_step_max) + print('dr_error_ylim',dr_error_ylim) + print('plot_text_data',plot_text_data) + print('plot_text_step',plot_text_step) + print('ref_data_name',ref_data_name) + print('data_name',data_name) + + # set data + if args.ref != None: + ref_data_df = util_prepro.set_ref_data(args.ref,config) + print("set ref_data") + + if args.input_target != None: + target_data_df = util_prepro.set_target_data(args.input_target,config) + print("set target_data") + + if args.input_ref_log != None: + ref_data_df, ref_raw_df = util_prepro.set_log_df(args.input_ref_log,plane,config) + print("set ref_data") + + if args.input_log_csv != None: + target_data_df, raw_df = util_prepro.set_log_df(args.input_log_csv,plane,config) + print("set target_data") + + if tf_x != 0 or tf_y != 0: + ref_data_df = util_prepro.set_tf_xy(ref_data_df,tf_x,tf_y) + print('set tf xy') + + + # syne time + print("start sync_time") + ref_df , data_df = util_calc.sync_time(ref_data_df,target_data_df,sync_threshold_time,leap_time) + print("finished sync_time") + + # Quaternion to Euler conversion + eagleye_rpy = pd.DataFrame() + if 'ori_x' in data_df.columns and 'ori_y' in data_df.columns and 'ori_z' in data_df.columns and 'ori_w' in data_df.columns: + eagleye_ori_df = pd.concat([data_df['ori_x'],data_df['ori_y'],data_df['ori_z'],data_df['ori_w']],axis=1) + eagleye_rpy = util_calc.quaternion_to_euler_zyx(eagleye_ori_df) + elif 'roll' in data_df.columns and 'pitch' in data_df.columns and 'yaw' in data_df.columns: + eagleye_rpy = pd.concat([data_df['roll'],data_df['pitch'],data_df['yaw']],axis=1) + + if tf_yaw != 0 or reverse_imu == True : + print("set eagleye yaw data") + if reverse_imu == True: + eagleye_rpy['yaw'] = -1 * eagleye_rpy['yaw'] + if 'angular_z' in data_df.columns: + data_df['angular_z'] = -1 * data_df['angular_z'] + if 'yawrate_offset_stop' in data_df.columns: + data_df['yawrate_offset_stop'] = -1 * data_df['yawrate_offset_stop'] + if 'yawrate_offset' in data_df.columns: + data_df['yawrate_offset'] = -1 * data_df['yawrate_offset'] + if 'slip' in data_df.columns: + data_df['slip'] = -1 * data_df['slip'] + eagleye_rpy['yaw'] = eagleye_rpy['yaw'] + tf_yaw + eagleye_rpy['yaw'] = np.rad2deg(util_calc.change_anglel_limit(np.deg2rad(eagleye_rpy['yaw']))) + + # correct anntena position + ref_rpy = pd.concat([ref_df['roll'],ref_df['pitch'],ref_df['yaw']],axis=1) + if tf_across != 0 or tf_along != 0 or tf_height != 0: + data_df = util_prepro.correct_anntenapos(data_df,ref_rpy['yaw'],tf_across,tf_along,tf_height) + print('set tf') + + # plot 6dof + ref_data_xyz = pd.concat([ref_df['x'],ref_df['y'],ref_df['z']],axis=1) + 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) + + + # calc 6dof error + Error_data = util_calc.calc_error_xyz(ref_df['elapsed_time'],ref_df['TimeStamp'],data_df['TimeStamp'],ref_data_xyz,eagleye_xyz,ref_rpy['yaw']) + print("finished calc_error_xyz") + + error_rpy = util_calc.calc_error_rpy(ref_rpy,eagleye_rpy) + print("finished calc error rpy") + + error_velocity = pd.DataFrame() + if 'velocity' in data_df.columns and 'velocity' in ref_df.columns: + error_velocity = util_calc.calc_velocity_error(data_df['velocity'],ref_df['velocity']) + + # 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) + + # plot Cumulative Error Distribution + diff_2d: List[float] = [] + if '2d' in Error_data.columns: + diff_2d = Error_data['2d'].values.tolist() + ErrTra_Rate = util_calc.calc_TraRate(diff_2d , eval_step_max) + + 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) + 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]) + + # twist Performance Evaluation + if 'vel_x' in data_df.columns and 'vel_y' in data_df.columns and 'yawrate_offset_stop' in data_df.columns and 'yawrate_offset' in data_df.columns and 'slip' in data_df.columns: + print("start calc relative position") + eagleye_vel_xyz = pd.concat([data_df['vel_x'],data_df['vel_y'],data_df['vel_z']],axis=1) + ref_xyz = pd.concat([ref_df['x'],ref_df['y'],ref_df['z']],axis=1) + eagleye_twist_data = pd.concat([data_df['angular_z'],data_df['yawrate_offset_stop'],data_df['yawrate_offset'],data_df['velocity'],data_df['slip']],axis=1) + calc_error, dr_trajcetory = util_calc.calc_dr_eagleye(ref_df["TimeStamp"],data_df["distance"],eagleye_twist_data,np.deg2rad(ref_rpy["yaw"]),ref_xyz,distance_length,distance_step,based_heaing_angle) + print("finished calc relative position") + + dr_error_2d = calc_error['error_2d'].values.tolist() + ErrTra_dr_df = util_calc.calc_TraRate(dr_error_2d , eval_step_max) + + 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) + 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) + 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) + 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') + + # plot velocity + 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) + + 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) + + 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) + + # 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) + + if 'qual' in data_df.columns: + util_plot.plot_traj_qual(eagleye_xyz,data_df['qual']) + elif 'qual' in ref_df.columns: + util_plot.plot_traj_qual(ref_data_xyz,ref_df['qual']) + + + # plot 3d trajectory + util_plot.plot_traj_3d( ref_df, data_df, data_name, ref_data_name) + + print(error_table) + + plt.show() + \ No newline at end of file diff --git a/eagleye_util/trajectory_plot/scripts/twist_evaluation.py b/eagleye_util/trajectory_plot/scripts/twist_evaluation.py new file mode 100644 index 00000000..cc6d4aa7 --- /dev/null +++ b/eagleye_util/trajectory_plot/scripts/twist_evaluation.py @@ -0,0 +1,190 @@ +# 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_evaluation.py +# Author MapIV Hoda + +import argparse +import math +import yaml +from typing import List +import pandas as pd +import numpy as np + +import matplotlib.pyplot as plt +import matplotlib.ticker + +import util.preprocess as util_prepro +import util.calc as util_calc +import util.plot as util_plot + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-ref", "--ref", help="Path to the ref data(POSLV csv)") + parser.add_argument("-ref_twist", "--ref_twist", help="Path to the ref twist data") + parser.add_argument("-twist", "--input_twist", help="Path to the twist csv") + parser.add_argument("-log", "--input_log_csv", help="Path to the csv by df") + parser.add_argument("-ref_log", "--input_ref_log", help="Path to the csv by df") + parser.add_argument("-yaml", "--yaml_path",help="yaml name") + args = parser.parse_args() + + yaml_path_str: str = args.yaml_path + with open(yaml_path_str,"r") as yml: + config = yaml.safe_load(yml) + + # set param + reverse_imu = config["param"]["reverse_imu_flag"] + plane = config["param"]["plane_num"] + sync_threshold_time = config["param"]["sync_threshold_time_data_param"] + leap_time = config["param"]["leap_time_param"] + tf_x = config["param"]["tf_x_param"] + tf_y = config["param"]["tf_y_param"] + tf_across = config["param"]["tf_across_param"] + tf_along = config["param"]["tf_along_param"] + tf_height = config["param"]["tf_height_param"] + tf_yaw = config["param"]["tf_yaw_param"] + based_heaing_angle = config["param"]["based_heaing_angle"] + distance_length = config["param"]["distance_length_param"] + distance_step = config["param"]["distance_step_param"] + eval_step_max = config["param"]["eval_step_max_param"] + dr_error_ylim = config["twist_evaluation"]["dr_error_ylim"] + plot_text_data = config["twist_evaluation"]["plot_text_data"] + 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"] + + print('plane',plane) + print('sync_threshold_time',sync_threshold_time) + print('leap_time',leap_time) + print('tf_x',tf_x) + print('tf_y',tf_y) + print('tf_across',tf_across) + print('tf_along',tf_along) + print('tf_height',tf_height) + print('tf_yaw',tf_yaw) + print('based_heaing_angle',based_heaing_angle) + print('reverse_imu',reverse_imu) + print('distance_length',distance_length) + print('distance_step',distance_step) + print('eval_step_max',eval_step_max) + print('dr_error_ylim',dr_error_ylim) + print('plot_text_data',plot_text_data) + print('plot_text_step',plot_text_step) + print('ref_data_name',ref_data_name) + print('data_name',data_name) + + # set data + if args.ref != None: + ref_data_df = util_prepro.set_ref_data(args.ref,config) + print("set ref_data") + + if args.input_twist != None: + csv_data_df = util_prepro.set_twist_data(args.input_twist,config) + print("set csv_data") + + if args.ref_twist != None: + ref_data_df = util_prepro.set_twist_data(args.ref_twist,config) + print("set ref_data") + + if args.input_ref_log != None: + ref_data_df, ref_raw_df = util_prepro.set_log_df(args.input_ref_log,plane,config) + eagleye_ecef_base = pd.concat([ref_data_df['ecef_base_x'],ref_data_df['ecef_base_y'],ref_data_df['ecef_base_z']],axis=1) + for i in range(len(eagleye_ecef_base)): + if not eagleye_ecef_base['ecef_base_x'][i] == 0 and not eagleye_ecef_base['ecef_base_y'][i] == 0: + org_x = eagleye_ecef_base['ecef_base_x'][i] + org_y = eagleye_ecef_base['ecef_base_y'][i] + org_z = eagleye_ecef_base['ecef_base_z'][i] + org_xyz = [org_x,org_y,org_z] + break + raw_xyz_vel = pd.concat([ref_raw_df['vel_x'],ref_raw_df['vel_y'],ref_raw_df['vel_z']],axis=1) + vel = util_calc.xyz2enu_vel(raw_xyz_vel,org_xyz) + dopplor = pd.concat([ref_raw_df['elapsed_time'],vel],axis=1) + print("set ref_data") + + if args.input_log_csv != None: + csv_data_df, raw_df = util_prepro.set_log_df(args.input_log_csv,plane,config) + eagleye_ecef_base = pd.concat([csv_data_df['ecef_base_x'],csv_data_df['ecef_base_y'],csv_data_df['ecef_base_z']],axis=1) + for i in range(len(eagleye_ecef_base)): + if not eagleye_ecef_base['ecef_base_x'][i] == 0 and not eagleye_ecef_base['ecef_base_y'][i] == 0: + org_x = eagleye_ecef_base['ecef_base_x'][i] + org_y = eagleye_ecef_base['ecef_base_y'][i] + org_z = eagleye_ecef_base['ecef_base_z'][i] + org_xyz = [org_x,org_y,org_z] + break + raw_xyz_vel = pd.concat([raw_df['vel_x'],raw_df['vel_y'],raw_df['vel_z']],axis=1) + vel = util_calc.xyz2enu_vel(raw_xyz_vel,org_xyz) + dopplor = pd.concat([raw_df['elapsed_time'],vel],axis=1) + print("set csv_data") + + # syne time + print("start sync_time") + ref_df , data_df = util_calc.sync_time(ref_data_df,csv_data_df,sync_threshold_time,leap_time) + print("finished sync_time") + + if reverse_imu == True: + if 'angular_z' in data_df.columns: + data_df['angular_z'] = -1 * data_df['angular_z'] + if 'yawrate_offset_stop' in data_df.columns: + data_df['yawrate_offset_stop'] = -1 * data_df['yawrate_offset_stop'] + if 'yawrate_offset' in data_df.columns: + data_df['yawrate_offset'] = -1 * data_df['yawrate_offset'] + if 'slip' in data_df.columns: + data_df['slip'] = -1 * data_df['slip'] + + if 'x' in ref_df.columns and 'y' in ref_df.columns and 'yaw' in ref_df.columns and 'velocity' in data_df and 'angular_z' in data_df: + print('calc dr') + ref_xyz = pd.concat([ref_df['x'],ref_df['y']],axis=1) + if 'yawrate_offset_stop' in data_df and'yawrate_offset' in data_df and 'slip' in data_df and 'distance' in data_df: + eagleye_twist_data = pd.concat([data_df['angular_z'],data_df['yawrate_offset_stop'],data_df['yawrate_offset'],data_df['velocity'],data_df['slip']],axis=1) + distance = util_calc.calc_distance_xy(ref_xyz) + dr_error, dr_trajcetory = util_calc.calc_dr_eagleye(ref_df["TimeStamp"],distance["distance"],eagleye_twist_data,np.deg2rad(ref_df["yaw"]),ref_xyz,distance_length,distance_step,based_heaing_angle) + calc_error_csv = pd.concat([dr_error['start_distance'],dr_error['error_2d']],axis=1) + calc_error_csv.to_csv("eagleye_dr_error.csv", header=True, index=False,float_format='%.9f') + + else: + twist_data = pd.concat([data_df['angular_z'],data_df['velocity']],axis=1) + distance = util_calc.calc_distance_xy(ref_xyz) + dr_error, dr_trajcetory = util_calc.calc_dr_twist(ref_df["TimeStamp"],distance["distance"],twist_data,np.deg2rad(ref_df["yaw"]),ref_xyz,distance_length,distance_step,based_heaing_angle) + calc_error_csv = pd.concat([dr_error['start_distance'],dr_error['error_2d']],axis=1) + calc_error_csv.to_csv("target_dr_error.csv", header=True, index=False,float_format='%.9f') + + 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) + 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) + 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) + + + plt.show() + \ No newline at end of file diff --git a/eagleye_util/trajectory_plot/scripts/util/calc.py b/eagleye_util/trajectory_plot/scripts/util/calc.py new file mode 100644 index 00000000..95d48a9d --- /dev/null +++ b/eagleye_util/trajectory_plot/scripts/util/calc.py @@ -0,0 +1,658 @@ + + + +from typing import List +import pandas as pd +import numpy as np +import math +import mgrs +import sys + +from tqdm import tqdm +from scipy.spatial.transform import Rotation as R +from decimal import Decimal, ROUND_HALF_UP + +def xyz2enu(ecef_xyz,org_xyz): + org_llh = xyz2llh(org_xyz) + phi = org_llh[0] + lam = org_llh[1] + sinphi = np.sin(phi) + cosphi = np.cos(phi) + sinlam = np.sin(lam) + coslam = np.cos(lam) + R = np.matrix([[-sinlam , coslam , 0],[-sinphi*coslam , -sinphi*sinlam , cosphi],[cosphi*coslam , cosphi*sinlam , sinphi]]) + + set_enu_data: List[float] = [] + for i in range(len(ecef_xyz)): + if ecef_xyz['ecef_x'][i] == 0 and ecef_xyz['ecef_y'][i] == 0: + set_enu_data.append([0,0,0]) + continue + diff_xyz = np.array([[ecef_xyz['ecef_x'][i] - org_xyz[0]] , [ecef_xyz['ecef_y'][i] - org_xyz[1]] , [ecef_xyz['ecef_z'][i] - org_xyz[2]]]) + enu = R * diff_xyz + x: float = float(enu[0]) + y: float = float(enu[1]) + z: float = float(enu[2]) + set_enu_data.append([x,y,z]) + set_enu = pd.DataFrame(set_enu_data,columns=['x','y','z']) + return set_enu + +def xyz2llh(xyz): + x = xyz[0] + y = xyz[1] + z = xyz[2] + x2 = x**2 + y2 = y**2 + z2 = z**2 + + a = 6378137.0000 # earth radius in meters + b = 6356752.3142 # earth semiminor in meters + e = (1-(b/a)**2)**0.5 + b2 = b*b + e2 = e**2 + ep = e*(a/b) + r = (x2+y2)**0.5 + r2 = r*r + E2 = a**2 - b**2 + F = 54*b2*z2 + G = r2 + (1-e2)*z2 - e2*E2 + c = (e2*e2*F*r2)/(G*G*G) + s = ( 1 + c + (c*c + 2*c)**0.5 )**(1/3) + P = F / (3 * (s+1/s+1)**2 * G*G) + Q = (1+2*e2*e2*P)**0.5 + ro = -(P*e2*r)/(1+Q) + ((a*a/2)*(1+1/Q) - (P*(1-e2)*z2)/(Q*(1+Q)) - P*r2/2) ** 0.5 + tmp = (r - e2*ro)**2 + U = ( tmp + z2 )**0.5 + V = ( tmp + (1-e2)*z2 )**0.5 + zo = (b2*z)/(a*V) + + height = U*( 1 - b2/(a*V) ) + + lat = math.atan( (z + ep*ep*zo)/r ) + temp = math.atan(y/x) + if x >=0: + long = temp + elif (x < 0) & (y >= 0): + long = math.pi + temp + else: + long = temp - math.pi + + llh = [lat,long,height] + return llh + +def llh2xyz(llh): + set_xyz_data: List[float] = [] + for i in range(len(llh)): + lat = llh['latitude'][i] + lon = llh['longitude'][i] + ht = llh['altitude'][i] + + PI_180 = math.pi / 180.0 + A = 6378137.0 + ONE_F = 298.257223563 + E2 = (1.0 / ONE_F) * (2 - (1.0 / ONE_F)) + + n = lambda x: A / \ + math.sqrt(1.0 - E2 * math.sin(x * PI_180)**2) + x = (n(lat) + ht) \ + * math.cos(lat * PI_180) \ + * math.cos(lon * PI_180) + y = (n(lat) + ht) \ + * math.cos(lat * PI_180) \ + * math.sin(lon * PI_180) + z = (n(lat) * (1.0 - E2) + ht) \ + * math.sin(lat * PI_180) + set_xyz_data.append([x,y,z]) + set_xyz = pd.DataFrame(set_xyz_data,columns=['ecef_x','ecef_y','ecef_z']) + return set_xyz + +def change_anglel_limit(heading): + set_output: List[float] = [] + for i in range(len(heading)): + heading_tmp = heading[i] + while heading_tmp < -math.pi or math.pi < heading_tmp: + if heading_tmp < -math.pi: + heading_tmp += math.pi * 2 + else: + heading_tmp -= math.pi * 2 + set_output.append(heading_tmp) + output = pd.DataFrame(set_output,columns=['yaw']) + return output + +def xyz2enu_vel(vel,org_xyz): + set_vel_data: List[float] = [] + for i in range(len(vel)): + x_vel = vel['vel_x'][i] + y_vel = vel['vel_y'][i] + z_vel = vel['vel_z'][i] + + org_llh = xyz2llh(org_xyz) + phi = org_llh[0] + lam = org_llh[1] + + sin_phi = math.sin(phi) + cos_phi = math.cos(phi) + sin_lam = math.sin(lam) + cos_lam = math.cos(lam) + + e_vel = (-x_vel * sin_lam) + (y_vel * cos_lam) + n_vel = (-x_vel * cos_lam * sin_phi) - (y_vel * sin_lam * sin_phi) + (z_vel * cos_phi) + u_vel = (x_vel * cos_lam * cos_phi) +( y_vel * sin_lam * cos_phi) + (z_vel * sin_phi) + vel_2d = (e_vel ** 2 + n_vel ** 2) ** 0.5 + + set_vel_data.append([e_vel,n_vel,u_vel,vel_2d]) + df = pd.DataFrame(set_vel_data,columns=['east_vel','north_vel','up_vel','velocity']) + return df + +def calc_dopplor_heading(vel): + heading = np.degrees(np.arctan2(vel['east_vel'], vel['north_vel'])) + df = pd.DataFrame(heading,columns=['yaw']) + return df + +def ll2mgrs(llh): + set_mgrs: List[float] = [] + m = mgrs.MGRS() + for i in range(len(llh)): + ll = (llh['latitude'][i], llh['longitude'][i] , 4) + coords = m.toMGRS(*ll) + zone, x, y = coords[:5], float(coords[5:10]), float(coords[10:]) + height = llh['altitude'][i] + set_mgrs.append([x,y,height]) + xyz = pd.DataFrame(set_mgrs,columns=['x','y','z']) + return xyz + +def calc_distance_vel(velocity, time): + last_distance = 0 + set_distance: List[float] = [] + for i in range(len(time)): + if i == 0: + last_time = time.iloc[i] + continue + distance = last_distance + velocity.iloc[i] * (time.iloc[i] - last_time) + last_time = time.iloc[i] + last_distance = distance + set_distance.append([distance]) + distance = pd.DataFrame(set_distance,columns=['distance']) + return distance + +def calc_distance_xy(xy): + last_distance = 0 + set_distance: List[float] = [] + for i in range(len(xy)): + if i == 0: + distance = 0 + else: + distance = last_distance + (((xy.iloc[i]['x']-xy.iloc[i-1]['x']) ** 2) + (xy.iloc[i]['y']-xy.iloc[i-1]['y']) ** 2)** 0.5 + last_distance = distance + set_distance.append([distance]) + distance = pd.DataFrame(set_distance,columns=['distance']) + return distance + +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] = [] + 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) + 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) + return ref_df_output , data_df_output + +def calc_error_xyz(elapsed_time,ref_time,eagleye_time,ref_xyz,data_xyz,ref_yaw): + error_xyz = pd.DataFrame() + error_xyz['elapsed_time'] = elapsed_time + error_xyz['TimeStamp'] = ref_time - eagleye_time + error_xyz['x'] = data_xyz['x'] - ref_xyz['x'] + error_xyz['y'] = data_xyz['y'] - ref_xyz['y'] + error_xyz['z'] = data_xyz['z'] - ref_xyz['z'] + error_xyz['2d'] = (error_xyz['x']**2 + error_xyz['y']**2)**0.5 + + set_aa: List[float] = [] + for i in range(len(ref_yaw)): + across = error_xyz.iloc[i]['x'] * math.cos(math.radians(ref_yaw[i])) - error_xyz.iloc[i]['y'] * math.sin(math.radians(ref_yaw[i])) + along = error_xyz.iloc[i]['x'] * math.sin(math.radians(ref_yaw[i])) + error_xyz.iloc[i]['y'] * math.cos(math.radians(ref_yaw[i])) + set_aa.append([across,along]) + error_aa = pd.DataFrame(set_aa,columns=['across','along']) + + error = pd.concat([error_xyz,error_aa],axis=1) + return error + + +def calc_TraRate(diff_2d , eval_step_max): # Calculation of error , x_label , ErrTra_Rate rate + ErrTra_cnt: List[int] = [] + set_ErrTra_middle: List[float] = [] + set_ErrTra: List[float] = [] + ErrTra_cnt = len(diff_2d) + + step = 0.01 + eval_step_middle = 0.1 + for i in np.arange(0, eval_step_middle, step): + cnt = 0 + eval_step = i + for data in diff_2d: + if data < eval_step: + cnt = cnt + 1 + rate = cnt / ErrTra_cnt + set_ErrTra_middle.append([eval_step,rate,rate *100]) + ErrTra_Rate_middle = pd.DataFrame(set_ErrTra_middle,columns=['x_label','ErrTra_Rate','ErrTra']) + + step = 0.1 + for i in np.arange(eval_step_middle, eval_step_max, step): + cnt = 0 + eval_step = i + for data in diff_2d: + if data < eval_step: + cnt = cnt + 1 + rate = cnt / ErrTra_cnt + set_ErrTra.append([eval_step,rate,rate *100]) + ErrTra_Rate_tmp = pd.DataFrame(set_ErrTra,columns=['x_label','ErrTra_Rate','ErrTra']) + ErrTra_Rate = pd.concat([ErrTra_Rate_middle,ErrTra_Rate_tmp]) + return ErrTra_Rate + +def quaternion_to_euler_zyx(ori): + set_eular_angle: List[float] = [] + for i in range(len(ori)): + q = [ori.iloc[i]['ori_x'],ori.iloc[i]['ori_y'],ori.iloc[i]['ori_z'],ori.iloc[i]['ori_w']] + r = R.from_quat([q[0], q[1], q[2], q[3]]) + roll = r.as_euler('ZYX', degrees=True)[2] + pitch = r.as_euler('ZYX', degrees=True)[1] + yaw = r.as_euler('ZYX', degrees=True)[0] + set_eular_angle.append([roll,pitch,yaw]) + euler_angle = pd.DataFrame(set_eular_angle,columns=['roll','pitch','yaw']) + return euler_angle + +def calc_error_rpy(ref_angle,eagleye_angle): + error_rpy = pd.DataFrame() + error_rpy['roll'] = eagleye_angle['roll'] - ref_angle['roll'] + error_rpy['pitch'] = eagleye_angle['pitch'] - ref_angle['pitch'] + error_rpy['yaw'] = eagleye_angle['yaw'] - ref_angle['yaw'] + for i in range(len(error_rpy)): + if error_rpy['yaw'][i] > math.degrees(math.pi): + error_rpy['yaw'][i] = error_rpy['yaw'][i] - 2 * math.degrees(math.pi) + elif error_rpy['yaw'][i] < -math.degrees(math.pi): + error_rpy['yaw'][i] = error_rpy['yaw'][i] + 2 * math.degrees(math.pi) + return error_rpy + +def calc_velocity(vel): + velocity = pd.DataFrame() + velocity['velocity'] = (vel['vel_x'] ** 2 + vel['vel_y'] ** 2 + vel['vel_z'] ** 2) ** 0.5 + return velocity + +def calc_velocity_error(eagleye_velocity,ref_velocity): + error_velocity = pd.DataFrame() + error_velocity['velocity'] = eagleye_velocity - ref_velocity + return error_velocity + +def calc_dr(TimeStamp,distance,eagleye_vel_xyz,ref_xyz,distance_length,distance_step): + last_distance = 0 + set_dr_trajcetory: List[float] = [] + set_calc_error: List[float] = [] + for i in range(len(TimeStamp)): + if i == 0: continue + if last_distance + distance_step < distance[i]: + start_distance = distance[i] + start_pos_x = ref_xyz['x'][i] + start_pos_y = ref_xyz['y'][i] + previous_pos_x = 0 + previous_pos_y = 0 + last_distance = start_distance + last_time = TimeStamp[i] + for j in range(i,len(TimeStamp)): + if eagleye_vel_xyz['vel_x'][j] == 0 and eagleye_vel_xyz['vel_y'][j] == 0: + last_time = TimeStamp[j] + distance_data = distance[j] - start_distance + absolute_pos_x = ref_xyz['x'][j] - start_pos_x + absolute_pos_y = ref_xyz['y'][j] - start_pos_y + continue + if distance[j] - start_distance < distance_length: + dr_pos_x = previous_pos_x + eagleye_vel_xyz['vel_x'][j] * (TimeStamp[j] - last_time) + dr_pos_y = previous_pos_y + eagleye_vel_xyz['vel_y'][j] * (TimeStamp[j] - last_time) + previous_pos_x = dr_pos_x + previous_pos_y = dr_pos_y + last_time = TimeStamp[j] + distance_data = distance[j] - start_distance + absolute_pos_x = ref_xyz['x'][j] - start_pos_x + absolute_pos_y = ref_xyz['y'][j] - start_pos_y + absolute_dr_pos_x = start_pos_x + dr_pos_x + absolute_dr_pos_y = start_pos_y + dr_pos_y + set_dr_trajcetory.append([absolute_dr_pos_x,absolute_dr_pos_y]) + else: + error_x = absolute_pos_x - dr_pos_x + error_y = absolute_pos_y - dr_pos_y + error_2d_fabs = math.fabs(error_x ** 2 + error_y ** 2) + error_2d= math.sqrt(error_2d_fabs) + set_calc_error.append([start_distance,distance_data,absolute_pos_x,dr_pos_x,absolute_pos_y,dr_pos_y,error_x,error_y,error_2d]) + break + calc_error = pd.DataFrame(set_calc_error,columns=['start_distance','distance','absolute_pos_x','absolute_pos_y','dr_pos_x','dr_pos_y','error_x','error_y','error_2d']) + dr_trajcetory = pd.DataFrame(set_dr_trajcetory,columns=['x','y']) + return calc_error,dr_trajcetory + +def calc_dr_twist(TimeStamp,distance,twist_data,ref_heading,ref_xyz,distance_length,distance_step,based_heaing_angle): + last_distance = 0 + last_heading_integtation = 0 + set_dr_trajcetory: List[float] = [] + set_calc_error: List[float] = [] + for i in range(len(TimeStamp)): + if i == 0: continue + if last_distance + distance_step < distance[i]: + data_set_flag = True + start_distance = distance[i] + start_pos_x = ref_xyz['x'][i] + start_pos_y = ref_xyz['y'][i] + previous_pos_x = 0 + previous_pos_y = 0 + last_distance = start_distance + last_time = TimeStamp[i] + for j in range(i,len(TimeStamp)): + if data_set_flag == True: + last_time = TimeStamp[j] + last_heading_integtation = ref_heading[j] + distance_data = distance[j] - start_distance + absolute_pos_x = ref_xyz['x'][j] - start_pos_x + absolute_pos_y = ref_xyz['y'][j] - start_pos_y + data_set_flag = False + continue + else: + heading_integtation = last_heading_integtation + twist_data['angular_z'][j] * (TimeStamp[j] - last_time) + if based_heaing_angle == True: + usr_vel_x = math.sin(heading_integtation) * twist_data['velocity'][j] + usr_vel_y = math.cos(heading_integtation) * twist_data['velocity'][j] + else: + usr_vel_x = math.cos(heading_integtation) * twist_data['velocity'][j] + usr_vel_y = math.sin(heading_integtation) * twist_data['velocity'][j] + if distance[j] - start_distance < distance_length: + dr_pos_x = previous_pos_x + usr_vel_x * (TimeStamp[j] - last_time) + dr_pos_y = previous_pos_y + usr_vel_y * (TimeStamp[j] - last_time) + last_heading_integtation = heading_integtation + previous_pos_x = dr_pos_x + previous_pos_y = dr_pos_y + last_time = TimeStamp[j] + distance_data = distance[j] - start_distance + absolute_pos_x = ref_xyz['x'][j] - start_pos_x + absolute_pos_y = ref_xyz['y'][j] - start_pos_y + absolute_dr_pos_x = start_pos_x + dr_pos_x + absolute_dr_pos_y = start_pos_y + dr_pos_y + set_dr_trajcetory.append([absolute_dr_pos_x,absolute_dr_pos_y]) + else: + error_x = absolute_pos_x - dr_pos_x + error_y = absolute_pos_y - dr_pos_y + error_2d_fabs = math.fabs(error_x ** 2 + error_y ** 2) + error_2d= math.sqrt(error_2d_fabs) + set_calc_error.append([start_distance,distance_data,absolute_pos_x,dr_pos_x,absolute_pos_y,dr_pos_y,error_x,error_y,error_2d]) + break + calc_error = pd.DataFrame(set_calc_error,columns=['start_distance','distance','absolute_pos_x','absolute_pos_y','dr_pos_x','dr_pos_y','error_x','error_y','error_2d']) + dr_trajcetory = pd.DataFrame(set_dr_trajcetory,columns=['x','y']) + return calc_error,dr_trajcetory + +def calc_dr_eagleye(TimeStamp,distance,eagleye_twist_data,ref_heading,ref_xyz,distance_length,distance_step,based_heaing_angle): + last_distance = 0 + last_heading_integtation = 0 + velocity_stop_threshold = 0.01 + velocity_slip_threshold = 1 + set_dr_trajcetory: List[float] = [] + set_calc_error: List[float] = [] + for i in range(len(TimeStamp)): + if i == 0: continue + if last_distance + distance_step < distance[i]: + data_set_flag = True + start_distance = distance[i] + start_pos_x = ref_xyz['x'][i] + start_pos_y = ref_xyz['y'][i] + previous_pos_x = 0 + previous_pos_y = 0 + last_distance = start_distance + last_time = TimeStamp[i] + for j in range(i,len(TimeStamp)): + if data_set_flag == True: + last_time = TimeStamp[j] + last_heading_integtation = ref_heading[j] + distance_data = distance[j] - start_distance + absolute_pos_x = ref_xyz['x'][j] - start_pos_x + absolute_pos_y = ref_xyz['y'][j] - start_pos_y + data_set_flag = False + continue + if eagleye_twist_data['velocity'][j] > velocity_stop_threshold: + heading_integtation = last_heading_integtation + (eagleye_twist_data['angular_z'][j] + eagleye_twist_data['yawrate_offset'][j]) * (TimeStamp[j] - last_time) + else: + heading_integtation = last_heading_integtation + (eagleye_twist_data['angular_z'][j] + eagleye_twist_data['yawrate_offset_stop'][j]) * (TimeStamp[j] - last_time) + if eagleye_twist_data['velocity'][j] > velocity_slip_threshold: + heading_correction_slip = heading_integtation + eagleye_twist_data['slip'][j] + else: + heading_correction_slip = heading_integtation + if distance[j] - start_distance < distance_length: + if based_heaing_angle == True: + usr_vel_x = math.sin(heading_correction_slip) * eagleye_twist_data['velocity'][j] + usr_vel_y = math.cos(heading_correction_slip) * eagleye_twist_data['velocity'][j] + else: + usr_vel_x = math.cos(heading_correction_slip) * eagleye_twist_data['velocity'][j] + usr_vel_y = math.sin(heading_correction_slip) * eagleye_twist_data['velocity'][j] + dr_pos_x = previous_pos_x + usr_vel_x * (TimeStamp[j] - last_time) + dr_pos_y = previous_pos_y + usr_vel_y * (TimeStamp[j] - last_time) + last_heading_integtation = heading_correction_slip + previous_pos_x = dr_pos_x + previous_pos_y = dr_pos_y + last_time = TimeStamp[j] + distance_data = distance[j] - start_distance + absolute_pos_x = ref_xyz['x'][j] - start_pos_x + absolute_pos_y = ref_xyz['y'][j] - start_pos_y + absolute_dr_pos_x = start_pos_x + dr_pos_x + absolute_dr_pos_y = start_pos_y + dr_pos_y + set_dr_trajcetory.append([absolute_dr_pos_x,absolute_dr_pos_y]) + else: + error_x = absolute_pos_x - dr_pos_x + error_y = absolute_pos_y - dr_pos_y + error_2d_fabs = math.fabs(error_x ** 2 + error_y ** 2) + error_2d= math.sqrt(error_2d_fabs) + set_calc_error.append([start_distance,distance_data,absolute_pos_x,dr_pos_x,absolute_pos_y,dr_pos_y,error_x,error_y,error_2d]) + break + calc_error = pd.DataFrame(set_calc_error,columns=['start_distance','distance','absolute_pos_x','absolute_pos_y','dr_pos_x','dr_pos_y','error_x','error_y','error_2d']) + dr_trajcetory = pd.DataFrame(set_dr_trajcetory,columns=['x','y']) + return calc_error,dr_trajcetory + +def error_evaluation_each(error, elem): + data_max = max(error[elem]) + data_average = np.average(error[elem]) + data_std = np.std(error[elem]) + data_rms = np.sqrt(np.square(error[elem]).mean(axis = 0)) + digits_num = '0.01' + return [elem, \ + (Decimal(str(data_max)).quantize(Decimal(digits_num), rounding=ROUND_HALF_UP) ), \ + (Decimal(str(data_average)).quantize(Decimal(digits_num), rounding=ROUND_HALF_UP) ), \ + (Decimal(str(data_std)).quantize(Decimal(digits_num), rounding=ROUND_HALF_UP) ), \ + (Decimal(str(data_rms)).quantize(Decimal(digits_num), rounding=ROUND_HALF_UP) )] + +def error_evaluation(error): + x = error_evaluation_each(error, 'x') + y = error_evaluation_each(error, 'y') + z = error_evaluation_each(error, 'z') + xy = error_evaluation_each(error, '2d') + roll = error_evaluation_each(error, 'roll') + pitch = error_evaluation_each(error, 'pitch') + yaw = error_evaluation_each(error, 'yaw') + error_table = pd.DataFrame([x, y, z, xy, roll, pitch, yaw], columns = ['data', 'max', 'average', 'std', 'rms']) + return error_table + diff --git a/eagleye_util/trajectory_plot/scripts/util/plot.py b/eagleye_util/trajectory_plot/scripts/util/plot.py new file mode 100644 index 00000000..443d5f4e --- /dev/null +++ b/eagleye_util/trajectory_plot/scripts/util/plot.py @@ -0,0 +1,294 @@ +# 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. + +# plot.py +# Author MapIV Hoda + +from typing import List +import pandas as pd +import numpy as np +from math import log10 , floor + +import matplotlib.pyplot as plt + +def judge_qual(xyz,qual,qual_num): + set_output_df: List[float] = [] + for i in range(len(xyz)): + if qual_num == qual[i]: + x = xyz['x'][i] + y = xyz['y'][i] + z = xyz['z'][i] + qual_data = qual[i] + set_output_df.append([x,y,z,qual_data]) + 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): + 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_title(title) + ax.legend(loc='upper right') + ax.grid() + +def plot_rpy(ax, x_data, eagleye_plot_rpy, dopplor, elem, title, y_label): + 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_title(title) + ax.legend(loc='upper right') + ax.grid() + +def plot_each(ax, x_data , eagleye, ref, y_data, title, y_label,data_name, ref_data_name): + 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_title(title) + ax.legend(loc='upper right') + 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): + fig1 = plt.figure() + ax_x = fig1.add_subplot(2, 3, 1) + ax_y = fig1.add_subplot(2, 3, 2) + ax_z = fig1.add_subplot(2, 3, 3) + 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]') + +def plot_6DoF(x_data,eagleye, ref,data_name, ref_data_name): + fig1 = plt.figure() + ax_x = fig1.add_subplot(2, 3, 1) + ax_y = fig1.add_subplot(2, 3, 2) + ax_z = fig1.add_subplot(2, 3, 3) + 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) + +def plot_each_error(ax, error_data, y_data, title, y_label): + 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_title(title) + ax.grid() + +def plot_one(ax, error_data, x_data, y_data, title, x_label, y_label, line_style, marker_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_title(title) + ax.grid() + +def plot_two(ax, ref, eagleye, x_data, y_data, title, x_label, y_label, line_style, 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_title(title) + ax.legend(loc='upper right') + 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): + 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_title(title) + ax.legend(loc='upper right') + ax.grid() + +def plot_error_6DoF(error_data,ref_data_name, error_table): + fig2 = plt.figure() + fig2.suptitle(ref_data_name + ' - eagleye Error') + ax_x = fig2.add_subplot(4, 3, 1) + ax_y = fig2.add_subplot(4, 3, 2) + ax_z = fig2.add_subplot(4, 3, 3) + ax_x_table = fig2.add_subplot(4, 3, 4) + ax_y_table = fig2.add_subplot(4, 3, 5) + ax_z_table = fig2.add_subplot(4, 3, 6) + ax_roll = fig2.add_subplot(4, 3, 7) + ax_pitch = fig2.add_subplot(4, 3, 8) + ax_yaw = fig2.add_subplot(4, 3, 9) + 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]") + +def plot_table(ax, error_table, unit): + 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) + ax.axis('tight') + ax.axis('off') + +def plot_error(error_data,ref_data_name): + 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]') + +def plot_error_distributiln(error_data,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) + 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): + 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_title('2D Trajectory') + ax.legend(loc='upper right') + ax.grid() + ax.set_aspect('equal') + ax.axis('square') + +def plot_traj_text(title, ref_data, data, text, step, 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=16) + cnt = cnt + 1 + ax.set_xlabel('East [m]') + ax.set_ylabel('North [m]') + ax.set_title(title) + ax.legend(loc='upper right') + ax.grid() + ax.set_aspect('equal') + ax.axis('square') + +def plot_traj_three(raw_data, ref_data, data, 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_title('2D Trajectory') + ax.legend(loc='upper right') + ax.grid() + ax.set_aspect('equal') + ax.axis('square') + +def plot_traj_3d(ref_data, data, 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.legend(loc='upper right') + ax.grid() + +def plot_traj_3d_three(raw_data, ref_data, data, 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.legend(loc='upper right') + ax.grid() + +def plot_traj_qual(xyz, qual): + gnss_single = judge_qual(xyz,qual,1) + gnss_differential = judge_qual(xyz,qual,2) + gnss_fix = judge_qual(xyz,qual,4) + gnss_float = judge_qual(xyz,qual,5) + + fig = plt.figure() + ax_fix = fig.add_subplot(1, 1, 1) + ax_fix.set_title('GNSS positioning solution') + 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.legend(loc='upper right') + 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 new file mode 100644 index 00000000..217313ef --- /dev/null +++ b/eagleye_util/trajectory_plot/scripts/util/preprocess.py @@ -0,0 +1,478 @@ +# 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. + +# preprocess.py +# Author MapIV Hoda + +from typing import List +import pandas as pd +import numpy as np +import math +import yaml + +import util.calc as util_calc + +def set_ref_data(input_path,config): # Creation of dataset with reference to column number + use_separate_time_stamp = config["ref_index"]["separate_time_stamp"] + index_time_unit = config["ref_index"]["time_unit"] + use_quaternion_flag = config["ref_index"]["use_quaternion"] + use_radian_flag = config["ref_index"]["use_radian"] + use_vel_flag = config["ref_index"]["use_vel"] + use_angular_flag = config["ref_index"]["use_angular"] + use_qual_flag = config["ref_index"]["use_qual"] + index_time_sec = config["ref_index"]["time_sec"] + index_time_nsec = config["ref_index"]["time_nsec"] + index_time = config["ref_index"]["time"] + index_x = config["ref_index"]["x"] + index_y = config["ref_index"]["y"] + index_z = config["ref_index"]["z"] + index_ori_x = config["ref_index"]["ori_x"] + index_ori_y = config["ref_index"]["ori_y"] + index_ori_z = config["ref_index"]["ori_z"] + index_ori_w = config["ref_index"]["ori_w"] + index_roll = config["ref_index"]["roll"] + index_pitch = config["ref_index"]["pitch"] + index_yaw = config["ref_index"]["yaw"] + index_vel_x = config["ref_index"]["vel_x"] + index_vel_y = config["ref_index"]["vel_y"] + index_vel_z = config["ref_index"]["vel_z"] + index_angular_x = config["ref_index"]["angular_x"] + index_angular_y = config["ref_index"]["angular_y"] + index_angular_z = config["ref_index"]["angular_z"] + tmp_df = pd.read_csv(input_path,header=0, index_col=None) + if use_separate_time_stamp == True: + ref_data_time = pd.Series(tmp_df.iloc[:,index_time_sec] + tmp_df.iloc[:,index_time_nsec] * 10 ** (-9), name='TimeStamp') + elif index_time_unit == 1: + ref_data_time = pd.Series(tmp_df.iloc[:,index_time] * 10 ** (-9), name='TimeStamp') + else: + ref_data_time = pd.Series(tmp_df.iloc[:,index_time], name='TimeStamp') + ref_data_x = pd.Series(tmp_df.iloc[:,index_x], name='x') + ref_data_y = pd.Series(tmp_df.iloc[:,index_y], name='y') + ref_data_z = pd.Series(tmp_df.iloc[:,index_z], name='z') + ref_data_xyz = pd.concat([ref_data_x,ref_data_y,ref_data_z],axis=1) + if use_quaternion_flag == True: + ref_data_ori_x = pd.Series(tmp_df.iloc[:,index_ori_x], name='ori_x') + ref_data_ori_y = pd.Series(tmp_df.iloc[:,index_ori_y], name='ori_y') + ref_data_ori_z = pd.Series(tmp_df.iloc[:,index_ori_z], name='ori_z') + ref_data_ori_w = pd.Series(tmp_df.iloc[:,index_ori_w], name='ori_w') + ref_data_ori = pd.concat([ref_data_ori_x,ref_data_ori_y,ref_data_ori_z,ref_data_ori_w],axis=1) + ref_rpy = util_calc.quaternion_to_euler_zyx(ref_data_ori) + elif use_quaternion_flag == False and use_radian_flag == True: + ref_data_roll = pd.Series(tmp_df.iloc[:,index_roll], name='roll') + ref_data_pitch = pd.Series(tmp_df.iloc[:,index_pitch], name='pitch') + ref_data_yaw_tmp = pd.Series(tmp_df.iloc[:,index_yaw], name='yaw') + ref_data_yaw = util_calc.change_anglel_limit(ref_data_yaw_tmp) + ref_rpy = pd.concat([np.rad2deg(ref_data_roll),np.rad2deg(ref_data_pitch),np.rad2deg(ref_data_yaw)],axis=1) + elif use_quaternion_flag == False and use_radian_flag == False: + ref_data_roll = pd.Series(tmp_df.iloc[:,index_roll], name='roll') + ref_data_pitch = pd.Series(tmp_df.iloc[:,index_pitch], name='pitch') + ref_data_yaw_tmp = pd.Series(tmp_df.iloc[:,index_yaw], name='yaw') + ref_data_yaw = util_calc.change_anglel_limit(np.deg2rad(ref_data_yaw_tmp)) + ref_rpy = pd.concat([ref_data_roll,ref_data_pitch,np.rad2deg(ref_data_yaw)],axis=1) + + set_ref_vel = pd.DataFrame() + if use_vel_flag == True: + ref_data_vel_x = pd.Series(tmp_df.iloc[:,index_vel_x], name='vel_x') + ref_data_vel_y = pd.Series(tmp_df.iloc[:,index_vel_y], name='vel_y') + ref_data_vel_z = pd.Series(tmp_df.iloc[:,index_vel_z], name='vel_z') + set_ref_vel_tmp = pd.concat([ref_data_vel_x, ref_data_vel_y, ref_data_vel_z],axis=1) + ref_velocity = util_calc.calc_velocity(set_ref_vel_tmp) + distance = util_calc.calc_distance_vel(ref_velocity["velocity"],ref_data_time) + set_ref_vel = pd.concat([set_ref_vel_tmp, ref_velocity],axis=1) + else: + distance = util_calc.calc_distance_xy(ref_data_xyz) + set_ref_angular = pd.DataFrame() + if use_angular_flag == True: + ref_data_angular_x = pd.Series(tmp_df.iloc[:,index_angular_x], name='angular_x') + ref_data_angular_y = pd.Series(tmp_df.iloc[:,index_angular_y], name='angular_y') + ref_data_angular_z = pd.Series(tmp_df.iloc[:,index_angular_z], name='angular_z') + set_ref_angular = pd.concat([ref_data_angular_x, ref_data_angular_y, ref_data_angular_z],axis=1) + if use_qual_flag == True: + index_qual = config["ref_index"]["qual"] + ref_data_qual = pd.Series(tmp_df.iloc[:,index_qual], name='qual') + else: + index_qual = config["ref_index"]["qual"] + ref_data_qual = pd.Series(tmp_df.iloc[:,index_qual], name='qual_not_set') + set_ref_df = pd.concat([ref_data_time, ref_data_xyz, ref_rpy, set_ref_vel, set_ref_angular, distance, ref_data_qual],axis=1) + return set_ref_df + +def set_target_data(input_path,config): # Creation of dataset with reference to column number + use_separate_time_stamp = config["target_data_index"]["separate_time_stamp"] + index_time_unit = config["target_data_index"]["time_unit"] + use_qual_flag = config["target_data_index"]["use_qual"] + index_time_sec = config["target_data_index"]["time_sec"] + index_time_nsec = config["target_data_index"]["time_nsec"] + index_time = config["target_data_index"]["time"] + index_x = config["target_data_index"]["x"] + index_y = config["target_data_index"]["y"] + index_z = config["target_data_index"]["z"] + index_ori_x = config["target_data_index"]["ori_x"] + index_ori_y = config["target_data_index"]["ori_y"] + index_ori_z = config["target_data_index"]["ori_z"] + index_ori_w = config["target_data_index"]["ori_w"] + tmp_df = pd.read_csv(input_path,header=0, index_col=None) + if use_separate_time_stamp == True: + data_time = pd.Series(tmp_df.iloc[:,index_time_sec] + tmp_df.iloc[:,index_time_nsec] * 10 ** (-9), name='TimeStamp') + elif index_time_unit == 1: + data_time = pd.Series(tmp_df.iloc[:,index_time] * 10 ** (-9), name='TimeStamp') + else: + data_time = pd.Series(tmp_df.iloc[:,index_time], name='TimeStamp') + data_x = pd.Series(tmp_df.iloc[:,index_x], name='x') + data_y = pd.Series(tmp_df.iloc[:,index_y], name='y') + data_z = pd.Series(tmp_df.iloc[:,index_z], name='z') + data_ori_x = pd.Series(tmp_df.iloc[:,index_ori_x], name='ori_x') + data_ori_y = pd.Series(tmp_df.iloc[:,index_ori_y], name='ori_y') + data_ori_z = pd.Series(tmp_df.iloc[:,index_ori_z], name='ori_z') + data_ori_w = pd.Series(tmp_df.iloc[:,index_ori_w], name='ori_w') + if use_qual_flag == True: + index_qual = config["target_data_index"]["qual"] + data_qual = pd.Series(tmp_df.iloc[:,index_qual], name='qual') + else: + index_qual = config["target_data_index"]["qual"] + data_qual = pd.Series(tmp_df.iloc[:,index_qual], name='qual_not_set') + set_df = pd.concat([data_time, data_x, data_y, data_z, data_ori_x, data_ori_y, data_ori_z, data_ori_w, data_qual],axis=1) + return set_df + +def set_twist_data(input_path,config): # Creation of dataset with reference to column number + use_separate_time_stamp = config["twist_index"]["separate_time_stamp"] + index_time_unit = config["twist_index"]["time_unit"] + index_time_sec = config["twist_index"]["time_sec"] + index_time_nsec = config["twist_index"]["time_nsec"] + index_time = config["twist_index"]["time"] + index_linear_x = config["twist_index"]["linear_x"] + index_linear_y = config["twist_index"]["linear_y"] + index_linear_z = config["twist_index"]["linear_z"] + index_angular_x = config["twist_index"]["angular_x"] + index_angular_y = config["twist_index"]["angular_y"] + index_angular_z = config["twist_index"]["angular_z"] + tmp_df = pd.read_csv(input_path,header=0, index_col=None) + if use_separate_time_stamp == True: + data_time = pd.Series(tmp_df.iloc[:,index_time_sec] + tmp_df.iloc[:,index_time_nsec] * 10 ** (-9), name='TimeStamp') + elif index_time_unit == 1: + data_time = pd.Series(tmp_df.iloc[:,index_time] * 10 ** (-9), name='TimeStamp') + else: + data_time = pd.Series(tmp_df.iloc[:,index_time], name='TimeStamp') + data_linear_x = pd.Series(tmp_df.iloc[:,index_linear_x], name='velocity') + data_linear_y = pd.Series(tmp_df.iloc[:,index_linear_y], name='linear_y') + data_linear_z = pd.Series(tmp_df.iloc[:,index_linear_z], name='linear_z') + data_angular_x = pd.Series(tmp_df.iloc[:,index_angular_x], name='angular_x') + data_angular_y = pd.Series(tmp_df.iloc[:,index_angular_y], name='angular_y') + data_angular_z = pd.Series(tmp_df.iloc[:,index_angular_z], name='angular_z') + set_df = pd.concat([data_time, data_linear_x, data_linear_y, data_linear_z, data_angular_x, data_angular_y, data_angular_z],axis=1) + return set_df + +def set_log_df(input,plane,config): # Creation of dataset with reference to labels in df + df = pd.read_csv(input,delimiter=None, header='infer', index_col=None, usecols=None) + index_time_unit = config["eagleye_log"]["time_unit"] + tf_num = config["eagleye_log"]["tf_num"] + ros_reverse_imu = config["eagleye_log"]["ros_reverse_imu"] + eagleye_df = df[["timestamp", + "rtklib_nav.tow", + "velocity_scale_factor.scale_factor", + "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", + "rolling.rolling_angle", + "pitching.pitching_angle", + "heading_interpolate_1st.heading_angle", + "heading_interpolate_2nd.heading_angle", + "heading_interpolate_3rd.heading_angle", + "yawrate_offset_stop.yawrate_offset", + "yawrate_offset_2nd.yawrate_offset", + "slip_angle.slip_angle", + "enu_vel.vector.x", + "enu_vel.vector.y", + "enu_vel.vector.z", + "eagleye_pp_llh.latitude", + "eagleye_pp_llh.longitude", + "eagleye_pp_llh.altitude", + "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', + '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', + 'rolling.rolling_angle': 'roll_rad', + 'pitching.pitching_angle': 'pitch_rad', + 'heading_interpolate_1st.heading_angle': 'heading_1st', + 'heading_interpolate_2nd.heading_angle': 'heading_2nd', + 'heading_interpolate_3rd.heading_angle': 'yaw_rad', + 'yawrate_offset_stop.yawrate_offset': 'yawrate_offset_stop', + 'yawrate_offset_2nd.yawrate_offset': 'yawrate_offset', + 'slip_angle.slip_angle': 'slip', + 'enu_vel.vector.x': 'vel_x', + 'enu_vel.vector.y': 'vel_y', + 'enu_vel.vector.z': 'vel_z', + 'eagleye_pp_llh.latitude': 'latitude', + 'eagleye_pp_llh.longitude': 'longitude', + 'eagleye_pp_llh.altitude': 'altitude', + "imu.angular_velocity.x":'angular_x', + "imu.angular_velocity.y":'angular_y', + "imu.angular_velocity.z":'angular_z', + 'gga_llh.gps_qual': 'qual', + }) + + raw_df = df[["timestamp", + "rtklib_nav.tow", + "velocity.twist.linear.x", + "distance.distance", + "imu.angular_velocity.x", + "imu.angular_velocity.y", + "imu.angular_velocity.z", + "rtklib_nav.ecef_vel.x", + "rtklib_nav.ecef_vel.y", + "rtklib_nav.ecef_vel.z", + "rtklib_nav.status.latitude", + "rtklib_nav.status.longitude", + "rtklib_nav.status.altitude", + "gga_llh.latitude", + "gga_llh.longitude", + "gga_llh.altitude", + "gga_llh.gps_qual", + ]] + + raw_df = raw_df.rename(columns={'timestamp': 'TimeStamp_tmp', + 'rtklib_nav.tow': 'TOW', + 'velocity.twist.linear.x': 'velocity', + 'distance.distance': 'distance', + 'imu.angular_velocity.x': 'rollrate', + 'imu.angular_velocity.y': 'pitchrate', + 'imu.angular_velocity.z': 'yawrate', + 'rtklib_nav.ecef_vel.x': 'vel_x', + 'rtklib_nav.ecef_vel.y': 'vel_y', + 'rtklib_nav.ecef_vel.z': 'vel_z', + 'rtklib_nav.status.latitude': 'latitude', + 'rtklib_nav.status.longitude': 'longitude', + 'rtklib_nav.status.altitude': 'altitude', + 'gga_llh.latitude': 'rtk_latitude', + 'gga_llh.longitude': 'rtk_longitude', + 'gga_llh.altitude': 'rtk_altitude', + '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): + raw_df = raw_df.drop(raw_index) + else: + raw_df = raw_df.drop(rtk_index) + raw_df = raw_df.reset_index() + + if index_time_unit == 1: + eagleye_df['TimeStamp'] = eagleye_df['TimeStamp_tmp'] * 10 ** (-9) + raw_df['TimeStamp'] = raw_df['TimeStamp_tmp'] * 10 ** (-9) + elif index_time_unit == 0: + eagleye_df['TimeStamp'] = eagleye_df['TimeStamp_tmp'] + raw_df['TimeStamp'] = raw_df['TimeStamp_tmp'] + eagleye_df['elapsed_time'] = eagleye_df['TimeStamp'] - raw_df['TimeStamp'][0] + raw_df['elapsed_time'] = raw_df['TimeStamp'] - raw_df['TimeStamp'][0] + eagleye_df['yaw'] = np.rad2deg(util_calc.change_anglel_limit(eagleye_df['yaw_rad'])) + eagleye_df['roll'] = np.rad2deg(eagleye_df['roll_rad']) + eagleye_df['pitch'] = np.rad2deg(eagleye_df['pitch_rad']) + llh = pd.concat([eagleye_df['latitude'],eagleye_df['longitude'],eagleye_df['altitude']],axis=1) + if tf_num == 0: + xyz = latlon_to_19(llh,plane) + elif tf_num == 1: + xyz = util_calc.ll2mgrs(llh) + if ros_reverse_imu == True: + eagleye_df['angular_z'] = -1 * eagleye_df['angular_z'] + eagleye_df['yawrate_offset_stop'] = -1 * eagleye_df['yawrate_offset_stop'] + eagleye_df['yawrate_offset'] = -1 * eagleye_df['yawrate_offset'] + eagleye_df['slip'] = -1 * eagleye_df['slip'] + set_eagleye_df = pd.concat([eagleye_df, xyz],axis=1) + return set_eagleye_df, raw_df + +def set_tf_xy(xy,tf_x,tf_y): + xy['x'] = xy['x'] + tf_x + xy['y'] = xy['y'] + tf_y + return xy + +def correct_anntenapos(eagleye_df,ref_yaw,tf_across,tf_along,tf_height): + d = [[tf_across],[tf_along],[0]] + for i in range(len(ref_yaw)): + sinphi = math.sin(math.radians(ref_yaw[i])) + cosphi = math.cos(math.radians(ref_yaw[i])) + R = [[cosphi , sinphi , 0],[-sinphi , cosphi , 0],[0,0,0]] + diff = np.dot(R, d) + eagleye_df['x'][i] = eagleye_df['x'][i] + diff[0] + eagleye_df['y'][i] = eagleye_df['y'][i] + diff[1] + eagleye_df['z'] = eagleye_df['z'] + tf_height + return eagleye_df + +def latlon_to_19(llh,plane): + phi0_deg , lambda0_deg = plane_table(plane) + set_xy: List[float] = [] + for i in range(len(llh)): + phi_deg=llh['latitude'][i] + lambda_deg=llh['longitude'][i] + z=llh['altitude'][i] + """ Converts latitude and longitude to xy in plane rectangular coordinates + - input: + (phi_deg, lambda_deg): Latitude and longitude [degrees] to be converted + (note that these are decimal numbers, not minutes and seconds) + (phi0_deg, lambda0_deg): Latitude and longitude [degrees] of the origin of the rectangular coordinate system + (note that these are decimal degrees, not minutes and seconds) + - output: + x: Transformed plane rectangular coordinates[m] + y: Transformed plane rectangular coordinates[m] + """ + # Correcting the latitude-longitude and plane-rectangular coordinate system origin from degree to radian + phi_rad = np.deg2rad(phi_deg) + lambda_rad = np.deg2rad(lambda_deg) + phi0_rad = np.deg2rad(phi0_deg) + lambda0_rad = np.deg2rad(lambda0_deg) + + # Auxiliary function + def A_array(n): + A0 = 1 + (n**2)/4. + (n**4)/64. + A1 = - (3./2)*( n - (n**3)/8. - (n**5)/64. ) + A2 = (15./16)*( n**2 - (n**4)/4. ) + A3 = - (35./48)*( n**3 - (5./16)*(n**5) ) + A4 = (315./512)*( n**4 ) + A5 = -(693./1280)*( n**5 ) + return np.array([A0, A1, A2, A3, A4, A5]) + + def alpha_array(n): + a0 = np.nan # dummy + a1 = (1./2)*n - (2./3)*(n**2) + (5./16)*(n**3) + (41./180)*(n**4) - (127./288)*(n**5) + a2 = (13./48)*(n**2) - (3./5)*(n**3) + (557./1440)*(n**4) + (281./630)*(n**5) + a3 = (61./240)*(n**3) - (103./140)*(n**4) + (15061./26880)*(n**5) + a4 = (49561./161280)*(n**4) - (179./168)*(n**5) + a5 = (34729./80640)*(n**5) + return np.array([a0, a1, a2, a3, a4, a5]) + + # Constant (a, F: World Geodetic System - Geodetic Reference System 1980 (GRS80) Ellipsoid) + m0 = 0.9999 + a = 6378137. + F = 298.257222101 + + # (1), Calculation of n, A_i, alpha_i + n = 1. / (2*F - 1) + A_array = A_array(n) + alpha_array = alpha_array(n) + + # (2), Calculation of S, A + A_ = ( (m0*a)/(1.+n) )*A_array[0] # [m] + S_ = ( (m0*a)/(1.+n) )*( A_array[0]*phi0_rad + np.dot(A_array[1:], np.sin(2*phi0_rad*np.arange(1,6))) ) # [m] + + # (3) Calculation of lambda_c, lambda_s + lambda_c = np.cos(lambda_rad - lambda0_rad) + lambda_s = np.sin(lambda_rad - lambda0_rad) + + # (4)Calculation of t, t_ + t = np.sinh( np.arctanh(np.sin(phi_rad)) - ((2*np.sqrt(n)) / (1+n))*np.arctanh(((2*np.sqrt(n)) / (1+n)) * np.sin(phi_rad)) ) + t_ = np.sqrt(1 + t*t) + + # (5) Calculation of xi', eta' + xi2 = np.arctan(t / lambda_c) # [rad] + eta2 = np.arctanh(lambda_s / t_) + + # (6) Calculation of x, y + x = A_ * (xi2 + np.sum(np.multiply(alpha_array[1:], + np.multiply(np.sin(2*xi2*np.arange(1,6)), + np.cosh(2*eta2*np.arange(1,6)))))) - S_ # [m] + y = A_ * (eta2 + np.sum(np.multiply(alpha_array[1:], + np.multiply(np.cos(2*xi2*np.arange(1,6)), + np.sinh(2*eta2*np.arange(1,6)))))) # [m] + set_xy.append([y , x , z]) + xyz = pd.DataFrame(set_xy,columns=['x','y','z']) + return xyz + +def plane_table(plane): + if plane == 1: + phi0_deg = 33 + lambda0_deg = 129+30./60 + if plane == 2: + phi0_deg = 33 + lambda0_deg = 131 + if plane == 3: + phi0_deg = 36 + lambda0_deg = 132+10./60 + if plane == 4: + phi0_deg = 33 + lambda0_deg = 133+30./60 + if plane == 5: + phi0_deg = 36 + lambda0_deg = 134+20./60 + if plane == 6: + phi0_deg = 36 + lambda0_deg = 136 + if plane == 7: + phi0_deg = 36 + lambda0_deg = 137+10./60 + if plane == 8: + phi0_deg = 36 + lambda0_deg = 138+30./60 + if plane == 9: + phi0_deg = 36 + lambda0_deg = 139+50./60 + if plane == 10: + phi0_deg = 40 + lambda0_deg = 140+50./60 + if plane == 11: + phi0_deg = 44 + lambda0_deg = 140+15./60 + if plane == 12: + phi0_deg = 44 + lambda0_deg = 142+15./60 + if plane == 13: + phi0_deg = 44 + lambda0_deg = 144+15./60 + if plane == 14: + phi0_deg = 26 + lambda0_deg = 142 + if plane == 15: + phi0_deg = 26 + lambda0_deg = 127+30./60 + if plane == 16: + phi0_deg = 26 + lambda0_deg = 124+30./60 + if plane == 17: + phi0_deg = 26 + lambda0_deg = 131 + if plane == 18: + phi0_deg = 26 + lambda0_deg = 136 + if plane == 19: + phi0_deg = 26 + lambda0_deg = 154 + return phi0_deg , lambda0_deg \ No newline at end of file