Merge branch 'main' into 66-feature-stanley-controller

build/docker-compose.yml

@@ -65,9 +65,10 @@ services:
  - /tmp/.X11-unix:/tmp/.X11-unix
  # if you change the volume here also change the copy command
  # in ``build/docker/build/Dockerfile
- - ../:/workspace
+ - ../code:/workspace/code
  # mount git config for dvc
  - ../.gitconfig:/home/carla/.gitconfig
+ - ../:/workspace/
  networks:
  - carla
  - ros

code/acting/src/acting/helper_functions.py

@@ -102,10 +102,10 @@ def calc_path_yaw(path: Path, idx: int) -> float:
  point_current = path.poses[idx]
  point_next: PoseStamped
  point_next = path.poses[idx + 1]
- angle = math.atan2(point_next.pose.position.y
- - point_current.pose.position.y,
- point_next.pose.position.x
- - point_current.pose.position.x)
+ angle = math.atan2(point_next.pose.position.y -
+ point_current.pose.position.y,
+ point_next.pose.position.x -
+ point_current.pose.position.x)
  return normalize_angle(angle)
 
 

code/acting/src/acting/velocity_controller.py

@@ -6,6 +6,8 @@
 from simple_pid import PID
 from std_msgs.msg import Float32
 
+SPEED_LIMIT_DEFAULT: float = 36.0
+
 
 class VelocityController(CompatibleNode):
  """
@@ -83,7 +85,10 @@ def loop(timer_event=None):
  "publish a throttle value")
  return
  if self.__speed_limit is None or self.__speed_limit < 0:
- self.__speed_limit = float("inf")
+ self.logdebug("VelocityController hasn't received a acceptable"
+ " speed_limit yet. speed_limit has been set to"
+ f"default value {SPEED_LIMIT_DEFAULT}")
+ self.__speed_limit = SPEED_LIMIT_DEFAULT
  if self.__max_velocity < 0:
  self.logerr("Velocity controller doesn't support backward "
  "driving yet.")

code/acting/src/acting/velocity_publisher_dummy.py

@@ -23,10 +23,12 @@ def __init__(self):
  Float32,
  f"/carla/{self.role_name}/max_velocity",
  qos_profile=1)
- self.velocity = 5
+
+ self.velocity = 4.0
  self.delta_velocity = 0.125
- self.max_velocity = 7.5
- self.min_velocity = 5
+ self.max_velocity = 5.5
+ self.min_velocity = 4
+
  self.__dv = self.delta_velocity
 
  def run(self):

code/test_planning_top/behavior_agent/src/behavior_agent/behaviours/maneuvers.py

@@ -91,18 +91,83 @@ def terminate(self, new_status):
 
 
 class Cruise(py_trees.behaviour.Behaviour):
+ """
+ This behaviour is the lowest priority one and will be executed when no
+ other behaviour is triggered. It doesn't do much, as in the normal cruising
+ the holding of the lane and speed control is done by different parts of the
+ project. It might be possible to put the activation/deactivation of the ACC
+ here.
+
+ speed control = acting via speed limits and target_speed
+ following the trajectory = acting
+ """
  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(Cruise, 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.
+
+ :param timeout: an initial timeout to see if the tree generation is
+ successful
+ :return: True, as there is nothing to set up.
+ """
  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 blackboard to be able to access data written to it
+ by the ROS topics.
+ """
  self.blackboard = py_trees.blackboard.Blackboard()
 
  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 behaviour doesn't do anything else than just keep running unless
+ there is a higher priority behaviour
+
+ :return: py_trees.common.Status.RUNNING, keeps the decision tree from
+ finishing
+ """
+ # rospy.loginfo("Cruising around")
  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
+ """
  self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" %
  (self.name, self.status, new_status))

doc/05_acting/02_acc.md

@@ -73,5 +73,5 @@ To do so, enable the `AccDistancePublisherDummy` in the acting launch file.
 
 ## Also see
 
-* [Dev round 4](../dev_rounds/sprint_4.md)
+* [Dev round 4](../07_dev_talks/sprint_4.md)
 * [research](../03_research/01_acting/Readme.md)

doc/dev_rounds/sprint_4.md → doc/07_dev_talks/sprint_4.md

@@ -7,7 +7,9 @@
 ## Author
 
 Josef Kircher
+
 Julian Graf
+
 Marco Riedenauer
 
 ## Date

doc/07_dev_talks/sprint_5.md

@@ -0,0 +1,117 @@
+# Dev talk - Sprint 5
+
+**Summary:** For better communication a developer round is held in this sprint to improve communication and clarify responsibilities.
+
+---
+
+## Author
+
+Josef Kircher
+
+Gabriel Schwald
+
+Marco Riedenauer
+
+Korbinian Stein
+
+## Date
+
+01.02.2023
+
+## Prerequisite
+
+---
+<!-- TOC -->
+* [Dev talk - Sprint 5](#dev-talk---sprint-5)
+ * [Author](#author)
+ * [Date](#date)
+ * [Prerequisite](#prerequisite)
+ * [Planning](#planning)
+ * [Suggestions](#suggestions)
+ * [Things that need to handled](#things-that-need-to-handled)
+ * [Results](#results)
+ * [Acting](#acting)
+ * [Perception](#perception)
+ * [Sources](#sources)
+<!-- TOC -->
+
+## Planning
+
+### Suggestions
+
+How should be the naming convention for topics?
+
+traffic light message:
+
+ string color
+ float32 distance
+
+traffic sign message:
+
+ bool isStop
+ float distance
+
+speed limit message:
+
+ float speedlimit
+ float distance
+
+acc message:
+
+ bool activate/deactivate
+
+### Things that need to handled
+
+* Information from perception
+ * Are there speed limits on the road? Can you handle these?
+ * traffic signs are firstly limited to stop signs, I guess? Is the design modular enough to add different signs?
+ * should there be a state as in the traffic light suggestion? Would keep the message short
+ * is it alright to handle speed limits differently?
+ * We might need to track crossing traffic for unsignalized intersections.
+
+* Information from acting
+ * is it simple enough to have a bool message?
+ * Speed is published in m/s or km/h?
+ * this message would only be used to de-active in case of overturn and put back on afterwards and otherwise the ACC could turn on automatically when the distance to the car in front is below a threshold?
+
+### Results
+
+Topic naming: /paf/hero/*
+
+traffic light message:
+
+ string color
+ float32 distance
+
+acc message:
+
+ bool activate/deactivate
+
+Open:
+
+traffic sign message:
+
+ bool isStop
+ float distance
+
+speed limit message:
+
+ float speedlimit in m/s
+ float distance
+
+* Perception
+ * crossing traffic dependent on segmentation/lidar model accuracy
+ * speed limits are part of opendrive file/ could also have redundent street signs/
+
+* Acting
+ * Speed in m/s
+
+## Acting
+
+* Unpark routine/ other scenarios handled by acting/ hard coded behavior
+
+## Perception
+
+no questions so far
+
+### Sources