feat(#195): add a distance publisher for the stop line and next waypo…

…int (#197) # Description Old description: Adds two publisher for the distance to the stop line of an intersection and the next way point. The stop line is indicated by the way point before the intersection. This will be used by the decision tree. New description: Adds two publisher for the distance to the stop line of an intersection and the distance to the next planned lane change. The behaviour for approaching and entering an intersection is optimized. The behaviour before approaching and entering a lane change was added to the behaviour tree and also optimized. The behaviour with the Lidar sensor is implemented. To check if we can enter an intersection and enter a lane change event, the distance is checked through the Lidar topic. The stop line behaviour is implemented. There is no extra topic which publishes the distance to the next stop sign because the xodr file "signal" part only refers to ground truth traffic sign position data which cannot be used. The agent stops before every intersection and every possible traffic sign to check for other cars. Because of that the stop sign behaviour is also covered in this PR. For testing the environment was switched to the dev environment (town 12). ## Time invested Josef Kircher: 3 h Simon Erlbacher: 15 h Niklas Vogel: 15h ## Type of change Please delete options that are not relevant. - New feature (non-breaking change which adds functionality) ## Does this PR introduce a breaking change? Approved decision making with all current informations. - intersection behaviour - lane change bahaviour - distance publisher - integrated lidar topics - optimized parameter for agent behaviours # Checklist: - [x] My code follows the style guidelines of this project - [x] I have performed a self-review of my own code - [x] I have commented my code, particularly in hard-to-understand areas - [x] I have made corresponding changes to the documentation - [x] My changes generate no new warnings - [x] I have added tests that prove my fix is effective or that my feature works (might be obsolete with CI later on) - [x] New and existing unit tests pass locally with my changes (might be obsolete with CI later on)
ll7 · Mar 24, 2023 · c17b411 · c17b411
2 parents 8592441 + 2950061
commit c17b411
Show file tree

Hide file tree

Showing 26 changed files with 1,208 additions and 153 deletions.
diff --git 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
@@ -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
@@ -21,7 +21,7 @@
  <node pkg="acting" type="velocity_publisher_dummy.py" name="velocity_publisher_dummy" output="screen">
  <param name="role_name" value="$(arg role_name)" />
  <param name="control_loop_rate" value="0.2" />
- <param name="enabled" value="True" /> <!-- set to True to publish dummy velocities for testing-->
+ <param name="enabled" value="False" /> <!-- set to True to publish dummy velocities for testing-->
  </node>
 
  <node pkg="acting" type="vehicle_controller.py" name="vehicle_controller" output="screen">

diff --git 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
@@ -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
@@ -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: <Fixed 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
@@ -27,15 +27,15 @@ 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},
  {'type': 'sensor.other.gnss', 'id': 'GPS',
  '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
@@ -7,14 +7,14 @@
 <!-- <param name="role_name" value="$(arg role_name)" /> -->
 <!-- </node> -->
 
- <node pkg="mock" type="mock_stop_sign.py" name="mock_stop_sign" output="screen">
-  <param name="control_loop_rate" value="$(arg control_loop_rate)" />
+<!--  <node pkg="mock" type="mock_stop_sign.py" name="mock_stop_sign" output="screen">
+ <param name="control_loop_rate" value="$(arg control_loop_rate)" />
  <param name="role_name" value="$(arg role_name)" />
- </node>
+ </node> -->
 
- <node pkg="mock" type="mock_intersection_clear.py" name="mock_intersection_clear" output="screen">
+<!-- <node pkg="mock" type="mock_intersection_clear.py" name="mock_intersection_clear" output="screen">
  <param name="control_loop_rate" value="$(arg control_loop_rate)" />
  <param name="role_name" value="$(arg role_name)" />
- </node>
+ </node> -->
 
 </launch>
diff --git 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
@@ -32,5 +32,38 @@
  <param name="self_diagnose" value="true"/>
  </node>
 
+ <node pkg="perception" type="global_plan_distance_publisher.py" name="GlobalPlanDistance" output="screen">
+ <param name="control_loop_rate" value="0.1" />
+ <param name="role_name" value="$(arg role_name)" />
+ </node>
+
+ <node pkg="perception" type="lidar_distance.py" name="lidar_distance" output="screen">
+ <param name="max_y" value="1.5"/>
+ <param name="min_y" value="-1.5"/>
+ <param name="min_x" value="3"/>
+ <param name="min_z" value="-1.5"/>
+ <param name="max_z" value="0"/>
+ <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered"/>
+ <param name="range_topic" value="/carla/hero/LIDAR_range"/>
+ </node>
+
+ <node pkg="perception" type="lidar_distance.py" name="lidar_distance_rear_right" output="screen">
+ <param name="min_y" value="-5"/>
+ <param name="max_y" value="-2.5"/>
+ <param name="max_x" value="0"/>
+ <param name="min_z" value="-1.5"/>
+ <param name="max_z" value="0"/>
+ <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered_rear_right"/>
+ <param name="range_topic" value="/carla/hero/LIDAR_range_rear_right"/>
+ </node>
 
+ <node pkg="perception" type="lidar_distance.py" name="lidar_distance_rear_left" output="screen">
+ <param name="min_y" value="2.5"/>
+ <param name="max_y" value="5"/>
+ <param name="max_x" value="0"/>
+ <param name="min_z" value="-1.5"/>
+ <param name="max_z" value="0"/>
+ <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered_rear_left"/>
+ <param name="range_topic" value="/carla/hero/LIDAR_range_rear_left"/>
+ </node>
 </launch>