From 739d1db64feb570d1d3aeeebdf1876f1dbede58b Mon Sep 17 00:00:00 2001 From: Alexander Koufos Date: Fri, 25 Sep 2020 14:44:40 -0700 Subject: [PATCH] Issue #3: Cleans up code - Removes debugging information that was never meant to be committed. - Updates some comments. --- script/controller.py | 25 ++++++------------------- 1 file changed, 6 insertions(+), 19 deletions(-) diff --git a/script/controller.py b/script/controller.py index a678a6d..4fa593b 100755 --- a/script/controller.py +++ b/script/controller.py @@ -247,9 +247,11 @@ def timer_cb(self, event): if self.pathReady and self.stateReady: pos_x, pos_y = self.state.get_position() - # pick target w/o collision avoidance, closest point on the traj and one point ahead + # Pick target w/o collision avoidance. Find the closest point in the + # trajectory tree. _, idx = self.path_tree.query([pos_x, pos_y]) - # Target point for steering + + # Steering target. Three points ahead of closest point. if idx < self.traj_steps - 3: target_pt = self.path_tree.data[idx + 3, :] # str_idx = idx + 2 @@ -258,7 +260,7 @@ def timer_cb(self, event): # str_idx = self.vel_path.shape[0] - 1 print("CONTROLLER: at the end of the desired waypoits!!!") - # Target point for velocity + # Velocity target. Use the desired velocity from closest point. if idx < self.traj_steps: target_vel = self.vel_path[idx, :] # str_idx = idx + 2 @@ -304,20 +306,7 @@ def timer_cb(self, event): cmd_msg.acceleration cmd_msg.jerk = jerk - # Print out some debugging information - if abs(target_speed - self.max_speed) > 2.0: - # print("Posit diff:", pos_diff) - print("Speed diff:", speed_diff) - print("Current speed:", self.state.get_speed()) - print("Desired speed:", target_speed) - print("Desired accel:", acceleration) - print("Desired jerk:", jerk) - print("Speed (sent):", cmd_msg.speed) - print("Accel (sent):", cmd_msg.acceleration) - print("Jerk (sent):", cmd_msg.jerk) - print("delta t:", delta_t) - - # for visualization purposes and debuging control node + # For visualization purposes and debuging control node mk_msg = Marker() mk_msg.header.stamp = rospy.Time.now() mk_msg.header.frame_id = 'map' @@ -375,8 +364,6 @@ def compute_ackermann_steer(self, target_pt): else: d_steer = 0.0 - # print("steer, d_steer:", steer, d_steer) - steer = steer + d_steer*self.pid_str_deriv self.steer_cache = steer