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/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 0ea73aa2c..89de04639 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, 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 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 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: + self.turn_cmd.ui16_cmd = TURN_RIGHT + else: + self.turn_cmd.ui16_cmd = TURN_NONE self.turn_blink_pub.publish(self.turn_cmd) pass @@ -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" 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'