From e5f660a1fb6dfc3674e45b53c0ef71f134249424 Mon Sep 17 00:00:00 2001
From: Ioannis Souflas <isouflas@gmail.com>
Date: Thu, 11 Jan 2024 13:51:17 +0000
Subject: [PATCH 1/2] feat(obstacle_cruise_planner): add calculation of
 obstacle distance to ego

Add the arc length from the ego to the obstacle stop point to the stop_reasons topic.

Signed-off-by: Ioannis Souflas <isouflas@gmail.com>
---
 .../obstacle_cruise_planner/src/planner_interface.cpp     | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
index f00ad01efb0d9..f7ce218cf3ccf 100644
--- a/planning/obstacle_cruise_planner/src/planner_interface.cpp
+++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -37,19 +37,21 @@ StopSpeedExceeded createStopSpeedExceededMsg(
 }
 
 tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray(
-  const rclcpp::Time & current_time, const geometry_msgs::msg::Pose & stop_pose,
+  const PlannerData & planner_data, const geometry_msgs::msg::Pose & stop_pose,
   const StopObstacle & stop_obstacle)
 {
   // create header
   std_msgs::msg::Header header;
   header.frame_id = "map";
-  header.stamp = current_time;
+  header.stamp = planner_data.current_time;
 
   // create stop factor
   StopFactor stop_factor;
   stop_factor.stop_pose = stop_pose;
   geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_point;
   stop_factor_point.z = stop_pose.position.z;
+  stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength(
+    planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position);
   stop_factor.stop_factor_points.emplace_back(stop_factor_point);
 
   // create stop reason stamped
@@ -359,7 +361,7 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
     // Publish Stop Reason
     const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose;
     const auto stop_reasons_msg =
-      makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle);
+      makeStopReasonArray(planner_data, stop_pose, *closest_stop_obstacle);
     stop_reasons_pub_->publish(stop_reasons_msg);
     velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose));
 

From 2bc2ec8c65f8727295e22fcb0848f1fd6fe6bee3 Mon Sep 17 00:00:00 2001
From: Ioannis Souflas <isouflas@gmail.com>
Date: Sun, 25 Feb 2024 22:58:47 +0200
Subject: [PATCH 2/2] fix(tensorrt_yolox): renamed input output topic names

The input and output argument names of yolox.launch.xml do not match the input and output arguments names of multiple_yolox.launch.xml.

Signed-off-by: Ioannis Souflas <isouflas@gmail.com>
---
 .../launch/multiple_yolox.launch.xml          | 32 +++++++++----------
 1 file changed, 16 insertions(+), 16 deletions(-)

diff --git a/perception/tensorrt_yolox/launch/multiple_yolox.launch.xml b/perception/tensorrt_yolox/launch/multiple_yolox.launch.xml
index 5959f73757de1..9de37c76e034b 100644
--- a/perception/tensorrt_yolox/launch/multiple_yolox.launch.xml
+++ b/perception/tensorrt_yolox/launch/multiple_yolox.launch.xml
@@ -27,43 +27,43 @@
   <arg name="output_topic" default="rois"/>
 
   <include if="$(eval &quot;'$(var image_number)'>='1'&quot;)" file="$(find-pkg-share tensorrt_yolox)/launch/yolox.launch.xml">
-    <arg name="input_topic" value="$(var image_raw0)"/>
-    <arg name="output_topic" value="rois0"/>
+    <arg name="input/image" value="$(var image_raw0)"/>
+    <arg name="output/objects" value="rois0"/>
     <arg name="gpu_id" value="$(var gpu_id_image_raw0)"/>
   </include>
   <include if="$(eval &quot;'$(var image_number)'>='2'&quot;)" file="$(find-pkg-share tensorrt_yolox)/launch/yolox.launch.xml">
-    <arg name="input_topic" value="$(var image_raw1)"/>
-    <arg name="output_topic" value="rois1"/>
+    <arg name="input/image" value="$(var image_raw1)"/>
+    <arg name="output/objects" value="rois1"/>
     <arg name="gpu_id" value="$(var gpu_id_image_raw1)"/>
   </include>
   <include if="$(eval &quot;'$(var image_number)'>='3'&quot;)" file="$(find-pkg-share tensorrt_yolox)/launch/yolox.launch.xml">
-    <arg name="input_topic" value="$(var image_raw2)"/>
-    <arg name="output_topic" value="rois2"/>
+    <arg name="input/image" value="$(var image_raw2)"/>
+    <arg name="output/objects" value="rois2"/>
     <arg name="gpu_id" value="$(var gpu_id_image_raw2)"/>
   </include>
   <include if="$(eval &quot;'$(var image_number)'>='4'&quot;)" file="$(find-pkg-share tensorrt_yolox)/launch/yolox.launch.xml">
-    <arg name="input_topic" value="$(var image_raw3)"/>
-    <arg name="output_topic" value="rois3"/>
+    <arg name="input/image" value="$(var image_raw3)"/>
+    <arg name="output/objects" value="rois3"/>
     <arg name="gpu_id" value="$(var gpu_id_image_raw3)"/>
   </include>
   <include if="$(eval &quot;'$(var image_number)'>='5'&quot;)" file="$(find-pkg-share tensorrt_yolox)/launch/yolox.launch.xml">
-    <arg name="input_topic" value="$(var image_raw4)"/>
-    <arg name="output_topic" value="rois4"/>
+    <arg name="input/image" value="$(var image_raw4)"/>
+    <arg name="output/objects" value="rois4"/>
     <arg name="gpu_id" value="$(var gpu_id_image_raw4)"/>
   </include>
   <include if="$(eval &quot;'$(var image_number)'>='6'&quot;)" file="$(find-pkg-share tensorrt_yolox)/launch/yolox.launch.xml">
-    <arg name="input_topic" value="$(var image_raw5)"/>
-    <arg name="output_topic" value="rois5"/>
+    <arg name="input/image" value="$(var image_raw5)"/>
+    <arg name="output/objects" value="rois5"/>
     <arg name="gpu_id" value="$(var gpu_id_image_raw5)"/>
   </include>
   <include if="$(eval &quot;'$(var image_number)'>='7'&quot;)" file="$(find-pkg-share tensorrt_yolox)/launch/yolox.launch.xml">
-    <arg name="input_topic" value="$(var image_raw6)"/>
-    <arg name="output_topic" value="rois6"/>
+    <arg name="input/image" value="$(var image_raw6)"/>
+    <arg name="output/objects" value="rois6"/>
     <arg name="gpu_id" value="$(var gpu_id_image_raw6)"/>
   </include>
   <include if="$(eval &quot;'$(var image_number)'>='8'&quot;)" file="$(find-pkg-share tensorrt_yolox)/launch/yolox.launch.xml">
-    <arg name="input_topic" value="$(var image_raw7)"/>
-    <arg name="output_topic" value="rois7"/>
+    <arg name="input/image" value="$(var image_raw7)"/>
+    <arg name="output/objects" value="rois7"/>
     <arg name="gpu_id" value="$(var gpu_id_image_raw7)"/>
   </include>
 </launch>