diff --git a/build/docker-compose.yml b/build/docker-compose.yml index a8aa1b2c..89ae5936 100644 --- a/build/docker-compose.yml +++ b/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 diff --git a/code/acting/src/acting/helper_functions.py b/code/acting/src/acting/helper_functions.py index 9c5476e0..517258a4 100755 --- a/code/acting/src/acting/helper_functions.py +++ b/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) diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index bd0f258f..08346a00 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/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.") diff --git a/code/acting/src/acting/velocity_publisher_dummy.py b/code/acting/src/acting/velocity_publisher_dummy.py index 70abb23c..7b8f1fb0 100755 --- a/code/acting/src/acting/velocity_publisher_dummy.py +++ b/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): diff --git a/code/test_planning_top/behavior_agent/src/behavior_agent/behaviours/maneuvers.py b/code/test_planning_top/behavior_agent/src/behavior_agent/behaviours/maneuvers.py index 1c3ebe52..ed0dbbc0 100755 --- a/code/test_planning_top/behavior_agent/src/behavior_agent/behaviours/maneuvers.py +++ b/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)) diff --git a/doc/05_acting/02_acc.md b/doc/05_acting/02_acc.md index fd4c193d..a4cc6b12 100644 --- a/doc/05_acting/02_acc.md +++ b/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) diff --git a/doc/dev_rounds/sprint_4.md b/doc/07_dev_talks/sprint_4.md similarity index 99% rename from doc/dev_rounds/sprint_4.md rename to doc/07_dev_talks/sprint_4.md index b4de94fc..ad2a29f5 100644 --- a/doc/dev_rounds/sprint_4.md +++ b/doc/07_dev_talks/sprint_4.md @@ -7,7 +7,9 @@ ## Author Josef Kircher + Julian Graf + Marco Riedenauer ## Date diff --git a/doc/07_dev_talks/sprint_5.md b/doc/07_dev_talks/sprint_5.md new file mode 100644 index 00000000..aab35610 --- /dev/null +++ b/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 + +--- + +* [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) + + +## 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