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 <[email protected]> * 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 <[email protected]> Co-authored-by: YutaHoda <[email protected]>
MapIV · Jul 11, 2022 · 951defe · 951defe
1 parent 4d38e16
commit 951defe
Show file tree

Hide file tree

Showing 77 changed files with 4,953 additions and 902 deletions.
diff --git 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
diff --git a/.gitignore b/.gitignore
@@ -0,0 +1 @@
+.vscode/
diff --git a/CONTRIBUTING.md 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/<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)
diff --git 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
@@ -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
@@ -28,7 +28,6 @@
 
 #include <cmath>
 #include <geodesy/utm.h>
-//#include "geographic_msgs/GeoPoint.h"
 #include "geographic_msgs/msg/geo_point.hpp"
 #include <GeographicLib/Geoid.hpp>
 #include <GeographicLib/MGRS.hpp>
@@ -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
@@ -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 <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;
+}
diff --git a/eagleye_core/coordinate/src/geoid_per_minute.cpp b/eagleye_core/coordinate/src/geoid_per_minute.cpp
@@ -35,7 +35,6 @@
 
 #include <string>
 #include <fstream>
-//#include <ros/package.h>
 #include <ament_index_cpp/get_package_share_directory.hpp>
 
 
@@ -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
@@ -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
@@ -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
@@ -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
 )