From 590d86f591c87cc8e702dc1959d888ba6facf703 Mon Sep 17 00:00:00 2001 From: Zixu Zhang Date: Tue, 21 Feb 2023 14:16:11 -0500 Subject: [PATCH] fix bug in control thread --- ROS_Core/src/Labs/Lab1/cfg/planner.cfg | 2 +- .../src/Labs/Lab1/scripts/traj_planner.py | 40 +++++++++---------- .../Labs/Lab1/scripts/utils/generate_pwm.py | 2 +- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/ROS_Core/src/Labs/Lab1/cfg/planner.cfg b/ROS_Core/src/Labs/Lab1/cfg/planner.cfg index 41556e2..e15f018 100644 --- a/ROS_Core/src/Labs/Lab1/cfg/planner.cfg +++ b/ROS_Core/src/Labs/Lab1/cfg/planner.cfg @@ -5,6 +5,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("latency", double_t, 0, "Latency compensation for control", 0.05, -1, 1) +gen.add("latency", double_t, 0, "Latency compensation for control", 0.25, -1, 1) exit(gen.generate("racecar_planner", "racecar_planner", "planner")) \ No newline at end of file diff --git a/ROS_Core/src/Labs/Lab1/scripts/traj_planner.py b/ROS_Core/src/Labs/Lab1/scripts/traj_planner.py index a3fc76c..c7654d9 100644 --- a/ROS_Core/src/Labs/Lab1/scripts/traj_planner.py +++ b/ROS_Core/src/Labs/Lab1/scripts/traj_planner.py @@ -222,7 +222,7 @@ def control_thread(self): ''' Main control thread to publish control command ''' - rate = rospy.Rate(50) + rate = rospy.Rate(40) u_queue = queue.Queue() # values to keep track of the previous control command @@ -234,7 +234,7 @@ def dyn_step(x, u, dt): dx = np.array([x[2]*np.cos(x[3]), x[2]*np.sin(x[3]), u[0], - x[2]*np.tan(u[1]*1.05)/0.257, + x[2]*np.tan(u[1]*1.1)/0.257, 0 ]) x_new = x + dx*dt @@ -258,10 +258,11 @@ def dyn_step(x, u, dt): # check if there is new state available if self.control_state_buffer.new_data_available: odom_msg = self.control_state_buffer.readFromRT() - t_prev = odom_msg.header.stamp.to_sec() + t_slam = odom_msg.header.stamp.to_sec() u = np.zeros(3) - while not u_queue.empty() and u_queue.queue[0][-1] < t_prev: + u[-1] = t_slam + while not u_queue.empty() and u_queue.queue[0][-1] < t_slam: u = u_queue.get() # remove old control commands # get the state from the odometry message @@ -277,27 +278,26 @@ def dyn_step(x, u, dt): psi, u[1] ]) - - # update the state buffer for the planning thread - plan_state = np.append(state_cur, t_act) - self.plan_state_buffer.writeFromNonRT(plan_state) - - # update the current state use past control command + + # predict the current state use past control command for i in range(u_queue.qsize()): u_next = u_queue.queue[i] - dt = u_next[-1] - t_prev + dt = u_next[-1] - u[-1] state_cur = dyn_step(state_cur, u, dt) u = u_next - t_prev = u[-1] - # update the cur state with the most recent control command - state_cur = dyn_step(state_cur, u, t_act - t_prev) + # predict the cur state with the most recent control command + state_cur = dyn_step(state_cur, u, t_act - u[-1]) + + # update the state buffer for the planning thread + plan_state = np.append(state_cur, t_act) + self.plan_state_buffer.writeFromNonRT(plan_state) # if there is no new state available, we do one step forward integration to predict the state elif prev_state is not None: t_prev = prev_u[-1] dt = t_act - t_prev - # predict the state when the control command is executed + # predict the state using the last control command is executed state_cur = dyn_step(prev_state, prev_u, dt) # Generate control command from the policy @@ -310,20 +310,20 @@ def dyn_step(x, u, dt): if state_ref is not None: accel, steer_rate = self.compute_control(state_cur, state_ref, u_ref, K) - steer = max(-0.35, min(0.35, prev_u[1] + steer_rate*dt)) + steer = max(-0.37, min(0.37, prev_u[1] + steer_rate*dt)) else: # reset the policy buffer if the policy is not valid rospy.logwarn("Try to retrieve a policy beyond the horizon! Reset the policy buffer!") self.policy_buffer.reset() # generate control command - if self.simulation: - throttle_pwm = accel - steer_pwm = steer - else: + if not self.simulation and state_cur is not None: # If we are using robot, # the throttle and steering angle needs to convert to PWM signal throttle_pwm, steer_pwm = self.pwm_converter.convert(accel, steer, state_cur[2]) + else: + throttle_pwm = accel + steer_pwm = steer # publish control command servo_msg = ServoMsg() diff --git a/ROS_Core/src/Labs/Lab1/scripts/utils/generate_pwm.py b/ROS_Core/src/Labs/Lab1/scripts/utils/generate_pwm.py index 5d3e1a3..009a2fe 100644 --- a/ROS_Core/src/Labs/Lab1/scripts/utils/generate_pwm.py +++ b/ROS_Core/src/Labs/Lab1/scripts/utils/generate_pwm.py @@ -45,7 +45,7 @@ def convert(self, accel: float, steer: float, v: float): v_bounded = v # negative pwm means turn left (positive steering angle) - steer_pwm = -np.clip(steer/0.35, -1, 1) + steer_pwm = -np.clip(steer/0.37, -1, 1) accel_bounded = np.sign(accel)*min(abs(accel), 2+v) # Generate Input vector