Tested latest merge from upstream/ros2 in the simulator (#1)

* Changelogs

* 3.4.0

* Adding issue templates

* Update issue templates

* install headers (cra-ros-pkg#786)

* bump Humble to 3.4.1 for release (cra-ros-pkg#789)

* bumping to 3.4.2 for humble release (cra-ros-pkg#806)

* bumping humble to 3.5.1 to override incorrect binaries (cra-ros-pkg#816)

* Utm using geographiclib humble branch (cra-ros-pkg#834)

* Add single test for navsat_conversions

* Add a southern point to the navsat_transform test

* LLtoUTM using GeographicLib

* Use GeographicLib for UTMtoLL conversions

* Linting

* Forgot include

* Fix compilation

* Calculate gamma because it's a function output and was supplied before

* Also test for gamma conversion

* Align naming and install

* Utm using geographiclib ros2 branch (cra-ros-pkg#833)

* Add single test for navsat_conversions

* Add a southern point to the navsat_transform test

* LLtoUTM using GeographicLib

* Use GeographicLib for UTMtoLL conversions

* Linting

* Forgot include

* Fix compilation

* Calculate gamma because it's a function output and was supplied before

* Also test for gamma conversion

* Align naming and install

* Test navsat transform functionality (cra-ros-pkg#838)

* Wait for odometry message before setting manual datum so that the base and world frame names can be set. (cra-ros-pkg#835)

* wait for odom msg before setting manual datum

* Wait for odometry message before setting manual datum so that the base and world frame names can be set. (cra-ros-pkg#836)

* wait for odom msg before setting manual datum

* fix header timestamp (cra-ros-pkg#852)

Co-authored-by: Luke Chang <[email protected]>

* fix header timestamp (cra-ros-pkg#852)

Co-authored-by: Luke Chang <[email protected]>

* Changelogs

* 3.5.2

* Fixing angle clamping for humble (cra-ros-pkg#854)

* fix: modify dual_ekf_navsat_example.launch file to remap the correct imu topic (cra-ros-pkg#857)

* Feature/set utm service (cra-ros-pkg#856)

* Forward port Fix/set utm map frame change

---------

Signed-off-by: Tim Clephas <[email protected]>

* simplified calculation

* simplified calculation

* Update issue templates

* Migrate static tfs to ros2 format. (cra-ros-pkg#864)

* Fix throttle duration (cra-ros-pkg#866)

* Fix throttle duration

* Update ukf.yaml to match ekf.yaml (cra-ros-pkg#867)

Add missing *_pose_use_child_frame parameter.

* updated file formatting

---------

Signed-off-by: Tim Clephas <[email protected]>
Co-authored-by: Tom Moore <[email protected]>
Co-authored-by: Olivier Kermorgant <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
Co-authored-by: Tim Clephas <[email protected]>
Co-authored-by: Tom Greier <[email protected]>
Co-authored-by: Luke Chang <[email protected]>
Co-authored-by: Luke Chang <[email protected]>
Co-authored-by: joeldushouyu <[email protected]>
Co-authored-by: Mukunda Bharatheesha <[email protected]>
Co-authored-by: thandal <[email protected]>

.github/ISSUE_TEMPLATE/bug_report.md

@@ -0,0 +1,35 @@
+---
+name: Bug report
+about: Create a bug report to fix _only_ critical issues in robot_localization
+title: ''
+labels: ''
+assignees: ayrton04
+
+---
+
+# ALL QUESTIONS SHOULD BE DIRECTED TO robotics.stackexchange.com
+
+## When asking a question on robotics.stackexchange.com, please include your full EKF config and one sample message from each sensor input**
+
+# THIS PACKAGE WILL SOON BE DEPRECATED. UNLESS SEVERE, BUG REPORTS ARE UNLIKELY TO BE ADDRESSED
+
+**Describe the bug**
+A clear and concise description of what the bug is.
+
+**To Reproduce**
+Steps to reproduce the behavior:
+1. 
+
+**Expected behavior**
+A clear and concise description of what you expected to happen.
+
+**Screenshots**
+If applicable, add screenshots to help explain your problem.
+
+**Desktop (please complete the following information):**
+ - OS:
+ - ROS Distribution:
+ - `robot_localization` Package Version:
+
+**Additional context**
+Add any other context about the problem here.

.github/ISSUE_TEMPLATE/feature_request.md

@@ -0,0 +1,14 @@
+---
+name: Feature request
+about: FEATURE REQUESTS ARE NOT BEING ACCEPTED
+title: ''
+labels: ''
+assignees: ''
+
+---
+
+# ALL QUESTIONS SHOULD BE DIRECTED TO robotics.stackexchange.com
+
+## When asking a question on robotics.stackexchange.com, please include your full EKF config and one sample message from each sensor input**
+
+# THIS PACKAGE WILL SOON BE DEPRECATED. FEATURE REQUESTS WILL NOT BE ADDRESSED.

.travis.yml

@@ -40,4 +40,4 @@ branches:
     - ros2
 
 notifications:
-  email: false
+email: false

CMakeLists.txt

@@ -45,6 +45,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/GetState.srv"
   "srv/SetDatum.srv"
   "srv/SetPose.srv"
+  "srv/SetUTMZone.srv"
   "srv/ToggleFilterProcessing.srv"
   "srv/ToLL.srv"
   DEPENDENCIES
@@ -305,6 +306,10 @@ if(BUILD_TESTING)
     WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   )
 
+  #### NAVSAT CONVERSION TESTS ####
+  #ament_add_gtest(test_navsat_conversions test/test_navsat_conversions.cpp)
+  #target_link_libraries(test_navsat_conversions ${library_name})
+
   ament_add_gtest_executable(test_ros_robot_localization_listener
                   test/test_ros_robot_localization_listener.cpp)
   target_link_libraries(test_ros_robot_localization_listener ${library_name})
@@ -339,6 +344,7 @@ if(BUILD_TESTING)
     #test_ekf_localization_node_bag2
     #test_ekf_localization_node_bag3
     test_robot_localization_estimator
+    #test_navsat_conversions
     test_ros_robot_localization_listener
     test_ros_robot_localization_listener_publisher
     #test_ukf_localization_node_bag1

include/robot_localization/filter_utilities.hpp

@@ -32,6 +32,12 @@
 #ifndef ROBOT_LOCALIZATION__FILTER_UTILITIES_HPP_
 #define ROBOT_LOCALIZATION__FILTER_UTILITIES_HPP_
 
+#include <angles/angles.h>
+#include <Eigen/Dense>
+#include <rclcpp/duration.hpp>
+#include <rclcpp/time.hpp>
+#include <std_msgs/msg/header.hpp>
+
 #include <iostream>
 #include <ostream>
 #include <string>
@@ -57,6 +63,16 @@ namespace robot_localization
 namespace filter_utilities
 {
 
+/**
+ * @brief Utility method keeping RPY angles in the range [-pi, pi]
+ * @param[in] rotation - The rotation to bind
+ * @return the bounded value
+ */
+inline double clampRotation(double rotation)
+{
+  return angles::normalize_angle(rotation);
+}
+
 /**
  * @brief Utility method for appending tf2 prefixes cleanly
  * @param[in] tf_prefix - the tf2 prefix to append

include/robot_localization/navsat_transform.hpp

@@ -37,11 +37,14 @@
 
 #include "Eigen/Dense"
 #include "GeographicLib/LocalCartesian.hpp"
+#include "GeographicLib/MGRS.hpp"
+#include "GeographicLib/UTMUPS.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/timer.hpp"
 #include "robot_localization/srv/from_ll.hpp"
 #include "robot_localization/srv/set_datum.hpp"
+#include "robot_localization/srv/set_utm_zone.hpp"
 #include "robot_localization/srv/to_ll.hpp"
 #include "sensor_msgs/msg/imu.hpp"
 #include "sensor_msgs/msg/nav_sat_fix.hpp"
@@ -77,15 +80,15 @@ class NavSatTransform : public rclcpp::Node
   /**
    * @brief Computes the transform from the Cartesian frame to the odom frame
    */
-  void computeTransform();
+  bool computeTransform();
 
   /**
    * @brief Callback for the datum service
    */
   bool datumCallback(
-    const std::shared_ptr<robot_localization::srv::SetDatum::Request>
+    const robot_localization::srv::SetDatum::Request::SharedPtr
     request,
-    std::shared_ptr<robot_localization::srv::SetDatum::Response>);
+    robot_localization::srv::SetDatum::Response::SharedPtr);
 
   //! @brief Callback for the to Lat Long service
   //!
@@ -99,6 +102,23 @@ class NavSatTransform : public rclcpp::Node
     const std::shared_ptr<robot_localization::srv::FromLL::Request> request,
     std::shared_ptr<robot_localization::srv::FromLL::Response> response);
 
+  /**
+   * @brief Callback for the UTM zone service
+  */
+  bool setUTMZoneCallback(
+    const std::shared_ptr<robot_localization::srv::SetUTMZone::Request> request,
+    std::shared_ptr<robot_localization::srv::SetUTMZone::Response>);
+
+  /**
+   * @brief Given the pose of the navsat sensor in the Cartesian frame, removes the
+   * offset from the vehicle's centroid and returns the Cartesian-frame pose of said
+   * centroid.
+   */
+  void getRobotOriginCartesianPose(
+    const tf2::Transform & gps_cartesian_pose,
+    tf2::Transform & robot_cartesian_pose,
+    const rclcpp::Time & transform_time);
+
   /**
    * @brief Given the pose of the navsat sensor in the world frame, removes the
    * offset from the vehicle's centroid and returns the world-frame pose of said
@@ -167,6 +187,11 @@ class NavSatTransform : public rclcpp::Node
     const tf2::Vector3 & point, double & latitude, double & longitude,
     double & altitude) const;
 
+  /**
+   * @brief Sets the manual datum pose to be used by the transform computation
+   */
+  void setManualDatum();
+
   /**
    * @brief Frame ID of the robot's body frame
    *
@@ -206,6 +231,11 @@ class NavSatTransform : public rclcpp::Node
    */
   rclcpp::Service<robot_localization::srv::FromLL>::SharedPtr from_ll_srv_;
 
+  /**
+   * @brief Service for set UTM zone
+  */
+  rclcpp::Service<robot_localization::srv::SetUTMZone>::SharedPtr set_utm_zone_srv_;
+
   /**
    * @brief Navsatfix publisher
    */
@@ -262,11 +292,6 @@ class NavSatTransform : public rclcpp::Node
    */
   rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
 
-  /**
-   * @brief whether the pose is in northern hemisphere
-   */
-  bool is_northern_hemis_;
-
   /**
    * @brief Covariance for most recent odometry data
    */
@@ -371,8 +396,12 @@ class NavSatTransform : public rclcpp::Node
   bool use_local_cartesian_;
 
   /**
-   * @brief Local Cartesian projection around gps origin
+   * @brief Whether we want to force the user's UTM zone and not rely on current GPS data for determining it
    */
+  bool force_user_utm_;
+
+  //! @brief Local Cartesian projection around gps origin
+  //!
   GeographicLib::LocalCartesian gps_local_cartesian_;
 
   /**
@@ -426,9 +455,14 @@ class NavSatTransform : public rclcpp::Node
   tf2::Transform transform_enu_to_world_;
 
   /**
-   * @brief Cartesian zone as determined after transforming GPS message
+   * @brief @brief the UTM zone (zero means UPS)
    */
-  int utm_zone_id_;
+  int utm_zone_;
+
+  /**
+   * @brief hemisphere (true means north, false means south)
+  */
+  bool northp_;
 
   /**
    * @brief Frame ID of the GPS odometry output
@@ -453,6 +487,15 @@ class NavSatTransform : public rclcpp::Node
    * converted GPS odometry message.
    */
   bool zero_altitude_;
+
+  /**
+   * @brief Manual datum pose to be used by the transform computation
+   *
+   * Then manual datum requested by a service request (or configuration) is stored
+   * here until the odom message is received, and the manual datum pose can be
+   * set.
+   */
+  geographic_msgs::msg::GeoPose manual_datum_geopose_;
 };
 
 }  // namespace robot_localization

launch/dual_ekf_navsat_example.launch.py

@@ -56,7 +56,7 @@ def generate_launch_description():
             name='navsat_transform',
 	        output='screen',
             parameters=[parameters_file_path],
-            remappings=[('imu/data', 'imu/data'),
+            remappings=[('imu', 'imu/data'),
                         ('gps/fix', 'gps/fix'), 
                         ('gps/filtered', 'gps/filtered'),
                         ('odometry/gps', 'odometry/gps'),

params/ukf.yaml

@@ -110,6 +110,13 @@ ukf_filter_node:
         # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
         # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
         odom0_relative: false
+
+        # [ADVANCED] Whether to use the starting pose of child_frame_id as the origin of odometry.
+        # Note: this is different from setting odom0_relative to true, as when child_frame is different from 
+        # base_link_frame, the rotation of base_link will be coupled into the translation of child_frame.
+        # Set to true for fusing secondary odometry sources that are rigidly connected to base_link but has a non-zero 
+        # offset from base_link.
+        odom0_pose_use_child_frame: false
 
         # [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
         # control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to