From cfbd1700b1537a086bc5a3b97b73da27e270c6d8 Mon Sep 17 00:00:00 2001
From: seefelke <kevin.seefeld@uni-a.de>
Date: Thu, 21 Nov 2024 18:36:02 +0100
Subject: [PATCH] Added new message types Trajectory and Navigation Point.
 Added Behaviour Enum in localplanning/utils.py

---
 code/planning/CMakeLists.txt             | 23 ++++++++++++++++-------
 code/planning/msg/NavigationPoint.msg    |  3 +++
 code/planning/msg/Trajectory.msg         |  2 ++
 code/planning/package.xml                |  4 ++++
 code/planning/src/local_planner/utils.py |  7 +++++++
 5 files changed, 32 insertions(+), 7 deletions(-)
 create mode 100644 code/planning/msg/NavigationPoint.msg
 create mode 100644 code/planning/msg/Trajectory.msg

diff --git a/code/planning/CMakeLists.txt b/code/planning/CMakeLists.txt
index 34c3ccd0..9da52430 100755
--- a/code/planning/CMakeLists.txt
+++ b/code/planning/CMakeLists.txt
@@ -13,8 +13,11 @@ project(planning)
 find_package(catkin REQUIRED COMPONENTS
   perception
   rospy
+  rosout
   roslaunch
   std_msgs
+  geometry_msgs
+  message_generation
 )
 
 roslaunch_add_file_check(launch)
@@ -48,9 +51,11 @@ catkin_python_setup()
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   MinDistance.msg
-# )
+ add_message_files(
+   FILES
+   NavigationPoint.msg
+   Trajectory.msg
+ )
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -67,9 +72,12 @@ catkin_python_setup()
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   perception
-# )
+ generate_messages(
+   DEPENDENCIES
+   std_msgs
+   perception
+   geometry_msgs
+ )
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -105,7 +113,8 @@ catkin_package(
 #  LIBRARIES planning
 #  CATKIN_DEPENDS other_catkin_pkg
 #  DEPENDS system_lib
-  CATKIN_DEPENDS perception rospy
+#  CATKIN_DEPENDS perception rospy
+ CATKIN_DEPENDS message_runtime
 )
 
 ###########
diff --git a/code/planning/msg/NavigationPoint.msg b/code/planning/msg/NavigationPoint.msg
new file mode 100644
index 00000000..e9eb53c9
--- /dev/null
+++ b/code/planning/msg/NavigationPoint.msg
@@ -0,0 +1,3 @@
+geometry_msgs/Point position
+float32 speed
+uint8 behaviour
\ No newline at end of file
diff --git a/code/planning/msg/Trajectory.msg b/code/planning/msg/Trajectory.msg
new file mode 100644
index 00000000..94060cdd
--- /dev/null
+++ b/code/planning/msg/Trajectory.msg
@@ -0,0 +1,2 @@
+Header header
+planning/NavigationPoint[] navigationPoints
\ No newline at end of file
diff --git a/code/planning/package.xml b/code/planning/package.xml
index a321a109..daf36bc6 100755
--- a/code/planning/package.xml
+++ b/code/planning/package.xml
@@ -47,6 +47,9 @@
   <!--   <test_depend>gtest</test_depend> -->
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
   <build_depend>rospy</build_depend>
   <build_depend>roslaunch</build_depend>
 
@@ -57,6 +60,7 @@
   <exec_depend>carla_msgs</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>perception</exec_depend>
 
   <build_depend>perception</build_depend>
diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py
index d976506d..5d684df9 100644
--- a/code/planning/src/local_planner/utils.py
+++ b/code/planning/src/local_planner/utils.py
@@ -3,6 +3,7 @@
 import math
 import carla
 import os
+from enum import IntEnum
 
 # import rospy
 
@@ -23,6 +24,12 @@
 EARTH_RADIUS_EQUA = 6378137.0
 
 
+# getattr(Behaviour, "name").value returns the ID
+# Behaviour(id) returns Behaviour.NAME
+class Behaviour(IntEnum):
+    CRUISING = 0
+
+
 def get_distance(pos_1, pos_2):
     """Calculate the distance between two positions