726 overtake lane free motion aware

code/agent/config/dev_objects.json

@@ -70,7 +70,7 @@
           "id": "RADAR0",
           "spawn_point": {
             "x": 2.0,
-            "y": 1.5,
+            "y": -1.5,
             "z": 0.5,
             "roll": 0.0,
             "pitch": 0.0,
@@ -85,7 +85,7 @@
           "type": "sensor.other.radar",
           "id": "RADAR1",
           "spawn_point": {
-            "x": 2.0,
+            "x": -2.0,
             "y": -1.5,
             "z": 0.5,
             "roll": 0.0,

code/agent/src/agent/agent.py

@@ -1,5 +1,6 @@
 from leaderboard.autoagents.ros1_agent import ROS1Agent
 from leaderboard.autoagents.autonomous_agent import Track
+import math
 
 
 def get_entry_point():
@@ -49,7 +50,7 @@ def sensors(self):
                 "type": "sensor.other.radar",
                 "id": "RADAR0",
                 "x": 2.0,
-                "y": 1.5,
+                "y": -1.5,
                 "z": 0.5,
                 "roll": 0.0,
                 "pitch": 0.0,
@@ -60,12 +61,12 @@ def sensors(self):
             {
                 "type": "sensor.other.radar",
                 "id": "RADAR1",
-                "x": 2.0,
+                "x": -2.0,
                 "y": -1.5,
                 "z": 0.5,
                 "roll": 0.0,
                 "pitch": 0.0,
-                "yaw": 0.0,
+                "yaw": math.radians(180),
                 "horizontal_fov": 25,
                 "vertical_fov": 0.1,
             },

code/mapping/config/mapping.cfg

@@ -10,12 +10,11 @@ gen = ParameterGenerator()
 
 
 tab_inputs = gen.add_group("Sources", type="tab")
-tab_inputs.add("enable_lidar_marker", bool_t, 0, "Enable Lidar Marker integration (Clusters)", True)
-tab_inputs.add("enable_radar_marker", bool_t, 0, "Enable Radar Marker integration (Clusters)", True)
-tab_inputs.add("enable_lidar_cluster", bool_t, 0, "Enable Lidar Cluster integration (not yet implemented)", True)
 tab_inputs.add("enable_radar_cluster", bool_t, 0, "Enable Radar Cluster integration (not yet implemented)", True)
+tab_inputs.add("enable_lidar_cluster", bool_t, 0, "Enable Lidar Cluster integration (not yet implemented)", True)
+tab_inputs.add("enable_vision_cluster", bool_t, 0, "Enable Vision Node Cluster integration", True)
+tab_inputs.add("enable_raw_lidar_points", bool_t, 0, "Enable raw lidar input to mapping.", False)
 tab_inputs.add("enable_lane_marker", bool_t, 0, "Enable Lane Mark integration", True)
-tab_inputs.add("enable_raw_lidar_points", bool_t, 0, "Enable raw lidar input to mapping.", True)
 
 tab_inputs.add("enable_stop_marks", bool_t, 0, "Enable stop marks from the UpdateStopMarks service.", True)
 
@@ -26,6 +25,7 @@ tab_filters.add("enable_merge_filter", bool_t, 0, "Enable or disable the merging
 tab_filters.add("merge_growth_distance", double_t, 0, "Amount shapes grow before merging in meters.", 0.3, 0.0, 5.0)
 tab_filters.add("min_merging_overlap_percent", double_t, 0, "Min overlap of the grown shapes in percent.", 0.5, 0.0, 1.0)
 tab_filters.add("min_merging_overlap_area", double_t, 0, "Min overlap of the grown shapes in m2.", 0.5, 0.0, 5.0)
+tab_filters.add("polygon_simplify_tolerance", double_t, 0, "The polygon simplify tolerance.", 0.01, 0.1, 1.0)
 
 tab_lidar = gen.add_group("Lidar", type="tab")
 tab_lidar.add("lidar_z_min", double_t, 0, "Excludes lidar points below this height.", -1.5, -10, 2.0)

code/mapping/ext_modules/mapping_common/entity.py

@@ -480,7 +480,10 @@ def get_global_x_velocity(self) -> Optional[float]:
         return velocity
 
     def get_delta_forward_velocity_of(self, other: "Entity") -> Optional[float]:
-        """Calculates the delta velocity compared to other in the heading of self
+        """Calculates the delta velocity compared to other in the heading of self.
+            This function is only for objects in front. If the entity is behind this
+            value has to be inverted. Use the "get_delta_velocity_of" function for this
+            case.
 
         - result > 0: other moves away from self
         - result < 0: other moves nearer to self
@@ -501,6 +504,29 @@ def get_delta_forward_velocity_of(self, other: "Entity") -> Optional[float]:
         relative_motion = other_motion_in_self_coords - self.motion.linear_motion
         return relative_motion.x()
 
+    def get_delta_velocity_of(self, other: "Entity") -> Optional[float]:
+        """Calculates the delta velocity compared to other in the heading of self
+
+        - result > 0: other moves away from self
+        - result < 0: other moves nearer to self
+
+        Args:
+            other (Entity)
+
+        Returns:
+            Optional[float]: Delta velocity if both entities have one.
+        """
+        forward_velocity = self.get_delta_forward_velocity_of(other)
+        if forward_velocity is None:
+            return None
+
+        into_self_local: Transform2D = self.transform.inverse() * other.transform
+        other_position_in_self_coords: Vector2 = into_self_local.translation()
+
+        if other_position_in_self_coords.x() < 0.0:
+            return -forward_velocity
+        return forward_velocity
+
     def get_width(self) -> float:
         """Returns the local width (y-bounds) of the entity
 

code/mapping/ext_modules/mapping_common/map.py

@@ -22,6 +22,7 @@
 
 
 class LaneFreeState(Enum):
+    TO_BE_CHECKED = 2
     FREE = 1
     BLOCKED = 0
     MISSING_LANEMARK_ERR = -1
@@ -483,6 +484,7 @@ def is_lane_free(
         min_coverage_percent: float = 0.0,
         min_coverage_area: float = 0.0,
         lane_angle: float = 5.0,
+        motion_aware: bool = True,
     ) -> Tuple[LaneFreeState, Optional[shapely.Geometry]]:
         """Returns if a lane left or right of our car is free.
         There are three check methods available:
@@ -517,7 +519,7 @@ def is_lane_free(
             return LaneFreeState and if available the checkbox shape
         """
         if check_method == "rectangle":
-            return self.is_lane_free_rectangle(
+            lane_state, lane_box = self.is_lane_free_rectangle(
                 right_lane=right_lane,
                 lane_length=lane_length,
                 lane_transform=lane_transform,
@@ -526,7 +528,7 @@ def is_lane_free(
                 min_coverage_area=min_coverage_area,
             )
         elif check_method == "lanemarking":
-            return self.is_lane_free_lanemarking(
+            lane_state, lane_box = self.is_lane_free_lanemarking(
                 right_lane=right_lane,
                 lane_length=lane_length,
                 lane_transform=lane_transform,
@@ -536,7 +538,7 @@ def is_lane_free(
                 lane_angle=lane_angle,
             )
         elif check_method == "fallback":
-            lane_free_state, lane_check_shape = self.is_lane_free_lanemarking(
+            lane_state, lane_box = self.is_lane_free_lanemarking(
                 right_lane=right_lane,
                 lane_length=lane_length,
                 lane_transform=lane_transform,
@@ -547,20 +549,41 @@ def is_lane_free(
             )
 
             # return value of is_lane_free_lanemarking function of value is plausible
-            if not lane_free_state.is_error():
-                return lane_free_state, lane_check_shape
+            if lane_state.is_error():
+                lane_state, lane_box = self.is_lane_free_rectangle(
+                    right_lane=right_lane,
+                    lane_length=lane_length,
+                    lane_transform=lane_transform,
+                    reduce_lane=reduce_lane,
+                    min_coverage_percent=min_coverage_percent,
+                    min_coverage_area=min_coverage_area,
+                )
 
-            # use is_lane_free_rectangle function as fallback
-            return self.is_lane_free_rectangle(
-                right_lane=right_lane,
-                lane_length=lane_length,
-                lane_transform=lane_transform,
-                reduce_lane=reduce_lane,
-                min_coverage_percent=min_coverage_percent,
-                min_coverage_area=min_coverage_area,
-            )
+        if lane_state.is_error():
+            return lane_state, None
+
+        if lane_box is None or not LaneFreeState.TO_BE_CHECKED:
+            return LaneFreeState.SHAPE_ERR, None
+
+        colliding_entities = self.get_overlapping_entities(
+            lane_box,
+            min_coverage_percent=min_coverage_percent,
+            min_coverage_area=min_coverage_area,
+        )
+
+        hero = self.map.hero()
+        if motion_aware and hero is not None:
+            for i in range(len(colliding_entities) - 1, -1, -1):
+                entity = colliding_entities[i]
+                delta_motion = hero.get_delta_velocity_of(entity.entity)
+
+                if delta_motion is not None and delta_motion > 0.5:
+                    del colliding_entities[i]
+        # if there are colliding entities, the lane is not free
+        if not colliding_entities:
+            return LaneFreeState.FREE, lane_box
 
-        # check_method == "trajectory": not implemented yet
+        return LaneFreeState.BLOCKED, lane_box
 
     def is_lane_free_rectangle(
         self,
@@ -611,19 +634,9 @@ def is_lane_free_rectangle(
         )
 
         # converts lane box Rectangle to a shapely Polygon
-        lane_box_shapely = lane_box_shape.to_shapely(Transform2D.identity())
-
-        # get entities that are colliding with the checkbox entity
-        colliding_entities = self.get_overlapping_entities(
-            lane_box_shapely,
-            min_coverage_percent=min_coverage_percent,
-            min_coverage_area=min_coverage_area,
-        )
+        lane_box = lane_box_shape.to_shapely(Transform2D.identity())
 
-        # if list with lane box intersection is empty --> lane is free
-        if not colliding_entities:
-            return LaneFreeState.FREE, lane_box_shapely
-        return LaneFreeState.BLOCKED, lane_box_shapely
+        return LaneFreeState.TO_BE_CHECKED, lane_box
 
     def is_lane_free_lanemarking(
         self,
@@ -717,17 +730,7 @@ def is_lane_free_lanemarking(
         # TODO: motion detection check could be done here also,
         # deprecated function could be found in commit
         # d6d246fe181d6c60a1de33e578f2d4a47cb05ed4
-        colliding_entities = self.get_overlapping_entities(
-            lane_box,
-            min_coverage_percent=min_coverage_percent,
-            min_coverage_area=min_coverage_area,
-        )
-
-        # if there are colliding entities, the lane is not free
-        if not colliding_entities:
-            return LaneFreeState.FREE, lane_box
-        else:
-            return LaneFreeState.BLOCKED, lane_box
+        return LaneFreeState.TO_BE_CHECKED, lane_box
 
     def get_nearest_entity(
         self,

code/mapping/launch/mapping.launch

@@ -14,11 +14,13 @@
   <arg name="lidar_priority" default="0.25" />
   <arg name="lidar_discard_probability" default="0.9" />
 
+  <arg name="radar_shape_radius" default="0.15" />
+
   <arg name="enable_merge_filter" default="True" />
   <arg name="merge_growth_distance" default="0.65" />
   <arg name="min_merging_overlap_percent" default="0.5" />
   <arg name="min_merging_overlap_area" default="1.0" />
-  
+
   <arg name="enable_lane_index_filter" default="True" />
   <arg name="enable_pedestrian_grow_filter" default="True" />
 

code/mapping/src/mapping_data_integration.py

@@ -32,9 +32,6 @@
 from shapely.geometry import MultiPoint
 import shapely
 
-
-# from shapely.validation import orient
-
 from mapping.cfg import MappingIntegrationConfig
 from dynamic_reconfigure.server import Server
 
@@ -89,13 +86,15 @@ def __init__(self, name, **kwargs):
 
         # Sensor subscriptions:
 
+        self.lanemarkings = None
+
         self.new_subscription(
             topic=self.get_param("~lidar_topic", "/carla/hero/LIDAR"),
             msg_type=PointCloud2,
             callback=self.lidar_callback,
             qos_profile=1,
         )
-        self.lanemarkings = None
+
         self.new_subscription(
             topic=self.get_param(
                 "~lanemarkings_init_topic", "/paf/hero/mapping/init_lanemarkings"
@@ -342,13 +341,10 @@ def create_entities_from_clusters(self, sensortype="") -> List[Entity]:
         data = None
         if sensortype == "radar":
             data = self.radar_clustered_points_data
-            self.radar_clustered_points_data = None
         elif sensortype == "lidar":
             data = self.lidar_clustered_points_data
-            self.lidar_clustered_points_data = None
         elif sensortype == "vision":
             data = self.vision_clustered_points_data
-            self.vision_clustered_points_data = None
         else:
             raise ValueError(f"Unknown sensortype: {sensortype}")
 
@@ -382,27 +378,34 @@ def create_entities_from_clusters(self, sensortype="") -> List[Entity]:
 
             # Check if enough points for polygon are available
             if cluster_points_xy.shape[0] < 3:
-                continue
-
-            if not np.array_equal(cluster_points_xy[0], cluster_points_xy[-1]):
-                # add startpoint to close polygon
-                cluster_points_xy = np.vstack([cluster_points_xy, cluster_points_xy[0]])
-
-            cluster_polygon = MultiPoint(cluster_points_xy)
-            cluster_polygon_hull = cluster_polygon.convex_hull
-            if cluster_polygon_hull.is_empty or not cluster_polygon_hull.is_valid:
-                rospy.loginfo("Empty hull")
-                continue
-            if not isinstance(cluster_polygon_hull, shapely.Polygon):
-                rospy.loginfo("Cluster is not polygon, continue")
-                continue
-
-            shape = Polygon.from_shapely(
-                cluster_polygon_hull, make_centered=True  # type: ignore
-            )
+                if sensortype == "radar":
+                    shape = Circle(self.get_param("~lidar_shape_radius", 0.15))
+                    transform = Transform2D.new_translation(
+                        Vector2.new(cluster_points_xy[0, 0], cluster_points_xy[0, 1])
+                    )
+                else:
+                    continue
+            else:
+                if not np.array_equal(cluster_points_xy[0], cluster_points_xy[-1]):
+                    # add startpoint to close polygon
+                    cluster_points_xy = np.vstack(
+                        [cluster_points_xy, cluster_points_xy[0]]
+                    )
 
-            transform = shape.offset
-            shape.offset = Transform2D.identity()
+                cluster_polygon = MultiPoint(cluster_points_xy)
+                cluster_polygon_hull = cluster_polygon.convex_hull
+                if cluster_polygon_hull.is_empty or not cluster_polygon_hull.is_valid:
+                    rospy.loginfo("Empty hull")
+                    continue
+                if not isinstance(cluster_polygon_hull, shapely.Polygon):
+                    rospy.loginfo("Cluster is not polygon, continue")
+                    continue
+
+                shape = Polygon.from_shapely(
+                    cluster_polygon_hull, make_centered=True  # type: ignore
+                )
+                transform = shape.offset
+                shape.offset = Transform2D.identity()
 
             motion = None
             if motion_array_converted is not None:
@@ -523,10 +526,6 @@ def publish_new_map(self, timer_event=None):
 
             entities.extend(marks)
 
-        # Will be used when the new function for entity creation is implemented
-        # if self.get_param("enable_vision_points"):
-        #    entities.extend(self.entities_from_vision_points())
-
         stamp = rospy.get_rostime()
         map = Map(timestamp=stamp, entities=entities)