feat(#211): Merge main

.dockerignore

@@ -1 +1,3 @@
 code/output
+code/perception/src/panoptic_segmentation/lightning_logs
+code/perception/src/panoptic_segmentation/logs

.flake8

@@ -1,3 +1,3 @@
 [flake8]
-exclude= code/test_planning_top/behavior_agent/src/behavior_agent/behavior_tree.py,
- code/test_planning_top/behavior_agent/src/behavior_agent/behaviours/__init__.py
+exclude= code/planning/behavior_agent/src/behavior_agent/behavior_tree.py,
+ code/planning/behavior_agent/src/behavior_agent/behaviours/__init__.py

.github/workflows/build.yml

@@ -19,6 +19,12 @@ jobs:
  packages: write
 
  steps:
+ - name: Clear up some disk space
+ run: |
+ sudo rm -rf /usr/share/dotnet
+ sudo rm -rf "$AGENT_TOOLSDIRECTORY"
+ df -h
+
  - name: Checkout repository
  uses: actions/checkout@v3
 

build/docker-compose.yml

@@ -52,8 +52,10 @@ services:
  context: ../
  init: true
  tty: true
+ shm_size: 2gb
  #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS"
- command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
+ #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch"
+ command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP"
  logging:
  driver: "local"
  environment:
@@ -65,7 +67,7 @@ services:
  - /tmp/.X11-unix:/tmp/.X11-unix
  # if you change the volume here also change the copy command
  # in ``build/docker/build/Dockerfile
- - ../code:/workspace/code
+ - ../:/workspace
  # mount git config for dvc
  - ../.gitconfig:/home/carla/.gitconfig
  - ../:/workspace/

build/docker/agent/Dockerfile

@@ -40,7 +40,28 @@ RUN wget https://github.com/libgit2/libgit2/archive/refs/tags/v1.5.0.tar.gz -O l
  && cd libgit2-1.5.0/ \
  && cmake . \
  && make -j$(nproc --ignore=2) \
- && make install
+ && make install \
+ && cd ../ \
+ && rm libgit2-1.5.0.tar.gz \
+ && rm -r libgit2-1.5.0/
+
+# CUDA installation
+RUN wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-ubuntu2004.pin \
+ && mv cuda-ubuntu2004.pin /etc/apt/preferences.d/cuda-repository-pin-600 \
+ && wget https://developer.download.nvidia.com/compute/cuda/11.7.0/local_installers/cuda-repo-ubuntu2004-11-7-local_11.7.0-515.43.04-1_amd64.deb \
+ && dpkg -i cuda-repo-ubuntu2004-11-7-local_11.7.0-515.43.04-1_amd64.deb \
+ && cp /var/cuda-repo-ubuntu2004-11-7-local/cuda-*-keyring.gpg /usr/share/keyrings/ \
+ && apt-get update \
+ && DEBIAN_FRONTEND=noninteractive apt-get -y install cuda-nvcc-11-7 \
+ && rm cuda-repo-ubuntu2004-11-7-local_11.7.0-515.43.04-1_amd64.deb \
+ && apt-get -y remove cuda-repo-ubuntu2004-11-7-local \
+ && cat /etc/apt/sources.list \
+ && rm /etc/apt/preferences.d/cuda-repository-pin-600 \
+ && rm -rf /var/cuda-repo-ubuntu1804-11-7-local/ \
+ && rm /etc/apt/sources.list.d/cuda-ubuntu2004-11-7-local.list \
+ && apt-get clean
+
+ENV CUDA_HOME=/usr/local/cuda-11.7
 
 # override python path, carla pip package path didn't exist and was using Python 3.7 instead of 2.7
 ENV PYTHONPATH=/opt/ros/noetic/lib/python3/dist-packages
@@ -109,8 +130,7 @@ RUN sudo mkdir /opt/leaderboard-py3 && sudo chown $USERNAME:$USERNAME /opt/leade
  ln -s $LEADERBOARD_ROOT/leaderboard /opt/leaderboard-py3/leaderboard && \
  ln -s $SCENARIO_RUNNER_ROOT/srunner /opt/leaderboard-py3/srunner
 
-RUN python -m pip install roslibpy
-
+RUN python -m pip install roslibpy lxml
 
 # environment variables for the leaderboard_evaluator
 ENV ROUTES=$LEADERBOARD_ROOT/data/routes_devtest.xml
@@ -136,6 +156,15 @@ RUN echo /catkin_ws/install/lib/python3/dist-packages >> /home/$USERNAME/.local/
 
 RUN sudo mkdir /workspace && sudo chown $USERNAME:$USERNAME /workspace
 
+RUN echo "export CUDA_HOME=/usr/local/cuda-11" >> ~/.bashrc \
+ && echo "export PATH=$PATH:$CUDA_HOME/bin" >> ~/.bashrc
+
+COPY --chown=$USERNAME:$USERNAME ./code/requirements.txt /workspace/
+ENV TORCH_CUDA_ARCH_LIST="5.2 6.0 6.1 7.0 7.5 8.0 8.6+PTX"
+ENV IABN_FORCE_CUDA=1
+
+RUN source ~/.bashrc && cat /workspace/requirements.txt | xargs -n 1 -L 1 pip install
+
 # Add agent code
 COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/
 
@@ -152,8 +181,6 @@ WORKDIR /workspace/code
 
 RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
 
-RUN pip install -r requirements.txt
-
 ENTRYPOINT ["/entrypoint.sh"]
 CMD ["bash", "-c", "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=${DEBUG_CHALLENGE} \
 --repetitions=${REPETITIONS} \

code/acting/launch/acting.launch

@@ -28,11 +28,12 @@
  <param name="control_loop_rate" value="$(arg control_loop_rate)" />
  <param name="role_name" value="$(arg role_name)" />
  </node>
-
+<!--
  <node pkg="acting" type="DummyTrajectoryPublisher.py" name="DummyTrajectoryPublisher" output="screen">
  <param name="control_loop_rate" value="1" />
  <param name="role_name" value="$(arg role_name)" />
  </node>
+-->
 
  <node pkg="acting" type="MainFramePublisher.py" name="MainFramePublisher" output="screen">
  <param name="control_loop_rate" value="0.05" />

code/acting/src/acting/DummyTrajectoryPublisher.py

@@ -36,7 +36,7 @@ def __init__(self):
 
  # Static trajectory for testing purposes
  self.initial_trajectory = [
- (986.0, -5430.0),
+ (986.0, -5442.0),
  (986.0, -5463.2),
  (984.5, -5493.2),
 

code/acting/src/acting/pure_pursuit_controller.py

@@ -14,6 +14,8 @@
 from helper_functions import vector_angle
 from trajectory_interpolation import points_to_vector
 
+MIN_LD_V: float = 3.0
+
 
 class PurePursuitController(CompatibleNode):
  def __init__(self):
@@ -143,6 +145,10 @@ def __set_position(self, data: PoseStamped, min_diff=0.001):
  self.__position = (new_x, new_y)
 
  def __set_path(self, data: Path):
+ path_len = len(data.poses)
+ if path_len < 1:
+ self.loginfo("Pure Pursuit: Empty path received and disregarded")
+ return
  self.__path = data
 
  def __set_heading(self, data: Float32):
@@ -161,13 +167,17 @@ def __calculate_steer(self) -> float:
  :return:
  """
  l_vehicle = 2.85 # wheelbase
- k_ld = 2.0 # todo: tune
- look_ahead_dist = 5.0 # offset so that ld is never zero
-
- if round(self.__velocity, 1) < 0.1:
- look_ahead_dist += 1.0
+ k_ld = 1.0
+ look_ahead_dist = 3.5 # offset so that ld is never zero
+
+ if self.__velocity < 0:
+ # backwards driving is not supported
+ return 0.0
+ elif round(self.__velocity, 1) < MIN_LD_V:
+ # Offset for low velocity state
+ look_ahead_dist += 0.0 # no offset
  else:
- look_ahead_dist += k_ld * self.__velocity
+ look_ahead_dist += k_ld * (self.__velocity - MIN_LD_V)
 
  self.__tp_idx = self.__get_target_point_index(look_ahead_dist)
 

code/acting/src/acting/stanley_controller.py

@@ -142,6 +142,10 @@ def __set_position(self, data: PoseStamped, min_diff=0.001):
  self.__position = (new_x, new_y)
 
  def __set_path(self, data: Path):
+ path_len = len(data.poses)
+ if path_len < 1:
+ self.loginfo("Stanley: Empty path received and disregarded")
+ return
  self.__path = data
 
  def __set_heading(self, data: Float32):
@@ -171,6 +175,7 @@ def __calculate_steer(self) -> float:
 
  cross_err = self.__get_cross_err(closest_point.pose.position)
  heading_err = self.__heading - traj_heading
+ heading_err = ((heading_err + math.pi) % (2 * math.pi)) - math.pi
 
  steering_angle = heading_err + atan((k_ce * cross_err) /
  current_velocity * k_v)

code/acting/src/acting/vehicle_controller.py

@@ -11,6 +11,8 @@
 
 PURE_PURSUIT_CONTROLLER: int = 1
 STANLEY_CONTROLLER: int = 2
+STANLEY_CONTROLLER_MIN_V: float = 4.0 # ~14kph
+# STANLEY_CONTROLLER_MAX_V: float = 13.89 # ~50kph
 MAX_STEER_ANGLE: float = 0.75
 
 
@@ -39,12 +41,19 @@ def __init__(self):
  )
  self.status_pub: Publisher = self.new_publisher(
  Bool,
- f"/paf/{self.role_name}/status",
+ f"/carla/{self.role_name}/status",
  qos_profile=QoSProfile(
  depth=1,
  durability=DurabilityPolicy.TRANSIENT_LOCAL)
  )
 
+ self.controller_pub: Publisher = self.new_publisher(
+ Float32,
+ f"/paf/{self.role_name}/controller",
+ qos_profile=QoSProfile(depth=10,
+ durability=DurabilityPolicy.TRANSIENT_LOCAL)
+ )
+
  self.emergency_pub: Publisher = self.new_publisher(
  Bool,
  f"/paf/{self.role_name}/emergency",
@@ -85,10 +94,11 @@ def __init__(self):
  qos_profile=1)
 
  self.__emergency: bool = False
- self.__throttle: float = 0
- self.__pure_pursuit_steer: float = 0
- self.__stanley_steer: float = 0
- self.__current_steer: float = 0 # todo: check emergency behaviour
+ self.__throttle: float = 0.0
+ self.__velocity: float = 0.0
+ self.__pure_pursuit_steer: float = 0.0
+ self.__stanley_steer: float = 0.0
+ self.__current_steer: float = 0.0 # todo: check emergency behaviour
 
  def run(self):
  """
@@ -109,17 +119,17 @@ def loop(timer_event=None) -> None:
  if self.__emergency: # emergency is already handled in
  # __emergency_break()
  return
- controller = self.__choose_controller()
- if controller == PURE_PURSUIT_CONTROLLER:
+ p_stanley = self.__choose_controller()
+ if p_stanley < 0.5:
  self.logdebug('Using PURE_PURSUIT_CONTROLLER')
- steer = self.__pure_pursuit_steer
- elif controller == STANLEY_CONTROLLER:
+ self.controller_pub.publish(float(PURE_PURSUIT_CONTROLLER))
+ elif p_stanley >= 0.5:
  self.logdebug('Using STANLEY_CONTROLLER')
- steer = self.__stanley_steer
- else:
-   self.logerr("Vehicle Controller couldn't find requested "
-  "controller.")
-  raise Exception("Requested Controller not found")
+ self.controller_pub.publish(float(STANLEY_CONTROLLER))
+
+ f_stanley = p_stanley * self.__stanley_steer
+ f_pure_p = (1-p_stanley) * self.__pure_pursuit_steer
+ steer = f_stanley + f_pure_p
 
  message = CarlaEgoVehicleControl()
  message.reverse = False
@@ -134,7 +144,6 @@ def loop(timer_event=None) -> None:
  message.manual_gear_shift = False
  pid.setpoint = self.__map_steering(steer)
  message.steer = pid(self.__current_steer)
- # message.steer = 0 # zum testen mit bei gerader Fahrt
  message.gear = 1
  message.header.stamp = roscomp.ros_timestamp(self.get_time(),
  from_sec=True)
@@ -185,6 +194,7 @@ def __get_velocity(self, data: CarlaSpeedometer) -> None:
  :param data:
  :return:
  """
+ self.__velocity = data.speed
  if not self.__emergency: # nothing to do in this case
  return
  if data.speed < 0.1: # vehicle has come to a stop
@@ -215,12 +225,24 @@ def __set_pure_pursuit_steer(self, data: Float32):
  def __set_stanley_steer(self, data: Float32):
  self.__stanley_steer = data.data
 
- def __choose_controller(self) -> int:
+ def sigmoid(self, x: float):
+ """
+ Evaluates the sigmoid function s(x) = 1 / (1+e^-25x)
+ :param x: x
+ :return: s(x) = 1 / (1+e^-25x)
+ """
+ temp_x = min(-25 * x, 25)
+ res = 1 / (1 + math.exp(temp_x))
+ return res
+
+ def __choose_controller(self) -> float:
  """
- Chooses with steering controller to use
+ Returns the proportion of stanley to use.
+ Publishes the currently used controller
  :return:
  """
- return STANLEY_CONTROLLER
+ res = self.sigmoid(self.__velocity - STANLEY_CONTROLLER_MIN_V)
+ return res
 
 
 def main(args=None):