Develop ros1 (#149)

* Feature/illegal value diag (#122)

* Reformat eag;eue_core/navigation

* error in diag if velocity scale factor is too small or too large

* error in diag if corrected yawrate is too small or too large

* Update README.md

* Feature/kml generator (#124)

* Add kml_generator

* Reformat

* Update kml_generator

* Add color setting in kml_generator

* Add other info in eagleye.kml

* Install sibmodule in CI

* Update CI

* Update CI

* Update noetic CI

* Update README.md

Add recursive option when git clone

* Fix/reverse pitch (#125)

* Changed NSec to Sec

* Removed comma at end of line

* Fixed reversed pitch calculation

* Feature/ublox msgs (#127)

* Update CONTRIBUTORS

* Add CONTRIBUTING.md

* Support ublox_msgs/NavPVT

* Modify fix2kml for color option

* Feature/covariance output (#128)

* Fix incorrect names

* Changed NSec to Sec in eagleye_log.csv

* Reformat fix2pose.cpp

* Publish /eagleye/pose_with_covariance

* Reformat eagleye_pp_output.cpp

* Add covariance output in eagleye.csv

* Add covariance information to the position when using the receiver's positioning results as is.

* Update eagleye_pp/README.md

* Fix bug in fix2pose.cpp

* Fix submodule (#132)

* Fix submodule

* Update .gitmodules

* Refactoring kml_genarator (#134)

* Feature/logging node (#135)

* Rename global vriable

* Add log output function in monitor_node

* Fix output csv in monitor_node

* Fix yaml

* Update README.md

* Feature/converted imu (#133)

* bug in doTransform

* Fix  bug in doTransform

* Refactor tf_converted_imu.cpp

* Delete reverse_imu

* Fix name

* Apply imu transform also in eagleye_pp

* Fix core dump when pp is run with forward only (#136)

* Update RTKLIB version

* Fix/csv output (#137)

* Fix csv output

* Remove unused functions and variables

* Fixed forgetting to getParam.

* Fixing a mistake in gga status in rtk_heading.cpp

* Fixed forgetting semicolon in monitor_node.cpp

* Feature/eagleye evaluation (#131)

* Support for eagleye single evaluation

* Support for comparison with references

* Create util.py file

* Refactor eagleye_pp_single_evaluation.py

* Fix evaluation_plot.py

* Separate calc.py into calc.py and preprocess.py

* Translate Japanese into English

* Fixed a bug caused by the inclusion of 0 in llh

* Added description of evaluation_plot to README

* Change from enu coordinate system to plane Cartesian coordinate system

* Correspond to geometry_msgs/PoseStamped type, and fix a bug that occurs when there is no GNSS quality.

* Add setup instructions to README

* Fixed a bug related to the antenna correction function.

* Rename dataframe of reference data

* Support for yaw offset correction

* Support for reference use of eagleye_log

* Add error evaluation

* Corrected graph of cumulative error distribution

* Support for column references in yaml

Co-authored-by: Ryohei Sasaki <[email protected]>

* Fix/south and west support (#139)

* Fix nmea2fox_core.cpp

* Fix csv output

* Feature/can less (#138)

* Change Message Type

* Support for message changes

* add velocity estimation node

* change msgs types

* resolve conflict

* Resolve core dumping issues in smoothingTrajectory

* canless mode off by default

* Change input settings in canless mode

* Fix errors in topic search in canless mode

* Remove unused codes in eagleye_pp

Co-authored-by: Ryohei Sasaki <[email protected]>
Co-authored-by: Ryohei Sasaki <[email protected]>

* Feature/mgrs evaluation (#140)

* Support for mgrs and time unit

* Support for twist evaluation

* Fixed twist_evaluation

* Fixed calc dr

* Fixed import data

* Add output list to README

* Fixed bug in change_angle_limit

* Support for csv output

* Fix/import dataframe error (#142)

* support for canless output csv

* Fix problem with incorrect value at the beginning

* Fix realtime canless error (#143)

* Update README.md

Co-authored-by: Tomoya Sato <[email protected]>
Co-authored-by: YutaHoda <[email protected]>
Co-authored-by: Aoki-Takanose <[email protected]>

.github/workflows/build.yml

@@ -16,8 +16,9 @@ jobs:
 
     steps:
       - name: checkout
-        uses: actions/checkout@v2
-
+        uses: actions/checkout@v1
+        with:
+          submodules: true
       - name: rosdep update
         run: |
           apt-get update
@@ -54,7 +55,9 @@ jobs:
 
     steps:
       - name: checkout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v1
+        with:
+          submodules: true
 
       - name: rosdep update
         run: |

.gitmodules

@@ -0,0 +1,7 @@
+[submodule "eagleye_util/kml_generator"]
+	path = eagleye_util/kml_generator
+	url = https://github.com/MapIV/kml_generator
+[submodule "eagleye_pp/common/multi_rosbag_controller"]
+	path = eagleye_pp/common/multi_rosbag_controller
+	url = https://github.com/MapIV/multi_rosbag_controller.git
+

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/<feature_name>  
+Branches for new features and improvements
+- fix/<bug_name>  
+Branch for fixing non-hotfix bugs
+- hotfix/<bug_name>  
+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)

CONTRIBUTORS

@@ -1,7 +1,9 @@
-Aoki TAKANOSE - Meijo University/Map IV, Inc.
-Osamu SEKINO - KAIT/Map IV, Inc.
+Aoki TAKANOSE - Nagoya University/Map IV, Inc.
+Osamu SEKINO - Map IV, Inc.
 Junichi MEGURO - Meijo University
 Hideo INOUE - KAIT
 Shunsuke MIZUTANI - Meijo University
 Takuya ARAKAWA - Meijo University
 Yuki KITSUKAWA - Map IV, Inc.
+Ryohei SASAKI - Map IV, Inc.
+Yuta HODA - Meijo University/Map IV, Inc.

README.md

@@ -40,21 +40,22 @@ Clone and Build MapIV's fork of [RTKLIB](https://github.com/MapIV/RTKLIB/tree/rt
 
 	sudo apt-get install gfortran
 	cd $HOME
-	git clone -b rtklib_ros_bridge https://github.com/MapIV/RTKLIB.git
+	git clone -b rtklib_ros_bridge_b34 https://github.com/MapIV/RTKLIB.git
 	cd $HOME/RTKLIB/lib/iers/gcc/
 	make
-	cd $HOME/RTKLIB/app
+	cd $HOME/RTKLIB/app/consapp
 	make 
 
 ### ROS Packages
 
 Clone and build the necessary packages for Eagleye. 
 
 	cd $HOME/catkin_ws/src
-	git clone https://github.com/MapIV/eagleye.git
+	git clone --recursive https://github.com/MapIV/eagleye.git
 	git clone https://github.com/MapIV/rtklib_ros_bridge.git
 	git clone https://github.com/MapIV/nmea_comms.git
 	git clone https://github.com/MapIV/nmea_ros_bridge.git
+	cd ..
 	rosdep install --from-paths src --ignore-src -r -y
 	catkin_make -DCMAKE_BUILD_TYPE=Release
 
@@ -72,9 +73,9 @@ Clone and build the necessary packages for Eagleye.
 
 * Output rate 50Hz
 
-2. Check the rotation direction of z axis of IMU being used. If you look from the top of the vehicle, if the left turn is positive, set "reverse_imu" to `true` in `eagleye/eagleye_rt/config/eagleye_config.yaml`.
+2. Check the rotation direction of z axis of IMU being used. If you look from the top of the vehicle, if the left turn is positive, set `pitch` to `3.14159` in `eagleye/eagleye_util/tf/config/sensors_tf.yaml`.
 
-		 reverse_imu: true
+		 pitch: 3.14159
 
 ## Start each sensor driver
 
@@ -110,7 +111,7 @@ Clone and build the necessary packages for Eagleye.
 
 		roslaunch nmea_comms f9p_nmea_sentence.launch
 
-or
+   or
 
 		roslaunch nmea_ros_bridge nmea_udp.launch
 
@@ -134,6 +135,9 @@ or
 The 3D maps (point cloud and vector data) of the route is also available from [Autoware sample data](https://drive.google.com/file/d/1Uwp9vwvcZwaoZi4kdjJaY55-LEXIzSxf/view).
 
 ## Research Papers for Citation
+
+1. A. Takanose, et., al., "Eagleye: A Lane-Level Localization Using Low-Cost GNSS/IMU", Intelligent Vehicles (IV) workshop, 2021 [Link](https://www.autoware.org/iv2021video-workshoppapers3)
+
 1. J Meguro, T Arakawa, S Mizutani, A Takanose, "Low-cost Lane-level Positioning in Urban Area Using Optimized Long Time Series GNSS and IMU Data", International Conference on Intelligent Transportation Systems(ITSC), 2018 [Link](https://www.researchgate.net/publication/329619280_Low-cost_Lane-level_Positioning_in_Urban_Area_Using_Optimized_Long_Time_Series_GNSS_and_IMU_Data)
 
 1. Takeyama Kojiro, Kojima Yoshiko, Meguro Jun-ichi, Iwase Tatsuya, Teramoto Eiji, "Trajectory Estimation Based on Tightly Coupled Integration of GPS Doppler and INS" -Improvement of Trajectory Estimation in Urban Area-, Transactions of Society of Automotive Engineers of Japan   44(1) 199-204, 2013 [Link](https://www.jstage.jst.go.jp/article/jsaeronbun/44/1/44_20134048/_article/-char/en)
@@ -146,8 +150,6 @@ The 3D maps (point cloud and vector data) of the route is also available from [A
 
 1. Yoshiko Kojima, et., al., "Precise Localization using Tightly Coupled Integration based on Trajectory estimated from GPS Doppler", International Symposium on Advanced Vehicle Control(AVEC), 2010 [Link](https://ci.nii.ac.jp/naid/10029931657/)
 
-1. A. Takanose, et., al., "Eagleye: A Lane-Level Localization Using Low-Cost GNSS/IMU", Intelligent Vehicles (IV) workshop, 2021 [Link](https://www.autoware.org/iv2021video-workshoppapers3)
-
 ## License
 Eagleye is provided under the [BSD 3-Clause](https://github.com/MapIV/eagleye/blob/master/LICENSE) License.
 

eagleye_core/coordinate/CMakeLists.txt

@@ -36,6 +36,7 @@ link_directories(
 add_library(coordinate
   src/ecef2llh.cpp
   src/enu2llh.cpp
+  src/enu2xyz_vel.cpp
   src/ll2xy.cpp
   src/llh2xyz.cpp
   src/xyz2enu.cpp

eagleye_core/coordinate/include/coordinate/coordinate.hpp

@@ -41,7 +41,7 @@ class ConvertHeight
   double convert2ellipsoid();
   double getGeoidPerMinute();
   double getGeoidPerDegree();
-  void setLLH(double,double,double);
+  void setLLH(double, double, double);
 
 private:
   double _latitude;
@@ -56,11 +56,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();
 
 

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 "coordinate/coordinate.hpp"
+#include <GeographicLib/Geocentric.hpp>
+#include <eigen3/Eigen/StdVector>
+
+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<double> 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<Eigen::Vector3d> v_xyz(xyz_vel);
+  v_xyz = R.transpose() * v_enu;
+}

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;
 }

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])));

eagleye_core/navigation/CMakeLists.txt

@@ -1,6 +1,9 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(eagleye_navigation)
 
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
+
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
@@ -20,6 +23,7 @@ include_directories(
   include
   ${PROJECT_SOURCE_DIR}/include
   ${catkin_INCLUDE_DIRS}
+  ${YAML_CPP_INCLUDE_DIRS}
 )
 
 add_library(navigation
@@ -41,10 +45,12 @@ add_library(navigation
   src/rtk_heading.cpp
   src/enable_additional_rolling.cpp
   src/rolling.cpp
+  src/velocity_estimator.cpp
 )
 
 target_link_libraries(navigation
   ${catkin_LIBRARIES}
+  ${YAML_CPP_LIBRARIES}
 )
 add_dependencies(navigation ${catkin_EXPORTED_TARGETS})