631 feature execution check function for planned local trajectory

code/mapping/ext_modules/.debug_enabled

@@ -1 +1 @@
-False
+True

code/mapping/ext_modules/mapping_common/map.py

@@ -3,6 +3,7 @@
 
 import shapely
 from shapely import STRtree
+import shapely.plotting
 import numpy as np
 import numpy.typing as npt
 
@@ -15,6 +16,7 @@
 from shapely.geometry import Polygon, LineString
 
 from mapping import msg
+from nav_msgs.msg import Path
 
 
 @dataclass
@@ -176,6 +178,40 @@ def get_entities_with_coverage(self, polygon, entities: List[Entity], coverage):
 
         return collision_entities
 
+    def entities_in_area(self, radius, only_in_front=True) -> List[Entity]:
+        """ Returns the entities without the hero car
+            in a given radius around the hero car.
+            Flag only_in_front decides if entities
+            behind the car, are returend too.
+        Only checks if the first entity is_hero
+
+        Returns:
+            List[Entity]: Entities in front and/or back of hero
+            without the hero car in given radius
+        """
+        entities = self.entities_without_hero()
+        entities_in_area = []
+
+        if only_in_front:
+            for entity in entities:
+                distance = np.sqrt(
+                    np.power(entity.transform.translation().x(), 2)
+                    + np.power(entity.transform.translation().y(), 2)
+                )
+                if entity.transform.translation().x() > 0 and distance < radius:
+                    entities_in_area.append(entity)
+            return entities_in_area
+        else:
+            for entity in entities:
+                distance = np.sqrt(
+                    np.power(entity.transform.translation().x(), 2)
+                    + np.power(entity.transform.translation().y(), 2)
+                )
+                if distance < radius:
+                    entities_in_area.append(entity)
+
+        return entities_in_area
+
     def build_tree(
         self,
         f: Optional[FlagFilter] = None,
@@ -229,6 +265,75 @@ def to_ros_msg(self) -> msg.Map:
         header = Header(stamp=self.timestamp)
         return msg.Map(header=header, entities=entities)
 
+    def lane_marking_left(self) -> Entity:
+        for entity in self.entities:
+            if entity.flags._is_lanemark is True and entity.position_index == 1:
+                return entity
+        return None
+
+    def lane_marking_right(self) -> Entity:
+        for entity in self.entities:
+            if entity.flags._is_lanemark is True and entity.position_index == -1:
+                return entity
+        return None
+
+    def check_trajectory(self, local_path: Path) -> int:
+        """
+        Checks the calculated local path on multiple conditions
+        return -1:  local_path is None
+        return 0:   Path has no obstacles / infringments
+        return 1:   Path is planned through a gap,
+                    which is too small for the car (without lanes)
+        return 2:   Path cuts the left lane marking
+        return 3:   Path cuts the right lane marking
+        """
+
+        # check if there is a local path
+        if local_path.poses is None:
+            return -1
+
+        # transform local path to shapely object
+        trajectory_shapely_points = []
+        for pos in local_path.poses:
+            trajectory_shapely_points.append(
+                shapely.Point(pos.pose.position.x, pos.pose.position.y)
+            )
+        local_trajectory = shapely.LineString(trajectory_shapely_points)
+        # widen the Linestring to an area representing the cars width
+        local_trajectory_buffer = local_trajectory.buffer(1, 3)
+
+        # Check path collision with other entities
+        obstacles = self.entities_in_area(10)
+        for entity in obstacles:
+            if (
+                shapely.intersection(entity.to_shapely().poly, local_trajectory_buffer)
+                and entity.flags._is_lanemark is False
+            ):
+                return 1
+
+        # decrease buffer size for lane checkking
+        local_trajectory_buffer = local_trajectory.buffer(0.5, 3)
+        # Check left lane
+        left_lane = self.lane_marking_left()
+        if left_lane is not None:
+            if shapely.intersection(
+                left_lane.to_shapely().poly, local_trajectory_buffer
+            ):
+                # given path crosses lane marking on the left
+                return 2
+
+        # Check right lane
+        right_lane = self.lane_marking_right()
+        if right_lane is not None:
+            if shapely.intersection(
+                right_lane.to_shapely().poly, local_trajectory_buffer
+            ):
+                # given path crosses lane marking on the right
+                return 3
+
+        # given path is okay and is not colliding
+        return 0
+
 
 @dataclass(init=False)
 class MapTree:

code/mapping/src/mapping_data_integration.py

@@ -54,6 +54,7 @@ def __init__(self, name, **kwargs):
             callback=self.lanemarkings_callback,
             qos_profile=1,
         )
+        self.lidar_data = None
         self.new_subscription(
             topic=self.get_param("~hero_speed_topic", "/carla/hero/Speed"),
             msg_type=CarlaSpeedometer,
@@ -106,6 +107,9 @@ def __init__(self, name, **kwargs):
         self.rate = self.get_param("~map_publish_rate", 20)
         self.new_timer(1.0 / self.rate, self.publish_new_map)
 
+    def callback(self, data: Path):
+        self.local_trajectory = data
+
     def hero_speed_callback(self, data: CarlaSpeedometer):
         self.hero_speed = data
 
@@ -146,6 +150,7 @@ def radar_marker_callback(self, data: MarkerArray):
     def lidar_callback(self, data: PointCloud2):
         self.lidar_data = data
 
+<<<<<<<<< Temporary merge branch 1
     def entities_from_lidar_marker(self) -> List[Entity]:
         data = self.lidar_marker_data
         if data is None or not hasattr(data, "markers") or data.markers is None:
@@ -336,6 +341,7 @@ def publish_new_map(self, timer_event=None):
         stamp = rospy.get_rostime()
         map = Map(timestamp=stamp, entities=entities)
         msg = map.to_ros_msg()
+
         self.map_publisher.publish(msg)
 
 

code/mapping/src/path_test.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python
+import ros_compatibility as roscomp
+import rospy
+from geometry_msgs.msg import PoseStamped
+from ros_compatibility.node import CompatibleNode
+from nav_msgs.msg import Path
+import random
+
+
+class TestPath(CompatibleNode):
+
+    def __init__(self):
+        """
+        This node handles the translation from the static main frame to the
+        moving hero frame. The hero frame always moves and rotates as the
+        ego vehicle does. The hero frame is used by sensors like the lidar.
+        Rviz also uses the hero frame. The main frame is used for planning.
+        """
+        super(TestPath, self).__init__("TestPath")
+        self.loginfo("TestPath node started")
+        self.local_trajectory = Path()
+
+        self.publisher = self.new_publisher(
+            msg_type=Path,
+            topic="test_trajectory",
+            qos_profile=1,
+        )
+
+        self.rate = 0.5
+        self.new_timer(0.5, self.generate_trajectory)
+
+    def generate_trajectory(self, timer_event=None) -> Path:
+        path_msg = Path()
+        path_msg.header = rospy.Header()
+        path_msg.header.stamp = rospy.Time.now()
+        path_msg.header.frame_id = "hero"
+        path_msg.poses = []
+        curve = 0
+        factor = 1
+        if random.uniform(-1, 1) < 0:
+            factor = -1
+
+        for i in range(0, 10):
+            pos = PoseStamped()
+            pos.header.stamp = rospy.Time.now()
+            pos.header.frame_id = "hero"
+            curve = curve + 0.01 * i * random.uniform(0, 1)
+            pos.pose.position.x = i
+            pos.pose.position.y = curve * factor
+            pos.pose.position.z = 0
+            pos.pose.orientation.w = 1
+            path_msg.poses.append(pos)
+
+        self.local_trajectory = path_msg
+        self.publisher.publish(path_msg)
+
+
+if __name__ == "__main__":
+    name = "path_test_node"
+    roscomp.init(name)
+    node = TestPath()
+    node.spin()