From edfc9753e3028aaec525e31f412d18ce01399078 Mon Sep 17 00:00:00 2001 From: Dazhen Chen Date: Fri, 7 Feb 2025 01:19:26 +0000 Subject: [PATCH 1/2] hw1.3 --- .../knowledge/defaults/computation_graph.yaml | 3 +++ homework/blink.py | 16 ++++++++-------- homework/blink_launch.yaml | 4 ++-- 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/GEMstack/knowledge/defaults/computation_graph.yaml b/GEMstack/knowledge/defaults/computation_graph.yaml index d7a606326..b6d5c92f7 100644 --- a/GEMstack/knowledge/defaults/computation_graph.yaml +++ b/GEMstack/knowledge/defaults/computation_graph.yaml @@ -52,3 +52,6 @@ components: - trajectory_tracking: inputs: [vehicle, trajectory] outputs: + - signaling: + inputs: [intent] + outputs: diff --git a/homework/blink.py b/homework/blink.py index 0ea73aa2c..b8b07c697 100644 --- a/homework/blink.py +++ b/homework/blink.py @@ -6,7 +6,6 @@ TURN_NONE = 1 TURN_LEFT = 2 TURN_HAZARD = 3 -TURN_AROUND = 4 # For message format, see # https://github.com/astuff/astuff_sensor_msgs/blob/3.3.0/pacmod_msgs/msg/PacmodCmd.msg @@ -48,19 +47,20 @@ def cleanup(self): def get_dir_distress(self): pass - def update(self): + def update(self, inputs): """Run in a loop""" # TODO: Implement your control loop here # You will need to publish a PacmodCmd() to /pacmod/as_rx/turn_cmd. Read the documentation to see # what the data in the message indicates. #self.turn_cmd = PacmodCmd() # TODO change to actual direction in Part 2 - if self.turn_cmd.ui16_cmd == TURN_NONE: - self.turn_cmd.ui16_cmd = TURN_LEFT - elif self.turn_cmd.ui16_cmd == TURN_LEFT: - self.turn_cmd.ui16_cmd = TURN_RIGHT - else: - self.turn_cmd.ui16_cmd = TURN_NONE + if inputs.intent.intent == "HALTING": + if self.turn_cmd.ui16_cmd == TURN_NONE: + self.turn_cmd.ui16_cmd = TURN_LEFT + elif self.turn_cmd.ui16_cmd == TURN_LEFT: + self.turn_cmd.ui16_cmd = TURN_RIGHT + else: + self.turn_cmd.ui16_cmd = TURN_NONE self.turn_blink_pub.publish(self.turn_cmd) pass diff --git a/homework/blink_launch.yaml b/homework/blink_launch.yaml index 41174f692..0b17ac054 100644 --- a/homework/blink_launch.yaml +++ b/homework/blink_launch.yaml @@ -9,14 +9,14 @@ recovery: state_estimation : GNSSStateEstimator perception_normalization : StandardPerceptionNormalizer planning: - trajectory_tracking : blink_component.BlinkDistress + signaling: blink_component.BlinkDistress # Driving behavior for the GEM vehicle. Runs perception and planner but doesn't execute anything (no controller). drive: perception: state_estimation : GNSSStateEstimator perception_normalization : StandardPerceptionNormalizer planning: - trajectory_tracking : blink_component.BlinkDistress + signaling: blink_component.BlinkDistress log: # Specify the top-level folder to save the log files. Default is 'logs' folder : 'logs' From dfbafe25b991993ea9cf9dd286ca5e088009f81b Mon Sep 17 00:00:00 2001 From: Dazhen Chen Date: Wed, 12 Feb 2025 22:10:58 +0000 Subject: [PATCH 2/2] finalize hw1.3 --- GEMstack/onboard/planning/blink_component.py | 3 +++ homework/blink.py | 8 ++++---- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/GEMstack/onboard/planning/blink_component.py b/GEMstack/onboard/planning/blink_component.py index 4415c0c82..9ee6a9164 100644 --- a/GEMstack/onboard/planning/blink_component.py +++ b/GEMstack/onboard/planning/blink_component.py @@ -38,6 +38,9 @@ def update(self): # sent to the drive-by-wire system, simultaneously. To avoid doing arbitrary things # to the vehicle, let's maintain the current values (e.g., accelerator, brake pedal, # steering angle) from its current readings. + + #if Allstate.intent.intent == "HALTING" : + #return current_time = time.time() if current_time - self.last_update_time >= 2: # Change signal every 2 seconds if self.turn_state == TURN_OFF: diff --git a/homework/blink.py b/homework/blink.py index b8b07c697..89de04639 100644 --- a/homework/blink.py +++ b/homework/blink.py @@ -47,14 +47,14 @@ def cleanup(self): def get_dir_distress(self): pass - def update(self, inputs): + def update(self, Allstate): """Run in a loop""" # TODO: Implement your control loop here # You will need to publish a PacmodCmd() to /pacmod/as_rx/turn_cmd. Read the documentation to see # what the data in the message indicates. #self.turn_cmd = PacmodCmd() # TODO change to actual direction in Part 2 - if inputs.intent.intent == "HALTING": + if Allstate.intent.intent == "HALTING": if self.turn_cmd.ui16_cmd == TURN_NONE: self.turn_cmd.ui16_cmd = TURN_LEFT elif self.turn_cmd.ui16_cmd == TURN_LEFT: @@ -97,7 +97,7 @@ def accel_callback(self, msg): rospy.loginfo(f"Output: {msg.output} (Final Output to Vehicle)") rospy.loginfo("---------------------------\n") -def run_ros_loop(node): +def run_ros_loop(node, Allstate): """Executes the event loop of a node using ROS. `node` should support rate(), initialize(), cleanup(), update(), and done(). You should not modify this code. @@ -111,7 +111,7 @@ def run_ros_loop(node): termination_reason = "undetermined" try: while not rospy.is_shutdown() and not node.done() and node.healthy(): - node.update() + node.update(node, Allstate) rate.sleep() if node.done(): termination_reason = "Node done"