Merge pull request #158 from una-auxme/156-feature-import-carlaroute-…

…to-dev-environment

Feature: Import carlaRoute to dev env

code/perception/src/global_plan_distance_publisher.py

@@ -44,11 +44,17 @@ def __init__(self):
             self.update_position,
             qos_profile=1)
 
+        # Change comment for dev_launch
         self.global_plan_subscriber = self.new_subscription(
             CarlaRoute,
             "/carla/" + self.role_name + "/global_plan",
             self.update_global_route,
             qos_profile=1)
+        # self.global_plan_subscriber = self.new_subscription(
+        #     CarlaRoute,
+        #     "/paf/" + self.role_name + "/global_plan",
+        #     self.update_global_route,
+        #     qos_profile=1)
 
         self.waypoint_publisher = self.new_publisher(
             Waypoint,

code/planning/launch/planning.launch

@@ -16,11 +16,11 @@
            <param name="control_loop_rate" value="1" />
     </node>
 
-    <!-- <node pkg="planning" type="global_planner/dev_global_route.py" name="DevGlobalRoute" output="screen">
-           <param name="from_txt" value="False" />
+    <!-- <node pkg="planning" type="dev_global_route.py" name="DevGlobalRoute" output="screen">
+           <param name="from_txt" value="True" />
            <param name="sampling_resolution" value="75.0" />
            <param name="routes" value="/opt/leaderboard/data/routes_devtest.xml" />
-           <param name="global_route_txt" value="/code/planning/global_planner/src/global_route.txt" />
+           <param name="global_route_txt" value="/code/planning/src/global_planner/global_route.txt" />
            <param name="role_name" value="hero" />
     </node> -->
     <node pkg="planning" type="global_planner.py" name="PrePlanner" output="screen">

code/planning/src/global_planner/dev_global_route.py

@@ -12,6 +12,17 @@
 from agents.navigation.global_route_planner import GlobalRoutePlanner
 import xmltodict
 
+"""
+This node is currently used for importing a CarlaRoute to dev.launch.
+For this you need to follow 4 steps:
+1. In plannning.launch: Uncomment this node and change the .txt file if needed
+2. In global_planner.py: Change the subscriber of carla/hero/global_plan to paf
+3. In gobal_planner.py: At the end of the init uncomment the
+    self.dev_load_world_info()
+4. In global_plan_distance_publisher.py:
+    Change the subscriber of carla/hero/global_plan to paf
+"""
+
 
 class DevGlobalRoute(CompatibleNode):
 
@@ -23,7 +34,7 @@ def __init__(self):
         if self.from_txt:
             self.global_route_txt = self.get_param(
                 'global_route_txt',
-                "/code/planning/global_planner/src/global_route.txt")
+                "/code/planning/src/global_planner/global_route.txt")
         else:
             self.sampling_resolution = self.get_param('sampling_resolution',
                                                       100.0)
@@ -46,7 +57,7 @@ def __init__(self):
                 durability=DurabilityPolicy.TRANSIENT_LOCAL)
         )
 
-        self.loginfo('DevGlobalRoute-Node started')
+        self.logerr('DevGlobalRoute-Node started')
 
     def world_info_callback(self, data: CarlaWorldInfo) -> None:
         """
@@ -64,6 +75,7 @@ def world_info_callback(self, data: CarlaWorldInfo) -> None:
                             f"current working directory is'{os.getcwd()}'")
                 raise
 
+            self.logerr("DevRoute: TXT READ")
             global_routes = input_routes.split("---")
             header_list = []
             road_options_list = []

code/planning/src/global_planner/global_planner.py

@@ -49,18 +49,22 @@ def __init__(self):
             "distance_spawn_to_first_wp", 100)
 
         self.map_sub = self.new_subscription(
-            # msg_type=CarlaWorldInfo,
-            # topic="/carla/world_info",
             msg_type=String,
             topic=f"/carla/{self.role_name}/OpenDRIVE",
             callback=self.world_info_callback,
             qos_profile=10)
 
+        # uncomment /paf/hero/global_plan and comment /carla/... for dev_launch
         self.global_plan_sub = self.new_subscription(
             msg_type=CarlaRoute,
             topic='/carla/' + self.role_name + '/global_plan',
             callback=self.global_route_callback,
             qos_profile=10)
+        # self.global_plan_sub = self.new_subscription(
+        #     msg_type=CarlaRoute,
+        #     topic='/paf/' + self.role_name + '/global_plan',
+        #     callback=self.global_route_callback,
+        #     qos_profile=10)
 
         self.current_pos_sub = self.new_subscription(
             msg_type=PoseStamped,
@@ -79,6 +83,9 @@ def __init__(self):
             qos_profile=1)
         self.logdebug('PrePlanner-Node started')
 
+        # uncomment for self.dev_load_world_info() for dev_launch
+        # self.dev_load_world_info()
+
     def global_route_callback(self, data: CarlaRoute) -> None:
         """
         when the global route gets updated a new trajectory is calculated with
@@ -189,7 +196,6 @@ def global_route_callback(self, data: CarlaRoute) -> None:
         self.path_pub.publish(self.path_backup)
         self.loginfo("PrePlanner: published trajectory")
 
-#    def world_info_callback(self, data: CarlaWorldInfo) -> None:
     def world_info_callback(self, opendrive: String) -> None:
         """
         when the map gets updated a mew OpenDriveConverter instance is created
@@ -198,8 +204,10 @@ def world_info_callback(self, opendrive: String) -> None:
         """
         self.loginfo("PrePlanner: MapUpdate called")
 
-#        root = eTree.fromstring(data.opendrive)
-        root = eTree.fromstring(opendrive.data)
+        if type(opendrive) is str:
+            root = eTree.fromstring(opendrive)
+        else:
+            root = eTree.fromstring(opendrive.data)
 
         roads = root.findall("road")
         road_ids = [int(road.get("id")) for road in roads]
@@ -235,6 +243,14 @@ def position_callback(self, data: PoseStamped):
                          "route preplanning")
             self.global_route_callback(self.global_route_backup)
 
+    def dev_load_world_info(self):
+        file_path = \
+            "/workspace/code/planning/src/global_planner/string_world_info.txt"
+        with open(file_path, 'r') as file:
+            file_content = file.read()
+        self.logerr("DATA READ")
+        self.world_info_callback(file_content)
+
     def run(self):
         """
         Control loop

code/planning/src/global_planner/global_route.txt

@@ -9,7 +9,7 @@ poses:
   -
     position:
       x: 983.5927734375
-      y: -5381.88720703125
+      y: -5445.88720703125
       z: 371.9093017578125
     orientation:
       x: 0.07026641552209804

code/planning/src/global_planner/global_route_short.txt

@@ -0,0 +1,59 @@
+header:
+  seq: 5
+  stamp:
+    secs: 1674304465
+    nsecs: 171659708
+  frame_id: "/map"
+road_options: [4, 2, 4, 3, 4]
+poses:
+  -
+    position:
+      x: 983.5927734375
+      y: -5445.88720703125
+      z: 371.9093017578125
+    orientation:
+      x: 0.07026641552209804
+      y: 0.038563409597558
+      z: -0.8738328670313198
+      w: 0.47957441006136947
+  -
+    position:
+      x: 983.2598876953125
+      y: -5568.28662109375
+      z: 370.7098388671875
+    orientation:
+      x: 0.12320969016364677
+      y: 0.06761958345056983
+      z: -0.8679519692380901
+      w: 0.47634687285576993
+  -
+    position:
+      x: 974.2578735351562
+      y: -5575.4443359375
+      z: 370.6752014160156
+    orientation:
+      x: 0.7541724368151881
+      y: -0.3663661511612136
+      z: 0.490197382664243
+      w: 0.2381308565908366
+  -
+    position:
+      x: 805.0078735351562
+      y: -5575.51806640625
+      z: 370.8327941894531
+    orientation:
+      x: 0.7208694596446681
+      y: -0.35018804258474884
+      z: 0.5379746839529138
+      w: 0.26134038418895195
+  -
+    position:
+      x: 780.8638305664062
+      y: -5575.548828125
+      z: 370.8050537109375
+    orientation:
+      x: 0.6993724403633815
+      y: -0.292214697883295
+      z: 0.6018708299183292
+      w: 0.2514761700331955
+---