diff --git a/build/Taskfile b/build/Taskfile index ef40a638..9ad466af 100644 --- a/build/Taskfile +++ b/build/Taskfile @@ -32,7 +32,7 @@ task:shell() { ########################################## task:install() { task:install:git_hooks - task:gitconfig:copy + #task:gitconfig:copy install:gpu-support docker:install } diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index 4cab6ea2..41dc7e4e 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -73,7 +73,7 @@ RUN apt-get update && apt-get install -y \ ros-noetic-ackermann-msgs ros-noetic-derived-object-msgs \ ros-noetic-carla-msgs ros-noetic-pcl-conversions \ ros-noetic-rviz ros-noetic-rqt ros-noetic-pcl-ros ros-noetic-rosbridge-suite ros-noetic-rosbridge-server \ - ros-noetic-robot-pose-ekf \ + ros-noetic-robot-pose-ekf ros-noetic-ros-numpy \ ros-noetic-py-trees-ros ros-noetic-rqt-py-trees ros-noetic-rqt-reconfigure SHELL ["/bin/bash", "-c"] diff --git a/code/acting/launch/acting.launch b/code/acting/launch/acting.launch index 7f501d61..db8e8576 100644 --- a/code/acting/launch/acting.launch +++ b/code/acting/launch/acting.launch @@ -21,7 +21,7 @@ - + diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index 5831b05d..39882333 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -8,7 +8,8 @@ from std_msgs.msg import Float32, Float32MultiArray from nav_msgs.msg import Path -SPEED_LIMIT_DEFAULT: float = 36.0 +# TODO put back to 36 when controller can handle it +SPEED_LIMIT_DEFAULT: float = 6 # 36.0 class VelocityController(CompatibleNode): @@ -38,10 +39,9 @@ def __init__(self): self.__get_current_velocity, qos_profile=1) - self.speed_limit_sub: Subscriber = self.new_subscription( + self.speed_limit_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/speed_limit", - self.__get_speed_limit, qos_profile=1) self.max_tree_v_sub: Subscriber = self.new_subscription( @@ -109,9 +109,10 @@ def loop(timer_event=None): """ if self.__max_velocity is None: self.logdebug("VehicleController hasn't received max_velocity" - " yet and can therefore not publish a " - "throttle value") - return + " yet. max_velocity has been set to" + f"default value {SPEED_LIMIT_DEFAULT}") + # return + self.__max_velocity = SPEED_LIMIT_DEFAULT if self.__current_velocity is None: self.logdebug("VehicleController hasn't received " @@ -141,7 +142,6 @@ def loop(timer_event=None): self.logerr("Velocity controller doesn't support backward " "driving yet.") return - v = min(self.__max_velocity, self.__max_tree_v) v = min(v, self.__speed_limit) @@ -191,6 +191,7 @@ def __current_position_callback(self, data: PoseStamped): self.__current_wp_index += 1 self.__speed_limit = \ self.__speed_limits_OD[self.__current_wp_index] + self.speed_limit_pub.publish(self.__speed_limit) def main(args=None): diff --git a/code/agent/config/dev_objects.json b/code/agent/config/dev_objects.json index 09995008..d65936e6 100644 --- a/code/agent/config/dev_objects.json +++ b/code/agent/config/dev_objects.json @@ -109,7 +109,7 @@ "z": 1.60, "roll": 0.0, "pitch": 0.0, - "yaw": -45.0 + "yaw": 0.0 }, "range": 85, "rotation_frequency": 10, diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index bf6b1435..b62eadde 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -4,9 +4,11 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Path1 + - /PointCloud23 + - /PointCloud24 + - /PointCloud25 Splitter Ratio: 0.5 - Tree Height: 266 + Tree Height: 308 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -70,13 +72,13 @@ Visualization Manager: - Alpha: 1 Class: rviz_plugin_tutorials/Imu Color: 204; 51; 204 - Enabled: true + Enabled: false History Length: 1 Name: Imu Queue Size: 10 Topic: /carla/hero/IMU Unreliable: false - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -117,7 +119,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -132,7 +134,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path @@ -157,6 +159,90 @@ Visualization Manager: Topic: /paf/hero/trajectory Unreliable: false Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: /carla/hero/LIDAR_filtered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: /carla/hero/LIDAR_filtered_rear_left + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: /carla/hero/LIDAR_filtered_rear_right + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -185,7 +271,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 49.498538970947266 + Distance: 34.785499572753906 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -201,19 +287,19 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.33039820194244385 + Pitch: 0.19039836525917053 Target Frame: - Yaw: 1.720420479774475 + Yaw: 4.520427227020264 Saved: ~ Window Geometry: Camera: collapsed: false Displays: collapsed: false - Height: 1043 + Height: 1376 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000030400000379fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000193000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001d4000001e00000001600ffffff000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000003610000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000304000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000022a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026b000002960000001600ffffff000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b80000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000599000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -222,6 +308,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 0 - Y: 0 + Width: 2488 + X: 1992 + Y: 27 diff --git a/code/agent/src/agent/agent.py b/code/agent/src/agent/agent.py index 46d06855..e2732f00 100755 --- a/code/agent/src/agent/agent.py +++ b/code/agent/src/agent/agent.py @@ -27,7 +27,7 @@ def sensors(self): 'yaw': 0.0, 'width': 300, 'height': 200, 'fov': 100}, {'type': 'sensor.lidar.ray_cast', 'id': 'LIDAR', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, - 'yaw': -45.0}, + 'yaw': 0.0}, {'type': 'sensor.other.radar', 'id': 'RADAR', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': -45.0, 'horizontal_fov': 30, 'vertical_fov': 30}, @@ -35,7 +35,7 @@ def sensors(self): 'x': 0.7, 'y': -0.4, 'z': 1.60}, {'type': 'sensor.other.imu', 'id': 'IMU', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, - 'yaw': -45.0}, + 'yaw': 0.0}, {'type': 'sensor.opendrive_map', 'id': 'OpenDRIVE', 'reading_frequency': 1}, {'type': 'sensor.speedometer', 'id': 'Speed'}, diff --git a/code/mock/launch/mock.launch b/code/mock/launch/mock.launch index cc756851..a9298ca6 100755 --- a/code/mock/launch/mock.launch +++ b/code/mock/launch/mock.launch @@ -7,14 +7,14 @@ - - + - + diff --git a/code/perception/CMakeLists.txt b/code/perception/CMakeLists.txt index d9b5aafb..daa62be3 100644 --- a/code/perception/CMakeLists.txt +++ b/code/perception/CMakeLists.txt @@ -10,13 +10,18 @@ project(perception) find_package(catkin REQUIRED) ## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +find_package(catkin REQUIRED COMPONENTS + rosout + rospy + std_msgs + message_generation +) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## @@ -43,11 +48,11 @@ find_package(catkin REQUIRED) ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) +add_message_files( + FILES + Waypoint.msg + LaneChange.msg +) ## Generate services in the 'srv' folder # add_service_files( @@ -64,10 +69,10 @@ find_package(catkin REQUIRED) # ) ## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) +generate_messages( + DEPENDENCIES + std_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 9c7c2828..a70df7a8 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -32,5 +32,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/perception/msg/LaneChange.msg b/code/perception/msg/LaneChange.msg new file mode 100644 index 00000000..2121caa5 --- /dev/null +++ b/code/perception/msg/LaneChange.msg @@ -0,0 +1,3 @@ +float32 distance +bool isLaneChange +int8 roadOption diff --git a/code/perception/msg/Waypoint.msg b/code/perception/msg/Waypoint.msg new file mode 100644 index 00000000..6091552a --- /dev/null +++ b/code/perception/msg/Waypoint.msg @@ -0,0 +1,2 @@ +float32 distance +bool isStopLine diff --git a/code/perception/src/PositionPublisher.py b/code/perception/src/PositionPublisher.py index 48e820bb..d5f6ec8f 100755 --- a/code/perception/src/PositionPublisher.py +++ b/code/perception/src/PositionPublisher.py @@ -9,6 +9,7 @@ from ros_compatibility.node import CompatibleNode from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped from coordinate_transformation import CoordinateTransformer, GeoRef +import numpy as np import rospy @@ -55,6 +56,9 @@ def __init__(self): "/paf/" + self.role_name + "/current_pos", qos_profile=1) + self.avg_xyz = np.zeros((5, 3)) + self.__publish_counter = 0 + def update_pos_filtered_data(self, data: PoseWithCovarianceStamped): self.current_pos_gps = data @@ -92,6 +96,17 @@ def publish_current_pos(self): """ # self.loginfo("publishing data") temp_pos = self.update_current_pos() + pos = np.matrix([temp_pos.pose.position.x, + temp_pos.pose.position.y, + temp_pos.pose.position.z]) + self.avg_xyz = np.roll(self.avg_xyz, -1, axis=0) + self.avg_xyz[-1] = pos + + # compute average of each column + avg_x, avg_y, avg_z = np.mean(self.avg_xyz, axis=0) + temp_pos.pose.position.x = avg_x + temp_pos.pose.position.y = avg_y + temp_pos.pose.position.z = avg_z self.pos_publisher.publish(temp_pos) def run(self): diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py new file mode 100755 index 00000000..a6920408 --- /dev/null +++ b/code/perception/src/global_plan_distance_publisher.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python + +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode +from geometry_msgs.msg import PoseStamped +from carla_msgs.msg import CarlaRoute +from perception.msg import Waypoint, LaneChange +import math +# import rospy + + +class GlobalPlanDistance(CompatibleNode): + """ + Creates a node that publishes the distance to the next waypoint and the + stop line of an intersection specified by the global waypoint in front of + it. + """ + + def __init__(self): + """ + Constructor + :return: + """ + + super(GlobalPlanDistance, self).__init__('global_plan_distance' + '_publisher') + self.loginfo("GlobalPlanDistance node started") + + # basic info + self.role_name = self.get_param("role_name", "hero") + self.control_loop_rate = self.get_param("control_loop_rate", "0.05") + self.publish_loop_rate = 0.05 # 20Hz rate like the sensors + + self.current_pos = PoseStamped() + self.global_route = None + self.road_options = None + + # Subscriber + self.pos_subscriber = self.new_subscription( + PoseStamped, + "/paf/" + self.role_name + "/current_pos", + self.update_position, + qos_profile=1) + + self.global_plan_subscriber = self.new_subscription( + CarlaRoute, + "/carla/" + self.role_name + "/global_plan", + self.update_global_route, + qos_profile=1) + + # Publisher + # self.stopline_publisher = self.new_publisher( + # Float32, + # "/paf/" + self.role_name + "/stopline_distance", + # qos_profile=1) + + self.waypoint_publisher = self.new_publisher( + Waypoint, + "/paf/" + self.role_name + "/waypoint_distance", + qos_profile=1) + + self.lane_change_publisher = self.new_publisher( + LaneChange, + "/paf/" + self.role_name + "/lane_change_distance", + qos_profile=1) + + def update_position(self, pos): + """ + Updates the current position based on the most upto date + IMU, Speedometer and GNSS sensor data. + :return: + """ + def distance(a, b): + d_x = (a.x - b.x) ** 2 + d_y = (a.y - b.y) ** 2 + return math.sqrt(d_x + d_y) + + self.current_pos = pos.pose + + # check if the global route has been published and that there are still + # points to navigate to + if self.global_route is not None and self.global_route: + current_distance = distance(self.global_route[0].position, + self.current_pos.position) + next_distance = distance(self.global_route[1].position, + self.current_pos.position) + + # if the road option indicates an intersection, the distance to the + # next waypoint is also the distance to the stop line + if self.road_options[0] < 4: + # print("publish waypoint") + self.waypoint_publisher.publish( + Waypoint(current_distance, True)) + self.lane_change_publisher.publish( + LaneChange(current_distance, False, self.road_options[0])) + else: + self.waypoint_publisher.publish( + Waypoint(current_distance, False)) + if self.road_options[0] == 5 or self.road_options[0] == 6: + self.lane_change_publisher.publish( + LaneChange(current_distance, True, + self.road_options[0])) + # if we reached the next waypoint, pop it and the next point will + # be published + if current_distance < 2.5 or next_distance < current_distance: + self.road_options.pop(0) + self.global_route.pop(0) + + if self.road_options[0] in {5, 6} and \ + self.road_options[0] == self.road_options[1]: + self.road_options[1] = 4 + + print(f"next road option = {self.road_options[0]}") + + def update_global_route(self, route): + """ + Callback if the global route is published, saves the route and road + options locally + :param: route, CarlaRoute holding global route + :return: + """ + if self.global_route is None: + self.global_route = list(route.poses) + self.road_options = list(route.road_options) + self.road_options.pop(0) + self.global_route.pop(0) + + def run(self): + """ + Control loop + + :return: + """ + + self.spin() + + +def main(args=None): + """ + main function + + :param args: + :return: + """ + + roscomp.init("position_publisher", args=args) + try: + node = GlobalPlanDistance() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() + + +if __name__ == "__main__": + main() diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py new file mode 100755 index 00000000..fcd75096 --- /dev/null +++ b/code/perception/src/lidar_distance.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python +import rospy +import ros_numpy +import numpy as np +import lidar_filter_utility +from sensor_msgs.msg import PointCloud2, Range + + +class LidarDistance(): + """ See doc/06_perception/02_lidar_distance_utility.md on + how to configute this node + """ + + def callback(self, data): + """ Callback function, filters a PontCloud2 message + by restrictions defined in the launchfile. + + Publishes a range message containing the farest and + the closest point of the filtered result. Additionally, + publishes the filtered result as PointCloud2 + on the topic defined in the launchfile. + + :param data: a PointCloud2 + """ + coordinates = ros_numpy.point_cloud2.pointcloud2_to_array(data) + + # https://stackoverflow.com/questions/44295375/how-to-slice-a-numpy- + # ndarray-made-up-of-numpy-void-numbers + bit_mask = lidar_filter_utility.bounding_box( + coordinates, + max_x=rospy.get_param('~max_x', np.inf), + min_x=rospy.get_param('~min_x', -np.inf), + max_y=rospy.get_param('~max_y', np.inf), + min_y=rospy.get_param('~min_y', -np.inf), + max_z=rospy.get_param('~max_z', np.inf), + min_z=rospy.get_param('~min_z', -np.inf), + ) + + # Filter coordinates based in generated bit_mask + coordinates = coordinates[bit_mask] + + # Create pointcloud from manipulated data + coordinates_manipulated = ros_numpy \ + .point_cloud2.array_to_pointcloud2(coordinates) + coordinates_manipulated.header = data.header + + # Publish manipulated pointCloud2 + self.pub_pointcloud.publish(coordinates_manipulated) + + # https://stackoverflow.com/questions/1401712/how-can-the-euclidean- + # distance-be-calculated-with-numpy + coordinates_xyz = np.array( + lidar_filter_utility.remove_field_name(coordinates, 'intensity') + .tolist() + ) + distances = np.array( + [np.linalg.norm(c - [0, 0, 0]) for c in coordinates_xyz]) + + if len(distances) > 0: + range_msg = Range() + range_msg.max_range = max(distances) + range_msg.min_range = min(distances) + range_msg.range = min(distances) + + self.pub_range.publish(range_msg) + + def listener(self): + """ Initializes the node and it's publishers + """ + # run simultaneously. + rospy.init_node('lidar_distance') + + self.pub_pointcloud = rospy.Publisher( + rospy.get_param( + '~point_cloud_topic', + '/carla/hero/' + rospy.get_namespace() + '_filtered' + ), + PointCloud2 + ) + self.pub_range = rospy.Publisher( + rospy.get_param( + '~range_topic', + '/carla/hero/' + rospy.get_namespace() + '_range' + ), + Range + ) + + rospy.Subscriber(rospy.get_param('~source_topic', "/carla/hero/LIDAR"), + PointCloud2, self.callback) + + # spin() simply keeps python from exiting until this node is stopped + rospy.spin() + + +if __name__ == '__main__': + lidar_distance = LidarDistance() + lidar_distance.listener() diff --git a/code/perception/src/lidar_filter_utility.py b/code/perception/src/lidar_filter_utility.py new file mode 100644 index 00000000..4cd50260 --- /dev/null +++ b/code/perception/src/lidar_filter_utility.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python +import numpy as np + + +# https://gist.github.com/bigsnarfdude/bbfdf343cc2fc818dc08b58c0e1374ae +def bounding_box(points, min_x=-np.inf, max_x=np.inf, min_y=-np.inf, + max_y=np.inf, min_z=-np.inf, max_z=np.inf): + """ Compute a bounding_box filter on the given points + + Parameters + ---------- + points: (n,3) array + The array containing all the points's coordinates. + Expected format: + array([ + [x1,y1,z1], + ..., + [xn,yn,zn]]) + + min_i, max_i: float + The bounding box limits for each coordinate. + If some limits are missing, the default values + are -infinite for the min_i and infinite for the max_i. + + Returns + ------- + bb_filter : boolean array + The boolean mask indicating wherever a point should be + keeped or not. The size of the boolean mask will be the + same as the number of given points. + + """ + + bound_x = np.logical_and(points['x'] > min_x, points['x'] < max_x) + bound_y = np.logical_and(points['y'] > min_y, points['y'] < max_y) + bound_z = np.logical_and(points['z'] > min_z, points['z'] < max_z) + + bb_filter = bound_x & bound_y & bound_z + + return bb_filter + + +# https://stackoverflow.com/questions/15575878/how-do-you-remove-a-column-from-a-structured-numpy-array +def remove_field_name(a, name): + """ Removes a column from a structured numpy array + + :param a: structured numoy array + :param name: name of the column to remove + :return: structured numpy array without column + """ + names = list(a.dtype.names) + if name in names: + names.remove(name) + b = a[names] + return b diff --git a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py b/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py index 93634e21..fddad06b 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py +++ b/code/planning/behavior_agent/src/behavior_agent/behavior_tree.py @@ -71,6 +71,22 @@ def grow_a_tree(role_name): ]) ]), Selector("Laneswitching", children=[ + Sequence("Laneswitch", + children=[ + behaviours.road_features.LaneChangeAhead + ("Lane Change Ahead"), + Sequence("Lane Change Actions", + children=[ + behaviours.lane_change.Approach + ("Approach Change"), + behaviours.lane_change.Wait + ("Wait Change"), + behaviours.lane_change.Enter + ("Enter Change"), + behaviours.lane_change.Leave + ("Leave Change") + ]) + ]), Inverter(Selector("Overtaking", children=[ behaviours.traffic_objects.NotSlowedByCarInFront ("Not Slowed By Car in Front?"), diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py index c32f8209..5902aa61 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py +++ b/code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py @@ -1,2 +1,3 @@ from . import topics2blackboard, avoid_collisions, road_features -from . import intersection, roundabout, stop, traffic_objects, maneuvers, meta +from . import intersection, roundabout, stop, traffic_objects, maneuvers, meta,\ + lane_change diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py index 6b087ddc..fedde3a8 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py +++ b/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py @@ -29,6 +29,7 @@ def __init__(self, name): :param name: name of the behaviour """ super(Approach, self).__init__(name) + rospy.loginfo("Approach started") def setup(self, timeout): """ @@ -100,10 +101,10 @@ def update(self): self.traffic_light_detected = True # Update stopline Info - _dis = self.blackboard.get("/paf/hero/intersection_distance") + _dis = self.blackboard.get("/paf/hero/waypoint_distance") if _dis is not None: - self.intersection_distance = _dis.data - rospy.loginfo(f"Stopline distance: {self.intersection_distance}") + self.stopline_distance = _dis.distance + self.stopline_detected = _dis.isStopLine # Update stop sign Info stop_sign_msg = self.blackboard.get("/paf/hero/stop_sign") @@ -112,20 +113,25 @@ def update(self): self.stop_distance = stop_sign_msg.distance # calculate virtual stopline - if self.intersection_distance != np.inf: - self.virtual_stopline_distance = self.intersection_distance + if self.stopline_distance != np.inf and self.stopline_detected: + self.virtual_stopline_distance = self.stopline_distance elif self.traffic_light_detected: self.virtual_stopline_distance = self.traffic_light_distance elif self.stop_sign_detected: self.virtual_stopline_distance = self.stop_distance + else: + self.virtual_stopline_distance = 0.0 + + rospy.loginfo(f"Stopline distance: {self.virtual_stopline_distance}") + target_distance = 3.0 # calculate speed needed for stopping - v_stop = max(convert_to_ms(5.), - convert_to_ms((self.virtual_stopline_distance / 30) ** 1.5 + v_stop = max(convert_to_ms(10.), + convert_to_ms((self.virtual_stopline_distance / 30) * 50)) if v_stop > convert_to_ms(50.0): - v_stop = convert_to_ms(30.0) - if self.virtual_stopline_distance < 3.5: + v_stop = convert_to_ms(50.0) + if self.virtual_stopline_distance < target_distance: v_stop = 0.0 # stop when there is no or red/yellow traffic light or a stop sign is # detected @@ -150,12 +156,14 @@ def update(self): else: rospy.logwarn("no speedometer connected") return py_trees.common.Status.RUNNING - if self.virtual_stopline_distance > 5.0: + if self.virtual_stopline_distance > target_distance: # too far + print("still approaching") return py_trees.common.Status.RUNNING elif speed < convert_to_ms(2.0) and \ - self.virtual_stopline_distance < 5.0: + self.virtual_stopline_distance < target_distance: # stopped + print("stopped") return py_trees.common.Status.SUCCESS elif speed > convert_to_ms(5.0) and \ self.virtual_stopline_distance < 6.0 and \ @@ -172,15 +180,10 @@ def update(self): # ran over line return py_trees.common.Status.SUCCESS - next_lanelet_msg = self.blackboard.get("/psaf/ego_vehicle/" - "next_lanelet") - # TODO should be replaced by the next glob path point, adjust values - if next_lanelet_msg is None: - return py_trees.common.Status.FAILURE - if next_lanelet_msg.distance < 12 and not next_lanelet_msg.\ - isInIntersection: + if self.virtual_stopline_distance < target_distance and \ + not self.stopline_detected: rospy.loginfo("Leave intersection!") - self.update_local_path(leave_intersection=True) + # self.update_local_path(leave_intersection=True) return py_trees.common.Status.SUCCESS else: return py_trees.common.Status.RUNNING @@ -261,12 +264,20 @@ def update(self): to green or no traffic light is detected """ light_status_msg = self.blackboard.get("/paf/hero/traffic_light") - intersection_clear_msg = self.blackboard.get("/paf/hero/" - "intersection_clear") - if intersection_clear_msg is not None: - intersection_clear = intersection_clear_msg.data - else: - intersection_clear = False + # intersection_clear_msg = self.blackboard.get("/paf/hero/" + # "intersection_clear") + distance_lidar = self.blackboard.get("/carla/hero/LIDAR_range") + intersection_clear = True + if distance_lidar is not None: + # if distance smaller than 10m, intersection is blocked + if distance_lidar.min_range < 10.0: + intersection_clear = False + else: + intersection_clear = True + # if intersection_clear_msg is not None: + # intersection_clear = intersection_clear_msg.data + # else: + # intersection_clear = False if light_status_msg is not None: traffic_light_status = light_status_msg.color @@ -353,8 +364,9 @@ def initialise(self): return True else: traffic_light_status = light_status_msg.color + if traffic_light_status == "": - self.target_speed_pub.publish(convert_to_ms(10.0)) + self.target_speed_pub.publish(convert_to_ms(18.0)) else: rospy.loginfo(f"Light Status: {traffic_light_status}") self.target_speed_pub.publish(convert_to_ms(50.0)) @@ -375,23 +387,18 @@ def update(self): py_trees.common.Status.FAILURE, if no next path point can be detected. """ - # TODO this part needs to be refurbished when we have a publisher for - # the next global way point - # next_lanelet_msg = self.blackboard.get("/psaf/ego_vehicle/" - # "next_lanelet") - - rospy.loginfo("Through intersection") - return py_trees.common.Status.SUCCESS - - # if next_lanelet_msg is None: - # return py_trees.common.Status.FAILURE - # if next_lanelet_msg.distance < 12 and not next_lanelet_msg.\ - # isInIntersection: - # rospy.loginfo("Leave intersection!") - # self.update_local_path(leave_intersection=True) - # return py_trees.common.Status.SUCCESS - # else: - # return py_trees.common.Status.RUNNING + next_waypoint_msg = self.blackboard.get("/paf/hero/waypoint_distance") + + if next_waypoint_msg is None: + return py_trees.common.Status.FAILURE + # if next_waypoint_msg.distance < 5 and + # not next_waypoint_msg.isStopLine: + if next_waypoint_msg.distance < 5: + rospy.loginfo("Drive through intersection!") + # self.update_local_path(leave_intersection=True) + return py_trees.common.Status.RUNNING + else: + return py_trees.common.Status.SUCCESS def terminate(self, new_status): """ @@ -451,9 +458,9 @@ def initialise(self): the street speed limit. """ rospy.loginfo("Leave Intersection") - street_speed_msg = self.blackboard.get("/paf/hero/street_limit") + street_speed_msg = self.blackboard.get("/paf/hero/speed_limit") if street_speed_msg is not None: - self.target_speed_pub.publish(street_speed_msg.speed) + self.target_speed_pub.publish(street_speed_msg.data) return True def update(self): diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py new file mode 100644 index 00000000..d4b84d19 --- /dev/null +++ b/code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py @@ -0,0 +1,447 @@ +import py_trees +import numpy as np +from std_msgs.msg import Float32 +# from nav_msgs.msg import Odometry +# from custom_carla_msgs.srv import UpdateLocalPath + +import rospy + +""" +Source: https://github.com/ll7/psaf2 +""" + + +def convert_to_ms(speed): + return speed / 3.6 + + +class Approach(py_trees.behaviour.Behaviour): + """ + This behaviour is executed when the ego vehicle is in close proximity of + an intersection and behaviours.road_features.intersection_ahead is + triggered. It than handles the approaching the intersection, slowing the + vehicle down appropriately. + """ + def __init__(self, name): + """ + Minimal one-time initialisation. Other one-time initialisation + requirements should be met via the setup() method. + :param name: name of the behaviour + """ + super(Approach, self).__init__(name) + rospy.loginfo("Approach started") + + def setup(self, timeout): + """ + Delayed one-time initialisation that would otherwise interfere with + offline rendering of this behaviour in a tree to dot graph or + validation of the behaviour's configuration. + + This initializes the blackboard to be able to access data written to it + by the ROS topics and the target speed publisher. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as the set up is successful. + """ + self.target_speed_pub = rospy.Publisher("/paf/hero/" + "max_tree_velocity", + Float32, queue_size=1) + # rospy.wait_for_service('update_local_path') # TODO is this necessary? + # self.update_local_path = + # rospy.ServiceProxy("update_local_path", UpdateLocalPath) + self.blackboard = py_trees.blackboard.Blackboard() + return True + + def initialise(self): + """ + When is this called? + The first time your behaviour is ticked and anytime the status is not + RUNNING thereafter. + What to do here? + Any initialisation you need before putting your behaviour to work. + This initializes the variables needed to save information about the + stop line, stop signs and the traffic light. + """ + rospy.loginfo("Approaching Change") + # self.update_local_path(approach_intersection=True) + self.start_time = rospy.get_time() + self.change_detected = False + self.change_distance = np.inf + self.virtual_change_distance = np.inf + self.target_speed_pub.publish(convert_to_ms(30.0)) + + def update(self): + """ + When is this called? + Every time your behaviour is ticked. + What to do here? + - Triggering, checking, monitoring. Anything...but do not block! + - Set a feedback message + - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] + Gets the current traffic light status, stop sign status + and the stop line distance + :return: py_trees.common.Status.RUNNING, if too far from intersection + py_trees.common.Status.SUCCESS, if stopped in front of inter- + section or entered the intersection + py_trees.common.Status.FAILURE, if no next path point can be + detected. + """ + # Update stopline Info + _dis = self.blackboard.get("/paf/hero/lane_change_distance") + if _dis is not None: + self.change_distance = _dis.distance + self.change_detected = _dis.isLaneChange + self.change_option = _dis.roadOption + rospy.loginfo(f"Change distance: {self.change_distance}") + + # calculate virtual stopline + if self.change_distance != np.inf and self.change_detected: + self.virtual_change_distance = self.change_distance + + # calculate speed needed for stopping + v_stop = max(convert_to_ms(5.), + convert_to_ms((self.virtual_change_distance / 30) ** 1.5 + * 50)) + if v_stop > convert_to_ms(50.0): + v_stop = convert_to_ms(30.0) + # slow down before lane change + if self.virtual_change_distance < 15.0: + if self.change_option == 5: + distance_lidar = self.blackboard. \ + get("/carla/hero/LIDAR_range_rear_left") + elif self.change_option == 6: + distance_lidar = self.blackboard. \ + get("/carla/hero/LIDAR_range_rear_right") + else: + distance_lidar = None + + if distance_lidar is not None and distance_lidar.min_range > 15.0: + rospy.loginfo("Change is free not slowing down!") + # self.update_local_path(leave_intersection=True) + return py_trees.common.Status.SUCCESS + else: + v_stop = 0.5 + rospy.loginfo(f"Change blocked slowing down: {v_stop}") + self.target_speed_pub.publish(v_stop) + + # get speed + speedometer = self.blackboard.get("/carla/hero/Speed") + if speedometer is not None: + speed = speedometer.speed + else: + rospy.logwarn("no speedometer connected") + return py_trees.common.Status.RUNNING + if self.virtual_change_distance > 5.0: + # too far + print("still approaching") + return py_trees.common.Status.RUNNING + elif speed < convert_to_ms(2.0) and \ + self.virtual_change_distance < 5.0: + # stopped + print("stopped") + return py_trees.common.Status.SUCCESS + elif speed > convert_to_ms(5.0) and \ + self.virtual_change_distance < 3.5: + # running over line + return py_trees.common.Status.SUCCESS + + if self.virtual_change_distance < 5 and not self.change_detected: + rospy.loginfo("Leave Change!") + # self.update_local_path(leave_intersection=True) + return py_trees.common.Status.SUCCESS + else: + return py_trees.common.Status.RUNNING + + def terminate(self, new_status): + """ + When is this called? + Whenever your behaviour switches to a non-running state. + - SUCCESS || FAILURE : your behaviour's work cycle has finished + - INVALID : a higher priority branch has interrupted, or shutting + down + writes a status message to the console when the behaviour terminates + :param new_status: new state after this one is terminated + """ + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, + self.status, + new_status)) + + +class Wait(py_trees.behaviour.Behaviour): + """ + This behavior handles the waiting in front of the stop line at the inter- + section until there either is no traffic light, the traffic light is + green or the intersection is clear. + """ + def __init__(self, name): + """ + Minimal one-time initialisation. Other one-time initialisation + requirements should be met via the setup() method. + :param name: name of the behaviour + """ + super(Wait, self).__init__(name) + + def setup(self, timeout): + """ + Delayed one-time initialisation that would otherwise interfere with + offline rendering of this behaviour in a tree to dot graph or + validation of the behaviour's configuration. + + This initializes the blackboard to be able to access data written to it + by the ROS topics and the target speed publisher. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as the set up is successful. + """ + self.target_speed_pub = rospy.Publisher("/paf/hero/" + "max_tree_velocity", Float32, + queue_size=1) + self.blackboard = py_trees.blackboard.Blackboard() + return True + + def initialise(self): + """ + When is this called? + The first time your behaviour is ticked and anytime the status is + not RUNNING thereafter. + What to do here? + Any initialisation you need before putting your behaviour to work. + This just prints a state status message. + """ + self.old_ro = self.blackboard.\ + get("/paf/hero/lane_change_distance") + rospy.loginfo("Wait Change") + return True + + def update(self): + """ + When is this called? + Every time your behaviour is ticked. + What to do here? + - Triggering, checking, monitoring. Anything...but do not block! + - Set a feedback message + - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] + Waits in front of the intersection until there is a green light, the + intersection is clear or no traffic light at all. + :return: py_trees.common.Status.RUNNING, while traffic light is yellow + or red + py_trees.common.Status.SUCCESS, if the traffic light switched + to green or no traffic light is detected + """ + + lcd = self.blackboard.\ + get("/paf/hero/lane_change_distance") + + if self.old_ro.distance < lcd.distance + 1 or \ + lcd.roadOption != self.old_ro.roadOption: + return py_trees.common.Status.SUCCESS + self.old_ro = lcd + road_option = lcd.roadOption + if road_option == 5: + distance_lidar = self.blackboard. \ + get("/carla/hero/LIDAR_range_rear_left") + elif road_option == 6: + distance_lidar = self.blackboard. \ + get("/carla/hero/LIDAR_range_rear_right") + else: + distance_lidar = None + + change_clear = False + if distance_lidar is not None: + # if distance smaller than 15m, change is blocked + if distance_lidar.min_range < 15.0: + change_clear = False + else: + change_clear = True + if not change_clear: + rospy.loginfo("Change blocked") + return py_trees.common.Status.RUNNING + else: + rospy.loginfo("Change clear") + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status): + """ + When is this called? + Whenever your behaviour switches to a non-running state. + - SUCCESS || FAILURE : your behaviour's work cycle has finished + - INVALID : a higher priority branch has interrupted, or shutting + down + writes a status message to the console when the behaviour terminates + :param new_status: new state after this one is terminated + """ + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, + self.status, + new_status)) + + +class Enter(py_trees.behaviour.Behaviour): + """ + This behavior handles the driving through an intersection, it initially + sets a speed and finishes if the ego vehicle is close to the end of the + intersection. + """ + def __init__(self, name): + """ + Minimal one-time initialisation. Other one-time initialisation + requirements should be met via the setup() method. + :param name: name of the behaviour + """ + super(Enter, self).__init__(name) + + def setup(self, timeout): + """ + Delayed one-time initialisation that would otherwise interfere with + offline rendering of this behaviour in a tree to dot graph or + validation of the behaviour's configuration. + + This initializes the blackboard to be able to access data written to it + by the ROS topics and the target speed publisher. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as the set up is successful. + """ + self.target_speed_pub = rospy.Publisher("/paf/hero/" + "max_tree_velocity", Float32, + queue_size=1) + # rospy.wait_for_service('update_local_path') + # self.update_local_path = rospy.ServiceProxy("update_local_path", + # UpdateLocalPath) + self.blackboard = py_trees.blackboard.Blackboard() + return True + + def initialise(self): + """ + When is this called? + The first time your behaviour is ticked and anytime the status is + not RUNNING thereafter. + What to do here? + Any initialisation you need before putting your behaviour to work. + This prints a state status message and changes the driving speed for + the intersection. + """ + rospy.loginfo("Enter next Lane") + self.target_speed_pub.publish(convert_to_ms(20.0)) + + def update(self): + """ + When is this called? + Every time your behaviour is ticked. + What to do here? + - Triggering, checking, monitoring. Anything...but do not block! + - Set a feedback message + - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] + Continues driving through the intersection until the vehicle gets + close enough to the next global way point. + :return: py_trees.common.Status.RUNNING, if too far from intersection + py_trees.common.Status.SUCCESS, if stopped in front of inter- + section or entered the intersection + py_trees.common.Status.FAILURE, if no next path point can be + detected. + """ + next_waypoint_msg = self.blackboard.\ + get("/paf/hero/lane_change_distance") + + if next_waypoint_msg is None: + return py_trees.common.Status.FAILURE + # if next_waypoint_msg.distance < 5 and + # not next_waypoint_msg.isStopLine: + if next_waypoint_msg.distance < 5: + rospy.loginfo("Drive on the next lane!") + # self.update_local_path(leave_intersection=True) + return py_trees.common.Status.RUNNING + else: + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status): + """ + When is this called? + Whenever your behaviour switches to a non-running state. + - SUCCESS || FAILURE : your behaviour's work cycle has finished + - INVALID : a higher priority branch has interrupted, or shutting + down + writes a status message to the console when the behaviour terminates + :param new_status: new state after this one is terminated + """ + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, + self.status, + new_status)) + + +class Leave(py_trees.behaviour.Behaviour): + """ + This behaviour defines the leaf of this subtree, if this behavior is + reached, the vehicle left the intersection. + """ + def __init__(self, name): + """ + Minimal one-time initialisation. Other one-time initialisation + requirements should be met via the setup() method. + :param name: name of the behaviour + """ + super(Leave, self).__init__(name) + + def setup(self, timeout): + """ + Delayed one-time initialisation that would otherwise interfere with + offline rendering of this behaviour in a tree to dot graph or + validation of the behaviour's configuration. + + This initializes the blackboard to be able to access data written to it + by the ROS topics and the target speed publisher. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as the set up is successful. + """ + self.target_speed_pub = rospy.Publisher("/paf/hero/" + "max_tree_velocity", Float32, + queue_size=1) + self.blackboard = py_trees.blackboard.Blackboard() + return True + + def initialise(self): + """ + When is this called? + The first time your behaviour is ticked and anytime the status is + not RUNNING thereafter. + What to do here? + Any initialisation you need before putting your behaviour to work. + This prints a state status message and changes the driving speed to + the street speed limit. + """ + rospy.loginfo("Leave Change") + street_speed_msg = self.blackboard.get("/paf/hero/speed_limit") + if street_speed_msg is not None: + self.target_speed_pub.publish(street_speed_msg.data) + return True + + def update(self): + """ + When is this called? + Every time your behaviour is ticked. + What to do here? + - Triggering, checking, monitoring. Anything...but do not block! + - Set a feedback message + - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] + Abort this subtree + :return: py_trees.common.Status.FAILURE, to exit this subtree + """ + return py_trees.common.Status.FAILURE + + def terminate(self, new_status): + """ + When is this called? + Whenever your behaviour switches to a non-running state. + - SUCCESS || FAILURE : your behaviour's work cycle has finished + - INVALID : a higher priority branch has interrupted, or shutting + down + writes a status message to the console when the behaviour terminates + :param new_status: new state after this one is terminated + """ + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, + self.status, + new_status)) diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/road_features.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/road_features.py index d71c1f9c..97bf7657 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/road_features.py +++ b/code/planning/behavior_agent/src/behavior_agent/behaviours/road_features.py @@ -1,11 +1,11 @@ #!/usr/bin/env python3 # -*- coding: utf-8 -*- -import rospy +# import rospy import py_trees # from std_msgs.msg import Float64 # from nav_msgs.msg import Odometry # import numpy as np -import math +# import math """ Source: https://github.com/ll7/psaf2 @@ -71,12 +71,13 @@ def update(self): the intersection """ # TODO change this part to the actual source of intersection detection - bb = self.blackboard.get("/paf/hero/stop_sign") + bb = self.blackboard.get("/paf/hero/waypoint_distance") if bb is None: return py_trees.common.Status.FAILURE else: - self.dist = bb.distance - if self.dist < 30: + dist = bb.distance + isIntersection = bb.isStopLine + if dist < 30 and isIntersection: return py_trees.common.Status.SUCCESS else: return py_trees.common.Status.FAILURE @@ -95,39 +96,123 @@ def terminate(self, new_status): (self.name, self.status, new_status)) -class RoundaboutAhead(py_trees.behaviour.Behaviour): +class LaneChangeAhead(py_trees.behaviour.Behaviour): + """ + This behaviour checkes wheather there is an intersection in front of the + ego vehicle or not and triggers the rest of the decision tree handling the + intersection. + """ def __init__(self, name): - super(RoundaboutAhead, self).__init__(name) + """ + Minimal one-time initialisation. A good rule of thumb is to only + include the initialisation relevant for being able to insert this + behaviour in a tree for offline rendering to dot graphs. + Other one-time initialisation requirements should be met via the + setup() method. + :param name: name of the behaviour + """ + super(LaneChangeAhead, self).__init__(name) def setup(self, timeout): - self.Roundabout = False + """ + Delayed one-time initialisation that would otherwise interfere with + offline rendering of this behaviour in a tree to dot graph or + validation of the behaviour's configuration. + + This initializes the blackboard to be able to access data written to it + by the ROS topics and the target speed publisher. + :param timeout: an initial timeout to see if the tree generation is + successful + :return: True, as the set up is successful. + """ + self.blackboard = py_trees.blackboard.Blackboard() return True def initialise(self): - self.blackboard = py_trees.blackboard.Blackboard() + """ + When is this called? + The first time your behaviour is ticked and anytime the status is + not RUNNING thereafter. + What to do here? + Any initialisation you need before putting your behaviour to work. + This initializes the variables needed to save information about the + stop line. + """ self.dist = 0 def update(self): - bb = self.blackboard.get("/psaf/ego_vehicle/distance_next_roundabout") - self.odo = self.blackboard.get("/carla/ego_vehicle/odometry") + """ + When is this called? + Every time your behaviour is ticked. + What to do here? + - Triggering, checking, monitoring. Anything...but do not block! + - Set a feedback message + - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] + Gets the current distance to the next intersection. + :return: py_trees.common.Status.SUCCESS, if the vehicle is within range + of the intersection + py_trees.common.Status.FAILURE, if we are too far away from + the intersection + """ + # TODO change this part to the actual source of intersection detection + bb = self.blackboard.get("/paf/hero/lane_change_distance") if bb is None: return py_trees.common.Status.FAILURE else: - dist_x = bb.entry_point.x - self.odo.pose.pose.position.x - dist_y = bb.entry_point.y - self.odo.pose.pose.position.y - dist = math.sqrt(dist_x ** 2 + dist_y ** 2) - self.dist = dist - if self.dist < 30: - rospy.loginfo("approaching roundabout") + dist = bb.distance + isIntersection = bb.isLaneChange + if dist < 30 and isIntersection: return py_trees.common.Status.SUCCESS else: return py_trees.common.Status.FAILURE def terminate(self, new_status): + """ + When is this called? + Whenever your behaviour switches to a non-running state. + - SUCCESS || FAILURE : your behaviour's work cycle has finished + - INVALID : a higher priority branch has interrupted, or shutting + down + writes a status message to the console when the behaviour terminates + :param new_status: new state after this one is terminated + """ self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % (self.name, self.status, new_status)) +# class RoundaboutAhead(py_trees.behaviour.Behaviour): +# def __init__(self, name): +# super(RoundaboutAhead, self).__init__(name) +# +# def setup(self, timeout): +# self.Roundabout = False +# return True +# +# def initialise(self): +# self.blackboard = py_trees.blackboard.Blackboard() +# self.dist = 0 +# +# def update(self): +# bb = self.blackboard.get("/psaf/ego_vehicle/distance_next_roundabout") +# self.odo = self.blackboard.get("/carla/ego_vehicle/odometry") +# if bb is None: +# return py_trees.common.Status.FAILURE +# else: +# dist_x = bb.entry_point.x - self.odo.pose.pose.position.x +# dist_y = bb.entry_point.y - self.odo.pose.pose.position.y +# dist = math.sqrt(dist_x ** 2 + dist_y ** 2) +# self.dist = dist +# if self.dist < 30: +# rospy.loginfo("approaching roundabout") +# return py_trees.common.Status.SUCCESS +# else: +# return py_trees.common.Status.FAILURE +# +# def terminate(self, new_status): +# self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % +# (self.name, self.status, new_status)) +# + class StopAhead(py_trees.behaviour.Behaviour): def __init__(self, name): super(StopAhead, self).__init__(name) @@ -161,10 +246,8 @@ def __init__(self, name): Minimal one-time initialisation. A good rule of thumb is to only include the initialisation relevant for being able to insert this behaviour in a tree for offline rendering to dot graphs. - Other one-time initialisation requirements should be met via the setup() method. - :param name: name of the behaviour """ super(MultiLane, self).__init__(name) @@ -174,11 +257,11 @@ def setup(self, timeout): Delayed one-time initialisation that would otherwise interfere with offline rendering of this behaviour in a tree to dot graph or validation of the behaviour's configuration. - :param timeout: an initial timeout to see if the tree generation is successful :return: True, as there is nothing to set up. """ + self.blackboard = py_trees.blackboard.Blackboard() return True def initialise(self): @@ -186,30 +269,25 @@ def initialise(self): When is this called? The first time your behaviour is ticked and anytime the status is not RUNNING thereafter. - What to do here? Any initialisation you need before putting your behaviour to work. - This initializes the blackboard to be able to access data written to it by the ROS topics. """ - self.blackboard = py_trees.blackboard.Blackboard() + pass def update(self): """ When is this called? Every time your behaviour is ticked. - What to do here? - Triggering, checking, monitoring. Anything...but do not block! - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] - This part checks if the agent is on a multi-lane and corresponding overtaking behavior should be triggered. If we are not on a multi-lane it is check if overtaking on a single lane is possible. Otherwise, the overtaking process will be canceled. - :return: py_trees.common.Status.SUCCESS, if the agent is on a multi- lane py_trees.common.Status.FAILURE, if the agent is not on a @@ -230,7 +308,6 @@ def terminate(self, new_status): - SUCCESS || FAILURE : your behaviour's work cycle has finished - INVALID : a higher priority branch has interrupted, or shutting down - writes a status message to the console when the behaviour terminates """ self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % @@ -247,10 +324,8 @@ def __init__(self, name): Minimal one-time initialisation. A good rule of thumb is to only include the initialisation relevant for being able to insert this behaviour in a tree for offline rendering to dot graphs. - Other one-time initialisation requirements should be met via the setup() method. - :param name: name of the behaviour """ super(SingleLineDotted, self).__init__(name) @@ -260,7 +335,6 @@ def setup(self, timeout): Delayed one-time initialisation that would otherwise interfere with offline rendering of this behaviour in a tree to dot graph or validation of the behaviour's configuration. - :param timeout: an initial timeout to see if the tree generation is successful :return: True, as there is nothing to set up. @@ -273,10 +347,8 @@ def initialise(self): When is this called? The first time your behaviour is ticked and anytime the status is not RUNNING thereafter. - What to do here? Any initialisation you need before putting your behaviour to work. - This initializes the blackboard to be able to access data written to it by the ROS topics. """ @@ -286,15 +358,12 @@ def update(self): """ When is this called? Every time your behaviour is ticked. - What to do here? - Triggering, checking, monitoring. Anything...but do not block! - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] - Right now, there is no way for us to detect a single dotted line, so it is assumed that we are on one. - :return: py_trees.common.Status.SUCCESS, if the agent is on a single dotted line py_trees.common.Status.FAILURE, if the agent is not on a @@ -312,7 +381,6 @@ def terminate(self, new_status): - SUCCESS || FAILURE : your behaviour's work cycle has finished - INVALID : a higher priority branch has interrupted, or shutting down - writes a status message to the console when the behaviour terminates """ self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % @@ -329,10 +397,8 @@ def __init__(self, name): Minimal one-time initialisation. A good rule of thumb is to only include the initialisation relevant for being able to insert this behaviour in a tree for offline rendering to dot graphs. - Other one-time initialisation requirements should be met via the setup() method. - :param name: name of the behaviour """ super(RightLaneAvailable, self).__init__(name) @@ -342,7 +408,6 @@ def setup(self, timeout): Delayed one-time initialisation that would otherwise interfere with offline rendering of this behaviour in a tree to dot graph or validation of the behaviour's configuration. - :param timeout: an initial timeout to see if the tree generation is successful :return: True, as there is nothing to set up. @@ -355,10 +420,8 @@ def initialise(self): When is this called? The first time your behaviour is ticked and anytime the status is not RUNNING thereafter. - What to do here? Any initialisation you need before putting your behaviour to work. - This initializes the blackboard to be able to access data written to it by the ROS topics. """ @@ -368,14 +431,11 @@ def update(self): """ When is this called? Every time your behaviour is ticked. - What to do here? - Triggering, checking, monitoring. Anything...but do not block! - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] - This part checks if there is a lane to the right of the agent. - :return: py_trees.common.Status.SUCCESS, if there is a lane to the right of the agent py_trees.common.Status.FAILURE, if there is no lane to the @@ -396,7 +456,6 @@ def terminate(self, new_status): - SUCCESS || FAILURE : your behaviour's work cycle has finished - INVALID : a higher priority branch has interrupted, or shutting down - writes a status message to the console when the behaviour terminates """ self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % @@ -413,10 +472,8 @@ def __init__(self, name): Minimal one-time initialisation. A good rule of thumb is to only include the initialisation relevant for being able to insert this behaviour in a tree for offline rendering to dot graphs. - Other one-time initialisation requirements should be met via the setup() method. - :param name: name of the behaviour """ super(LeftLaneAvailable, self).__init__(name) @@ -426,12 +483,12 @@ def setup(self, timeout): Delayed one-time initialisation that would otherwise interfere with offline rendering of this behaviour in a tree to dot graph or validation of the behaviour's configuration. - :param timeout: an initial timeout to see if the tree generation is successful :return: True, as there is nothing to set up. """ + self.blackboard = py_trees.blackboard.Blackboard() return True def initialise(self): @@ -439,35 +496,28 @@ def initialise(self): When is this called? The first time your behaviour is ticked and anytime the status is not RUNNING thereafter. - What to do here? Any initialisation you need before putting your behaviour to work. - This initializes the blackboard to be able to access data written to it by the ROS topics. """ - self.blackboard = py_trees.blackboard.Blackboard() + pass def update(self): """ When is this called? Every time your behaviour is ticked. - What to do here? - Triggering, checking, monitoring. Anything...but do not block! - Set a feedback message - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE] - This part checks if there is a lane to the left of the agent and the rest of the overtaking can take place. - :return: py_trees.common.Status.SUCCESS, if there is a lane to the left of the agent py_trees.common.Status.FAILURE, if there is no lane to the left of the agent """ - # TODO this information could be retrieved from the openDrive map, not - # sure if that will happen though due to the limited time left bb = self.blackboard.get("/paf/hero/lane_status") if bb is None: return py_trees.common.Status.FAILURE @@ -483,7 +533,6 @@ def terminate(self, new_status): - SUCCESS || FAILURE : your behaviour's work cycle has finished - INVALID : a higher priority branch has interrupted, or shutting down - writes a status message to the console when the behaviour terminates """ self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s ]" % diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/topics2blackboard.py index c74909ff..21ace3eb 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/behavior_agent/src/behavior_agent/behaviours/topics2blackboard.py @@ -5,10 +5,12 @@ import py_trees_ros from std_msgs.msg import Float64, String, Bool, Float32 -from geometry_msgs.msg import Point +# from geometry_msgs.msg import Point from carla_msgs.msg import CarlaSpeedometer +from sensor_msgs.msg import Range from mock.msg import Traffic_light, Stop_sign +from perception.msg import Waypoint, LaneChange """ Source: https://github.com/ll7/psaf2 @@ -32,9 +34,9 @@ def create_node(role_name): {'name': f"/paf/{role_name}/slowed_by_car_in_front", 'msg': Bool, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/stopline_distance", 'msg': Float32, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/psaf/{role_name}/distance_exit_roundabout", 'msg': Point, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE}, + {'name': f"/paf/{role_name}/waypoint_distance", 'msg': Waypoint, + 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE}, {'name': f"/psaf/{role_name}/obstacle_on_left_lane", 'msg': Float64, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/psaf/{role_name}/obstacle_on_right_lane", 'msg': Float64, @@ -46,7 +48,17 @@ def create_node(role_name): {'name': f"/paf/{role_name}/traffic_light", 'msg': Traffic_light, 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, {'name': f"/paf/{role_name}/max_velocity", 'msg': Float32, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER} + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': f"/carla/{role_name}/LIDAR_range", 'msg': Range, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': f"/carla/{role_name}/LIDAR_range_rear_right", 'msg': Range, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': f"/carla/{role_name}/LIDAR_range_rear_left", 'msg': Range, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': f"/paf/{role_name}/speed_limit", 'msg': Float32, + 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, + {'name': f"/paf/{role_name}/lane_change_distance", 'msg': LaneChange, + 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE} ] topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard") diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index 514fe4f7..2e94697d 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -1,5 +1,5 @@ - +* [Lidar Distance Utility](#lidar-distance-utility) + * [Author](#author) + * [Date](#date) + * [Configuration](#configuration) + * [Example](#example) + + +## Configuration + +The possible params for this node are summed up in the following table: + +| Parameter | Description | Default value | +|-------------------|-------------------------------------------------------|-----------------------------------| +| min_x | Minimum x value of the coordinate | `np.inf` | +| max_x | Maximum x value of the coordinate | `np.inf` | +| min_y | Minimum y value of the coordinate | `np.inf` | +| max_y | Maximum y value of the coordinate | `np.inf` | +| min_z | Minimum z value of the coordinate | `np.inf` | +| max_z | Maximum z value of the coordinate | `np.inf` | +| point_cloud_topic | Topic to publish the filtered PointCloud2 | `/carla/hero/filtered` | +| range_topic | Topic to publish the Range | `/carla/hero/_range` | +| source_topic | Topic on which the PointCloud2 messages are read from | `/carla/hero/LIDAR` | + +❗️Important: + +The LIDAR sensor is mounted in a height of 1.7m (height of the car). Thereby, if you want all points +starting from 20cm above the ground you have to set min_z = -1.5. + +The meaning of the x and y values is described by the following image: + +![lidar filter](../00_assets/lidar_filter.png) + +### Example + +```xml + + + + + + + + + +```