Merge branch 'issue2_wheels_chassis_transform' into 'master'

Add wheels <-> chassis transform to tf tree

Closes #2

See merge request MaplessAI/external/a2d2_to_ros!54

include/a2d2_to_ros/conversions.hpp

@@ -31,6 +31,27 @@
 
 namespace a2d2_to_ros {
 
+/**
+ * @brief Convert seconds to microseconds
+ * @note This function does not protect against overflow
+ * TODO(jeff) should be using a dimensional analysis library
+ */
+uint64_t microseconds_to_nanoseconds(uint64_t s);
+
+/**
+ * @brief Convert seconds to microseconds
+ * @note This function does not protect against overflow
+ * TODO(jeff) should be using a dimensional analysis library
+ */
+uint64_t microseconds_to_seconds(uint64_t s);
+
+/**
+ * @brief Convert seconds to microseconds
+ * @note This function does not protect against overflow
+ * TODO(jeff) should be using a dimensional analysis library
+ */
+uint64_t seconds_to_microseconds(uint64_t s);
+
 /**
  * @brief Convert a 2D index to a 1D index according to CNPY convention.
  */

include/a2d2_to_ros/name_utils.hpp

@@ -28,6 +28,13 @@
 
 namespace a2d2_to_ros {
 
+/**
+ * @brief Convenience method to generate a standard TF frame name.
+ * @note This function has no test coverage.
+ */
+std::string tf_motion_compensated_sensor_frame_name(
+    const std::string& sensor_type, const std::string& sensor_frame);
+
 /**
  * @brief Convenience method to generate a standard TF frame name.
  * @note This function has no test coverage.

rviz_configs/visualize.rviz

@@ -24,7 +24,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Lidar - Front Left
+    SyncSource: Camera - Front Center
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -52,21 +52,33 @@ Visualization Manager:
       Value: true
     - Class: rviz/TF
       Enabled: true
-      Frame Timeout: 9999
+      Frame Timeout: 5
       Frames:
         All Enabled: true
         cameras_front_center:
           Value: true
+        cameras_front_center_point_cloud:
+          Value: true
         cameras_front_left:
           Value: true
+        cameras_front_left_point_cloud:
+          Value: true
         cameras_front_right:
           Value: true
+        cameras_front_right_point_cloud:
+          Value: true
         cameras_rear_center:
           Value: true
+        cameras_rear_center_point_cloud:
+          Value: true
         cameras_side_left:
           Value: true
+        cameras_side_left_point_cloud:
+          Value: true
         cameras_side_right:
           Value: true
+        cameras_side_right_point_cloud:
+          Value: true
         chassis:
           Value: true
         lidars_front_center:
@@ -88,6 +100,18 @@ Visualization Manager:
       Show Names: true
       Tree:
         wheels:
+          cameras_front_center_point_cloud:
+            {}
+          cameras_front_left_point_cloud:
+            {}
+          cameras_front_right_point_cloud:
+            {}
+          cameras_rear_center_point_cloud:
+            {}
+          cameras_side_left_point_cloud:
+            {}
+          cameras_side_right_point_cloud:
+            {}
           chassis:
             cameras_front_center:
               {}
@@ -128,9 +152,9 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 239
+      Max Intensity: 234
       Min Color: 0; 0; 0
-      Min Intensity: 1
+      Min Intensity: 0
       Name: Lidar - Front Center
       Position Transformer: XYZ
       Queue Size: 10
@@ -442,4 +466,4 @@ Window Geometry:
     collapsed: false
   Width: 1286
   X: 428
-  Y: 132
+  Y: 102

src/a2d2_to_ros/checks.cpp

@@ -25,7 +25,7 @@
 
 #include <cmath>
 
-static constexpr auto ONE_MILLION = static_cast<uint64_t>(1000000);
+#include "a2d2_to_ros/conversions.hpp"
 
 namespace a2d2_to_ros {
 
@@ -54,7 +54,7 @@ bool axes_are_valid(const Eigen::Vector3d& axis1, const Eigen::Vector3d& axis2,
 //------------------------------------------------------------------------------
 
 bool valid_ros_timestamp(uint64_t time) {
-  const auto secs = (time / ONE_MILLION);
+  const auto secs = microseconds_to_seconds(time);
   const auto boundary =
       static_cast<uint64_t>(std::numeric_limits<uint32_t>::max());
   return (secs <= boundary);

src/a2d2_to_ros/conversions.cpp

@@ -30,16 +30,28 @@ namespace a2d2_to_ros {
 
 //------------------------------------------------------------------------------
 
+uint64_t microseconds_to_nanoseconds(uint64_t s) { return (s * ONE_THOUSAND); }
+
+//------------------------------------------------------------------------------
+
+uint64_t microseconds_to_seconds(uint64_t s) { return (s / ONE_MILLION); }
+
+//------------------------------------------------------------------------------
+
+uint64_t seconds_to_microseconds(uint64_t s) { return (s * ONE_MILLION); }
+
+//------------------------------------------------------------------------------
+
 size_t flatten_2d_index(size_t width, size_t row, size_t col) {
   return ((row * width) + col);
 }
 
 //------------------------------------------------------------------------------
 
 ros::Time a2d2_timestamp_to_ros_time(uint64_t time) {
-  const auto truncated_secs = (time / ONE_MILLION);
-  const auto mu_secs = (time - (truncated_secs * ONE_MILLION));
-  const auto n_secs = (mu_secs * ONE_THOUSAND);
+  const auto truncated_secs = microseconds_to_seconds(time);
+  const auto mu_secs = (time - seconds_to_microseconds(truncated_secs));
+  const auto n_secs = microseconds_to_nanoseconds(mu_secs);
   return ros::Time(static_cast<uint32_t>(truncated_secs),
                    static_cast<uint32_t>(n_secs));
 }

src/a2d2_to_ros/name_utils.cpp

@@ -29,6 +29,13 @@ namespace a2d2_to_ros {
 
 //------------------------------------------------------------------------------
 
+std::string tf_motion_compensated_sensor_frame_name(
+    const std::string& sensor_type, const std::string& sensor_frame) {
+  return tf_frame_name(sensor_type, sensor_frame + "_motion_compensated");
+}
+
+//------------------------------------------------------------------------------
+
 std::string tf_frame_name(const std::string& sensor_type,
                           const std::string& sensor_frame) {
   return (sensor_type + "_" + sensor_frame);

src/sensor_fusion_bus_signals_to_ros.cpp

@@ -409,7 +409,21 @@ int main(int argc, char* argv[]) {
         msgtf.transforms.push_back(Tx_stamped_msg);
       }
 
-      {  // TODO(jeff): Compute this from roll/pitch
+      // Lidar data lives in camera frames, but it is motion corrected, so it
+      // lives in 'wheels' not 'chassis'
+      if (is_camera) {
+        geometry_msgs::Transform Tx_msg;
+        tf::transformEigenToMsg(Tx, Tx_msg);
+
+        geometry_msgs::TransformStamped Tx_stamped_msg;
+        Tx_stamped_msg.transform = Tx_msg;
+        Tx_stamped_msg.header.frame_id = "wheels";
+        Tx_stamped_msg.child_frame_id =
+            a2d2::tf_motion_compensated_sensor_frame_name(name, frame);
+        msgtf.transforms.push_back(Tx_stamped_msg);
+      }
+
+      {
         geometry_msgs::Transform Tx_msg;
         tf::transformEigenToMsg(Eigen::Affine3d::Identity(), Tx_msg);
 
@@ -475,7 +489,11 @@ int main(int argc, char* argv[]) {
 
       const auto& stamp = data.header.stamp;
       if (!first_time) {
-        first_time = stamp;
+        if (start_time != _START_TIME) {
+          first_time = a2d2::a2d2_timestamp_to_ros_time(start_time);
+        } else {
+          first_time = stamp;
+        }
       }
 
       const auto time_since_begin = (stamp - *first_time).toSec();

src/sensor_fusion_camera_to_ros.cpp

@@ -367,7 +367,11 @@ int main(int argc, char* argv[]) {
     }
 
     if (!first_time) {
-      first_time = frame_timestamp_ros;
+      if (start_time != _START_TIME) {
+        first_time = a2d2::a2d2_timestamp_to_ros_time(start_time);
+      } else {
+        first_time = frame_timestamp_ros;
+      }
     }
 
     const auto time_since_begin = (frame_timestamp_ros - *first_time).toSec();

src/sensor_fusion_lidar_to_ros.cpp

@@ -259,7 +259,11 @@ int main(int argc, char* argv[]) {
     }
 
     if (!first_time) {
-      first_time = frame_timestamp_ros;
+      if (start_time != _START_TIME) {
+        first_time = a2d2::a2d2_timestamp_to_ros_time(start_time);
+      } else {
+        first_time = frame_timestamp_ros;
+      }
     }
 
     const auto time_since_begin = (frame_timestamp_ros - *first_time).toSec();
@@ -306,8 +310,8 @@ int main(int argc, char* argv[]) {
     const auto lidar_file_name = a2d2::frame_from_filename(f);
     const auto lidar_name =
         a2d2::get_camera_name_from_frame_name(lidar_file_name);
-    const auto frame =
-        a2d2::tf_frame_name(a2d2::sensors::Names::CAMERAS, lidar_name);
+    const auto frame = a2d2::tf_motion_compensated_sensor_frame_name(
+        a2d2::sensors::Names::CAMERAS, lidar_name);
     if (frame.empty()) {
       X_FATAL("Could not find frame name in filename: "
               << f << ". Cannot continue.");

test/test_checks.cpp

@@ -179,7 +179,7 @@ TEST(A2D2_to_ROS_checks, verify_ego_bbox_params) {
 
 TEST(A2D2_to_ROS_checks, valid_ros_timestamp) {
   constexpr auto MAX = std::numeric_limits<uint64_t>::max();
-  constexpr auto TIME = static_cast<uint64_t>(1554122338652775);
+  constexpr auto TIME = static_cast<uint64_t>(1554122338652775ull);
   EXPECT_FALSE(valid_ros_timestamp(MAX));
   EXPECT_TRUE(valid_ros_timestamp(TIME));