Added COLCON IGNORE to most new localization pkgs. We use ground trut…

…h in ROS2 so it does not affect usability. Fully compiles

gnc/ctl/src/ctl.cc

@@ -351,7 +351,7 @@ void Control::Initialize(void) {
 
 void Control::ReadParams(config_reader::ConfigReader* config) {
   if (!config->GetReal("tun_vel_gain", &tun_vel_gain))
-    ROS_FATAL("Unspecified tun_vel_gain.");
+    FF_FATAL("Unspecified tun_vel_gain.");
   Eigen::Vector3d temp;
   if (!msg_conversions::config_read_vector(config, "tun_accel_gain", &temp))
     FF_FATAL("Unspecified tun_accel_gain.");

localization/localization_common/CMakeLists.txt

@@ -28,13 +28,13 @@ set( CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -Wall -Werror -O3 -fPIC" )
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(rclcpy REQUIRED)
 
 find_package(astrobee REQUIRED)
 find_package(camera REQUIRED)
 find_package(config_reader REQUIRED)
 find_package(msg_conversions REQUIRED)
 find_package(ff_msgs REQUIRED)
+find_package(ff_common REQUIRED)
 
 # Find GTSAM
 find_package(GTSAM REQUIRED)
@@ -45,6 +45,11 @@ find_package(Eigen3 REQUIRED)
 # Allow other packages to use python scripts from this package
 ament_python_install_package(${PROJECT_NAME})
 
+# Install Python executables
+#install(PROGRAMS
+#  scripts/py_node.py
+#  DESTINATION lib/${PROJECT_NAME}
+#)
 
 ###########
 ## Build ##
@@ -75,7 +80,7 @@ add_library(${PROJECT_NAME} SHARED
   src/utilities.cc
 )
 target_link_libraries(${PROJECT_NAME} gtsam ${OpenCV_LIBRARIES})
-ament_target_dependencies(${PROJECT_NAME} rclcpp astrobee ff_msgs config_reader camera msg_conversions)
+ament_target_dependencies(${PROJECT_NAME} rclcpp astrobee ff_msgs config_reader camera msg_conversions ff_common)
 ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
 
 if(BUILD_TESTING)

localization/localization_common/include/localization_common/timestamped_set.h

@@ -176,7 +176,7 @@ template <typename T>
 TimestampedSet<T>::TimestampedSet(const std::vector<Time>& timestamps, const std::vector<T>& values,
                                   const boost::optional<int> max_size)
     : max_size_(max_size) {
-  for (int i = 0; i < values.size(); ++i) {
+  for (long unsigned int i = 0; i < values.size(); ++i) {
     Add(timestamps[i], values[i]);
   }
 }
@@ -186,7 +186,7 @@ bool TimestampedSet<T>::Add(const Time timestamp, const T& value) {
   if (Contains(timestamp)) return false;
   timestamp_value_map_.emplace(timestamp, value);
   // Optionally shrink elements to half of max size if max size exceeded. Removes first half of set.
-  if (max_size_ && size() > *max_size_) {
+  if (max_size_ && size() > (size_t)(*max_size_) ) {
     auto end_it = timestamp_value_map_.begin();
     std::advance(end_it, *max_size_ / 2);
     timestamp_value_map_.erase(timestamp_value_map_.begin(), end_it);
@@ -431,7 +431,7 @@ template <typename T>
 std::pair<typename std::map<Time, T>::const_iterator, typename std::map<Time, T>::const_iterator>
 TimestampedSet<T>::InRangeValues(const Time oldest_allowed_timestamp, const Time latest_allowed_timestamp) {
   auto upper_bound = timestamp_value_map_.upper_bound(latest_allowed_timestamp);
-  auto lower_bound = timestamp_value_map_.lower_bound(oldest_allowed_timestamp);
+  //auto lower_bound = timestamp_value_map_.lower_bound(oldest_allowed_timestamp);
   // No values less than latest allowed time
   if (upper_bound == timestamp_value_map_.cbegin()) return {cend(), cend()};
   return std::make_pair(timestamp_value_map_.lower_bound(oldest_allowed_timestamp), upper_bound);

localization/localization_common/include/localization_common/utilities.h

@@ -21,7 +21,7 @@
 
 #include <config_reader/config_reader.h>
 #include <ff_msgs/msg/combined_nav_state.hpp>
-#include <ff_msgs/msg/graph_VIO_state.hpp>
+#include <ff_msgs/msg/graph_vio_state.hpp>
 #include <ff_msgs/msg/visual_landmarks.hpp>
 #include <localization_common/combined_nav_state.h>
 #include <localization_common/combined_nav_state_covariances.h>
@@ -41,6 +41,7 @@
 namespace ff_msgs {
 typedef msg::GraphState GraphState;
 typedef msg::VisualLandmarks VisualLandmarks;
+typedef msg::CombinedNavState CombinedNavState;
 }  // namespace ff_msgs
 
 #include <geometry_msgs/msg/pose.hpp>
@@ -129,7 +130,7 @@ ff_msgs::CombinedNavState CombinedNavStateToMsg(const CombinedNavState& combined
                                                 const PoseCovariance& pose_covariance,
                                                 const Eigen::Matrix3d& velocity_covariance,
                                                 const Eigen::Matrix<double, 6, 6>& imu_bias_covariance,
-                                                const TimestampedSet<PoseCovariance>& correlation_covariances = {});
+                                                const TimestampedSet<PoseCovariance>& correlation_covariances = TimestampedSet<PoseCovariance>());
 
 template <class LocMsgType>
 void CombinedNavStateToLocMsg(const CombinedNavState& combined_nav_state, LocMsgType& loc_msg);

localization/localization_common/package.xml

@@ -23,13 +23,14 @@
   <build_depend>config_reader</build_depend>
   <build_depend>msg_conversions</build_depend>
   <build_depend>ff_msgs</build_depend>
+  <build_depend>ff_common</build_depend>  
 
   <exec_depend>astrobee</exec_depend>
   <exec_depend>camera</exec_depend>
   <exec_depend>config_reader</exec_depend>
   <exec_depend>msg_conversions</exec_depend>
   <exec_depend>ff_msgs</exec_depend>
-
+  <exec_depend>ff_common</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

localization/localization_common/src/distance_scaled_pose_covariance_interpolater.cc

@@ -27,7 +27,7 @@ PoseCovariance DistanceScaledPoseCovarianceInterpolater::Interpolate(const PoseW
                                                                      const PoseWithCovariance& b,
                                                                      const Time& timestamp_a, const Time& timestamp_b) {
   const Eigen::Isometry3d relative_pose = a.pose.inverse() * b.pose;
-  const double translation_norm = relative_pose.translation().norm();
+  //const double translation_norm = relative_pose.translation().norm(); // -Werror=unused-variable
   const double orientation_angle = Eigen::AngleAxisd(relative_pose.linear()).angle();
   Eigen::Matrix<double, 6, 6> relative_covariance = Eigen::Matrix<double, 6, 6>::Zero();
   // Set translation part of covariance using translation norm

localization/localization_common/src/utilities.cc

@@ -112,12 +112,16 @@ Time TimeFromHeader(const std_msgs::Header& header) { return GetTime(header.stam
 
 Time TimeFromRosTime(const rclcpp::Time& time) { return GetTime(time.seconds(), time.nanoseconds()); }
 
-rclcpp::Time TimeToRosTime(const Time timestamp) { 
-  int sec = (int)(floor(timestamp)); int nanosec = (timestamp - floor(timestamp))*1e9; 
-  return rclcpp::Time(sec, nanosec, RCL_ROS_TIME); 
+void TimeToHeader(const Time timestamp, std_msgs::Header& header) { 
+  rclcpp::Time t; 
+  TimeToMsg(timestamp, t);
+  header.stamp = builtin_interfaces::msg::Time(t);
 }
 
-void TimeToHeader(const Time timestamp, std_msgs::Header& header) { header.stamp = TimeToRosTime(timestamp); }
+void TimeToMsg(const Time timestamp, rclcpp::Time& time_msg) {
+  int sec = (int)(floor(timestamp)); int nanosec = (timestamp - floor(timestamp))*1e9; 
+  time_msg = rclcpp::Time(sec, nanosec, RCL_ROS_TIME); 
+}
 
 gtsam::Pose3 PoseFromMsg(const geometry_msgs::PoseStamped& msg) { return PoseFromMsg(msg.pose); }
 
@@ -204,9 +208,11 @@ ff_msgs::CombinedNavState CombinedNavStateToMsg(const CombinedNavState& combined
 
   // Write correlation pose covariances if available
   for (const auto& correlation_covariance : correlation_covariances.set()) {
-    ff_msgs::PoseCovarianceStamped covariance_msg;
+    ff_msgs::msg::PoseCovarianceStamped covariance_msg;
     mc::EigenCovarianceToMsg(correlation_covariance.second, covariance_msg.covariance);
-    TimeToMsg(correlation_covariance.first, covariance_msg.time);
+    rclcpp::Time cov_time;
+    TimeToMsg(correlation_covariance.first, cov_time);
+    covariance_msg.time = builtin_interfaces::msg::Time(cov_time);
     msg.correlation_covariances.push_back(covariance_msg);
   }
   return msg;