From d448fd565d9f314a3d66ebae05e93f38774aba4e Mon Sep 17 00:00:00 2001
From: Roland Arsenault <roland@ccom.unh.edu>
Date: Wed, 14 Feb 2024 11:20:53 -0500
Subject: [PATCH] Update tf2 utils to support timestamps

---
 include/project11/gz4d_geo.h  |   9 ---
 include/project11/pid.h       |   2 +-
 include/project11/tf2_utils.h |  17 ++++--
 include/project11/utils.h     |  22 +++----
 launch/robot_core.launch      | 111 ++++++++++++++++++++--------------
 5 files changed, 88 insertions(+), 73 deletions(-)

diff --git a/include/project11/gz4d_geo.h b/include/project11/gz4d_geo.h
index 491b3e4..baea1ff 100644
--- a/include/project11/gz4d_geo.h
+++ b/include/project11/gz4d_geo.h
@@ -51,15 +51,6 @@ namespace gz4d
         return !(i%2);
     }
 
-//     inline bool IsTrue(std::string const &s)
-//     {
-//         std::string l = boost::algorithm::to_lower_copy(boost::algorithm::trim_copy(s));
-//         return (l == "true" || l == "yes" || l == "on" || l == "1" || l == "t" || l == "y");
-//     }
-    
-//    template<typename T> inline T Nan(){return std::numeric_limits<T>::quiet_NaN();}
-//    template<typename T> inline bool IsNan(T value){return (boost::math::isnan)(value);}
-
     /// Used by std::shared_ptr's to hold pointers it shouldn't auto-delete.
     struct NullDeleter
     {
diff --git a/include/project11/pid.h b/include/project11/pid.h
index 991547a..87e64f9 100644
--- a/include/project11/pid.h
+++ b/include/project11/pid.h
@@ -130,7 +130,7 @@ class PID
     Ki_ = msg->Ki;
     Kd_ = msg->Kd;
     windup_limit_ = msg->windup_limit;
-    upper_limit_ = msg->windup_limit;
+    upper_limit_ = msg->upper_limit;
     lower_limit_ = msg->lower_limit;
   }
 
diff --git a/include/project11/tf2_utils.h b/include/project11/tf2_utils.h
index e686495..fcce9b4 100644
--- a/include/project11/tf2_utils.h
+++ b/include/project11/tf2_utils.h
@@ -22,23 +22,28 @@ namespace project11
         }
       }
       
-      bool canTransform(std::string map_frame="map")
+      bool canTransform(std::string map_frame="map", ros::Time target_time = ros::Time(0), ros::Duration timeout = ros::Duration(0.5))
       {
-        return buffer()->canTransform("earth", map_frame, ros::Time(0), ros::Duration(0.5));
+        return buffer()->canTransform("earth", map_frame, target_time, timeout);
       }
 
-      geometry_msgs::Point wgs84_to_map(geographic_msgs::GeoPoint const &position, std::string map_frame="map")
+      geometry_msgs::Point wgs84_to_map(geographic_msgs::GeoPoint const &position, std::string map_frame="map", ros::Time target_time = ros::Time(0))
       {
         geometry_msgs::Point ret;
         try
         {
-          geometry_msgs::TransformStamped t = buffer()->lookupTransform(map_frame,"earth",ros::Time(0));
+          geometry_msgs::TransformStamped t = buffer()->lookupTransform(map_frame,"earth",target_time);
           LatLongDegrees p_ll;
           fromMsg(position, p_ll);
+          bool alt_is_nan = std::isnan(position.altitude);
+          if(alt_is_nan)
+            p_ll.altitude() = 0.0;
           ECEF p_ecef = p_ll;
           geometry_msgs::Point in;
           toMsg(p_ecef, in);
           tf2::doTransform(in,ret,t);
+          if(alt_is_nan)
+            ret.z = std::nan("");
         }
         catch (tf2::TransformException &ex)
         {
@@ -47,12 +52,12 @@ namespace project11
         return ret;
       }
       
-      geographic_msgs::GeoPoint map_to_wgs84(geometry_msgs::Point const &point, std::string map_frame="map")
+      geographic_msgs::GeoPoint map_to_wgs84(geometry_msgs::Point const &point, std::string map_frame="map", ros::Time target_time = ros::Time(0))
       {
           geographic_msgs::GeoPoint ret;
           try
           {
-            geometry_msgs::TransformStamped t = buffer()->lookupTransform("earth",map_frame,ros::Time(0));
+            geometry_msgs::TransformStamped t = buffer()->lookupTransform("earth",map_frame, target_time);
             geometry_msgs::Point out;
             tf2::doTransform(point,out,t);
             ECEF out_ecef;
diff --git a/include/project11/utils.h b/include/project11/utils.h
index bc27cdf..15d02df 100644
--- a/include/project11/utils.h
+++ b/include/project11/utils.h
@@ -23,24 +23,24 @@ namespace project11
     return v3.length();
   }
   
-  typedef gz4d::GeoPointLatLongDegrees LatLongDegrees;
-  typedef gz4d::GeoPointECEF ECEF;
-  typedef gz4d::Point<double> Point;
-  typedef gz4d::LocalENU ENUFrame;
+  using LatLongDegrees = gz4d::GeoPointLatLongDegrees;
+  using ECEF = gz4d::GeoPointECEF;
+  using Point = gz4d::Point<double>;
+  using ENUFrame = gz4d::LocalENU;
   
   // angle types that do not wrap
-  typedef gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::Unclamped> AngleDegrees;
-  typedef gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::Unclamped> AngleRadians;
+  using AngleDegrees = gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::Unclamped>;
+  using AngleRadians = gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::Unclamped>;
   
   // angle types that wrap at +/- half a circle
-  typedef gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::ZeroCenteredPeriod> AngleDegreesZeroCentered;
-  typedef gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::ZeroCenteredPeriod> AngleRadiansZeroCentered;
+  using AngleDegreesZeroCentered = gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::ZeroCenteredPeriod>;
+  using AngleRadiansZeroCentered = gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::ZeroCenteredPeriod>;
   
   // angle types that wrap at 0 and full circle
-  typedef gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::PositivePeriod> AngleDegreesPositive;
-  typedef gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::PositivePeriod> AngleRadiansPositive;
+  using AngleDegreesPositive = gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::PositivePeriod>;
+  using AngleRadiansPositive = gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::PositivePeriod>;
   
-  typedef gz4d::geo::WGS84::Ellipsoid WGS84;
+  using WGS84 = gz4d::geo::WGS84::Ellipsoid;
   
   template <typename A> void fromMsg(const A& a, LatLongDegrees &b)
   {
diff --git a/launch/robot_core.launch b/launch/robot_core.launch
index 209e6fb..24e6ace 100644
--- a/launch/robot_core.launch
+++ b/launch/robot_core.launch
@@ -5,53 +5,72 @@
   <arg name="default_nav_stack" default="true"/>
   <arg name="bridge_name" default="$(arg namespace)"/>
 
-  <rosparam command="load" file="$(find project11)/config/robot.yaml" ns="$(arg namespace)"/>
+  <arg name="tf_prefix" default="$(arg namespace)"/>
+  <arg name="base_frame" value="$(arg tf_prefix)/base_link"/>
+  <arg name="map_frame" value="$(arg tf_prefix)/map"/>
+  <arg name="odom_frame" value="$(arg tf_prefix)/odom"/>
 
-  <!-- The udp_bridge_node is what sends and receives messages from select topics to the operator. -->
-  <node if="$(arg enableBridge)" pkg="udp_bridge" type="udp_bridge_node" name="udp_bridge" ns="$(arg namespace)">
-    <param name="port" value="$(arg localPort)"/>
-    <param name="name" value="$(arg bridge_name)"/>
-  </node>
+
+  <group ns="$(arg namespace)">
+
+    <rosparam command="load" file="$(find project11)/config/robot.yaml"/>
+
+    <!-- The udp_bridge_node is what sends and receives messages from select topics to the operator. -->
+    <node if="$(arg enableBridge)" pkg="udp_bridge" type="udp_bridge_node" name="udp_bridge">
+      <param name="port" value="$(arg localPort)"/>
+      <param name="name" value="$(arg bridge_name)"/>
+    </node>
+      
+    <!-- Command bridge is used to robustly send operator commands over an unreliable connection. -->
+    <node pkg="command_bridge" type="command_bridge_receiver_node.py" name="command_bridge_receiver"/>
+
+    <!-- mru_transform Provides tf2 transforms from multiple gps and motion sensor sources. -->
+    <node pkg="mru_transform" type="mru_transform_node" name="mru_transform">
+      <param name="base_frame" value="$(arg base_frame)"/>
+      <param name="map_frame" value="$(arg map_frame)"/>
+      <param name="odom_frame" value="$(arg odom_frame)"/>
+      <remap from="nav/position" to="project11/nav/position"/>
+      <remap from="nav/orientation" to="project11/nav/orientation"/>
+      <remap from="nav/velocity" to="project11/nav/velocity"/>
+      <remap from="nav/active_sensor" to="project11/nav/active_sensor"/>
+    </node>
     
-  <!-- Command bridge is used to robustly send operator commands over an unreliable connection. -->
-  <node pkg="command_bridge" type="command_bridge_receiver_node.py" name="command_bridge_receiver" ns="$(arg namespace)"/>
-
-  <!-- mru_transform Provides tf2 transforms from multiple gps and motion sensor sources. -->
-  <node pkg="mru_transform" type="mru_transform_node" name="mru_transform" ns="$(arg namespace)">
-    <param name="base_frame" value="$(arg namespace)/base_link"/>
-    <param name="map_frame" value="$(arg namespace)/map"/>
-    <param name="odom_frame" value="$(arg namespace)/odom"/>
-    <remap from="nav/position" to="project11/nav/position"/>
-    <remap from="nav/orientation" to="project11/nav/orientation"/>
-    <remap from="nav/velocity" to="project11/nav/velocity"/>
-    <remap from="nav/active_sensor" to="project11/nav/active_sensor"/>
-  </node>
-  
-  <!-- helm_manager is the low level heart of project11. It manages which control messages get 
-  sent to the robot based on piloting mode and reports the piloting mode and other status
-  as a heartbeat message. -->
-  <node pkg="helm_manager" type="helm_manager_node" name="helm_manager" ns="$(arg namespace)">
-    <remap from="out/helm" to="project11/control/helm"/>
-    <remap from="out/cmd_vel" to="project11/control/cmd_vel"/>
-    <remap from="heartbeat" to="project11/heartbeat"/>
-    <remap from="status/helm" to="project11/status/helm"/>
-    <remap from="piloting_mode" to="project11/piloting_mode"/>
-    <param name="piloting_mode_prefix" value="project11/piloting_mode/"/>
-  </node>
-
-  <!-- mission_manager handles commands from the operator and manages the task list sent to the navigator.-->
-  <node pkg="mission_manager" type="mission_manager_node.py" name="mission_manager" ns="$(arg namespace)">
-      <param name="map_frame" value="$(arg namespace)/map"/>
-  </node>
-
-  <!-- project11_navigation is the plugin based navigation stack that uses a list of tasks to plan trajectories
-  and generate commands to drive the robot. -->
-  <node pkg="project11_navigation" type="navigator" name="navigator" ns="$(arg namespace)">
-    <remap from="~/cmd_vel" to="project11/piloting_mode/autonomous/cmd_vel"/>
-    <remap from="~/enable" to="project11/piloting_mode/autonomous/active"/>
-  </node>
-  <include if="$(arg default_nav_stack)" file="$(find project11_navigation)/launch/default_nav_stack.launch">
-    <arg name="namespace" value="$(arg namespace)/navigator"/>
-  </include>
+    <!-- helm_manager is the low level heart of project11. It manages which control messages get 
+    sent to the robot based on piloting mode and reports the piloting mode and other status
+    as a heartbeat message. -->
+    <node pkg="helm_manager" type="helm_manager_node" name="helm_manager">
+      <remap from="out/helm" to="project11/control/helm"/>
+      <remap from="out/cmd_vel" to="project11/control/cmd_vel"/>
+      <remap from="heartbeat" to="project11/heartbeat"/>
+      <remap from="status/helm" to="project11/status/helm"/>
+      <remap from="piloting_mode" to="project11/piloting_mode"/>
+      <param name="piloting_mode_prefix" value="project11/piloting_mode/"/>
+    </node>
+
+    <!-- mission_manager handles commands from the operator and manages the task list sent to the navigator.-->
+    <node pkg="mission_manager" type="mission_manager_node.py" name="mission_manager">
+        <param name="map_frame" value="$(arg map_frame)"/>
+    </node>
+
+    <!-- project11_navigation is the plugin based navigation stack that uses a list of tasks to plan trajectories
+    and generate commands to drive the robot. -->
+    <node pkg="project11_navigation" type="navigator" name="navigator">
+      <remap from="~/cmd_vel" to="project11/piloting_mode/autonomous/cmd_vel"/>
+      <remap from="~/enable" to="project11/piloting_mode/autonomous/active"/>
+    </node>
+    <node pkg="project11_navigation" type="occupancy_vector_map_from_geo" name="occupancy_vector_map_from_geo">
+      <remap from="~/input" to="project11/avoidance_map"/>
+      <remap from="~/output" to="project11/avoidance_map_local"/>
+      <param name="frame_id" value="$(arg map_frame)"/>
+    </node>
+    <node pkg="project11_navigation" type="occupancy_grid_from_vector_map" name="occupancy_grid_from_vector_map">
+      <remap from="~/input" to="project11/avoidance_map_local"/>
+      <remap from="~/output" to="project11/avoidance_grid"/>
+    </node>
+    <include if="$(arg default_nav_stack)" file="$(find project11_navigation)/launch/default_nav_stack.launch">
+      <arg name="namespace" value="navigator"/>
+    </include>
+
+  </group>
 
 </launch>