From ea88f74793f92742138960b7d7aa0e657d4e3f16 Mon Sep 17 00:00:00 2001
From: Dongmyeong Lee <domlee@utexas.edu>
Date: Mon, 30 Oct 2023 10:22:44 -0500
Subject: [PATCH] check frame_id for multiple laserscan

---
 config/navigation.lua             |  7 ++-----
 src/navigation/navigation_main.cc | 11 +++++++----
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/config/navigation.lua b/config/navigation.lua
index 2f982be..e75af62 100644
--- a/config/navigation.lua
+++ b/config/navigation.lua
@@ -5,17 +5,14 @@ end
 NavigationParameters = {
   laser_topics = {
     "/velodyne_2dscan",
-    "/kinect_2dscan",
+    "/kinect_laserscan",
   };
+  laser_frame = "base_link";
   odom_topic = "/jackal_velocity_controller/odom";
   localization_topic = "localization";
   image_topic = "/camera/rgb/image_raw/compressed";
   init_topic = "initialpose";
   enable_topic = "autonomy_arbiter/enabled";
-  laser_loc = {
-    x = 0.065;
-    y = 0;
-  };
   dt = 0.025;
   max_linear_accel = 0.5;
   max_linear_decel = 0.5;
diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc
index bf9a8ce..030af82 100644
--- a/src/navigation/navigation_main.cc
+++ b/src/navigation/navigation_main.cc
@@ -107,12 +107,11 @@ DEFINE_bool(no_joystick, true, "Whether to use a joystick or not");
 
 CONFIG_STRING(image_topic, "NavigationParameters.image_topic");
 CONFIG_STRINGLIST(laser_topics, "NavigationParameters.laser_topics");
+CONFIG_STRING(laser_frame, "NavigationParameters.laser_frame");
 CONFIG_STRING(odom_topic, "NavigationParameters.odom_topic");
 CONFIG_STRING(localization_topic, "NavigationParameters.localization_topic");
 CONFIG_STRING(init_topic, "NavigationParameters.init_topic");
 CONFIG_STRING(enable_topic, "NavigationParameters.enable_topic");
-CONFIG_FLOAT(laser_loc_x, "NavigationParameters.laser_loc.x");
-CONFIG_FLOAT(laser_loc_y, "NavigationParameters.laser_loc.y");
 
 DEFINE_string(map, "UT_Campus", "Name of navigation map file");
 DEFINE_string(twist_drive_topic, "navigation/cmd_vel", "Drive Command Topic");
@@ -197,6 +196,11 @@ void LaserHandler(const sensor_msgs::LaserScan& msg,
            msg.header.stamp.toSec(),
            GetWallTime() - msg.header.stamp.toSec());
   }
+  ROS_WARN_COND(msg.header.frame_id != CONFIG_laser_frame,
+                "laser_topic=%s, frame_id=%s, expected=%s",
+                topic.c_str(),
+                msg.header.frame_id.c_str(),
+                CONFIG_laser_frame.c_str());
 
   static unordered_map<string, LaserCache> laser_caches_;
   laser_caches_.emplace(topic, LaserCache());
@@ -218,12 +222,11 @@ void LaserHandler(const sensor_msgs::LaserScan& msg,
   size_t start_idx = point_cloud_.size();
   point_cloud_.resize(start_idx + cache.rays.size());
   // Location of the laser on the robot. Assumes the laser is forward-facing.
-  const Vector2f kLaserLoc(CONFIG_laser_loc_x, CONFIG_laser_loc_y);
   for (size_t i = 0; i < cache.rays.size(); ++i) {
     const float r =
       ((msg.ranges[i] > msg.range_min && msg.ranges[i] < msg.range_max) ?
       msg.ranges[i] : msg.range_max);
-    point_cloud_[start_idx + i] = r * cache.rays[i] + kLaserLoc;
+    point_cloud_[start_idx + i] = r * cache.rays[i];
   } 
 }