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
+
+
+
+
+
+
+
+
+
+```