def update_step(): global turning_goal, theta_delta pos = get_d_position() goal_to_navigate_to = goals[int(math.ceil(pos))] theta = current_frame_odom.M.GetRPY()[2] gt_theta = current_frame_world.M.GetRPY()[2] target_pos = (goal_to_navigate_to[0], goal_to_navigate_to[1], 0) target_theta = goal_to_navigate_to[2] d_pos = tf_conversions.Vector(*target_pos) - current_frame_odom.p rho, alpha, beta = drive_to_pose_controller.rho_alpha_beta(d_pos.x(), d_pos.y(), theta, target_theta) # v = gain_rho * rho # omega = gain_alpha * alpha + gain_beta * beta v = 0.10 omega = -1.5 * theta_delta turn_rotation = math.degrees(wrapToPi(goals[int(math.ceil(pos))][2] - goals[int(math.ceil(pos))-1][2])) turn_distance = math.sqrt(np.sum((goals[int(math.ceil(pos))][:2] - goals[int(math.ceil(pos))-1][:2])**2)) turning_goal = abs(turn_rotation / turn_distance) > 150 # deg / m turning_goals.append(turning_goal) if turning_goal: # todo: do I need to disable the correction on sharp turns? v = 0.05 # omega *= 10 omega = 1 * wrapToPi(target_theta-theta) pass # Note: rho builds up over time if we turn for one goal, but using turning goal it never moves... # if rho < TURNING_TARGET_RANGE: # v = 0 # omega = gain_alpha * wrapToPi(target_theta-theta) # v, omega = drive_to_pose_controller.scale_velocities(v, omega, False) v_encoder = v #+ np.random.randn() * MAX_V / 10 omega_encoder = omega + np.random.randn() * MAX_OMEGA / 10 current_frame_odom.p += tf_conversions.Vector(dt * 1*v_encoder * math.cos(theta), dt * v_encoder * math.sin(theta), 0.0) current_frame_odom.M.DoRotZ(dt * omega_encoder) current_frame_world.p += tf_conversions.Vector(dt * v * math.cos(gt_theta), dt * v * math.sin(gt_theta), 0.0) current_frame_world.M.DoRotZ(dt * omega) # todo: make it so that this still works during sharp turns along_path_prediction(dt * v_encoder) dR = (WHEELBASE*dt*omega_encoder + 2*dt*v_encoder) / 2. dL = 2.*dt*v - dR orientation_prediction(dt * omega, dt * omega_encoder, dL, dR) xs.append(current_frame_odom.p.x()) ys.append(current_frame_odom.p.y()) thetas.append(current_frame_odom.M.GetRPY()[2]) gt_xs.append(current_frame_world.p.x()) gt_ys.append(current_frame_world.p.y()) gt_thetas.append(current_frame_world.M.GetRPY()[2])
def update_step():
global turning_goal
theta = current_frame_odom.M.GetRPY()[2]
gt_theta = current_frame_world.M.GetRPY()[2]
target_pos = (goal_to_navigate_to[0], goal_to_navigate_to[1], 0)
target_theta = goal_to_navigate_to[2]
d_pos = tf_conversions.Vector(*target_pos) - current_frame_odom.p
rho, alpha, beta = drive_to_pose_controller.rho_alpha_beta(
d_pos.x(), d_pos.y(), theta, target_theta)
# v = gain_rho * rho
# omega = gain_alpha * alpha + gain_beta * beta
# Note: rho builds up over time if we turn for one goal, but using turning goal it never moves...
# if rho < TURNING_TARGET_RANGE:
# v = 0
# omega = gain_alpha * wrapToPi(target_theta-theta)
if rho < TURNING_TARGET_RANGE or alpha > math.pi / 2 or alpha < -math.pi / 2:
# we're not in the stable region of the controller - rotate to face the goal
# Better would be to reverse the direction of motion, but this would break along-path localisation
v = 0
omega = gain_theta * wrapToPi(target_theta - theta)
else:
# we're in the stable region of the controller
v = gain_rho * rho
omega = gain_alpha * alpha + gain_beta * beta
v, omega = scale_velocities(v, omega, 0.2, 2.2, 4.8, turning_goal)
vN = np.random.randn() * MAX_V / 10
omegaN = np.random.randn() * MAX_OMEGA / 10
current_frame_odom.p += tf_conversions.Vector(
dt * 1 * (v + vN) * math.cos(theta),
dt * (v + vN) * math.sin(theta), 0.0)
current_frame_odom.M.DoRotZ(dt * (omega + omegaN))
current_frame_world.p += tf_conversions.Vector(dt * v * math.cos(gt_theta),
dt * v * math.sin(gt_theta),
0.0)
current_frame_world.M.DoRotZ(dt * omega)
xs.append(current_frame_odom.p.x())
ys.append(current_frame_odom.p.y())
thetas.append(current_frame_odom.M.GetRPY()[2])
gt_xs.append(current_frame_world.p.x())
gt_ys.append(current_frame_world.p.y())
gt_thetas.append(current_frame_world.M.GetRPY()[2])
def publish_goal(self, pose, lookahead_distance=0.0, stop_at_goal=False):
goal = Goal()
goal.pose.header.stamp = rospy.Time.now()
goal.pose.header.frame_id = "odom"
lookahead = tf_conversions.Frame(
tf_conversions.Vector(lookahead_distance, 0, 0))
# add the offset for navigating to the goal:
# (offset * odom).Inverse() * goal = odom.Invserse() * pose
# goal = (offset * odom) * odom.Inverse() * pose
# goal = offset * pose
original_pose_frame = tf_conversions.fromMsg(pose)
pose_frame = self.zero_odom_offset * original_pose_frame
original_pose_frame_lookahead = original_pose_frame * lookahead
pose_frame_lookahead = pose_frame * lookahead
goal.pose.pose.position.x = pose_frame_lookahead.p.x()
goal.pose.pose.position.y = pose_frame_lookahead.p.y()
goal.pose.pose.orientation = tf_conversions.toMsg(
pose_frame_lookahead).orientation
goal.stop_at_goal.data = stop_at_goal
self.goal_pub.publish(goal)
self.goal_plus_lookahead = tf_conversions.toMsg(
original_pose_frame_lookahead)
if self.publish_gt_goals:
try:
trans = self.tfBuffer.lookup_transform('map', 'odom',
rospy.Time())
trans_frame = tf_conversions.Frame(
tf_conversions.Rotation(trans.rotation.x, trans.rotation.y,
trans.rotation.z,
trans.rotation.w),
tf_conversions.Vector(trans.translation.x,
trans.translation.y,
trans.translation.z))
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = "map"
goal_pose.pose = tf_conversions.toMsg(trans_frame *
pose_frame_lookahead)
self.goal_pose_pub.publish(goal_pose)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
print('Could not lookup transform from /map to /odom')
pass
def process_odom_data(self, msg):
if self.ready:
self.mutex.acquire()
if self.goal_pos is None:
self.mutex.release()
return
current_frame = tf_conversions.fromMsg(msg.pose.pose)
theta = current_frame.M.GetRPY()[2]
d_pos = tf_conversions.Vector(*self.goal_pos) - current_frame.p
rho, alpha, beta = rho_alpha_beta(d_pos.x(), d_pos.y(), theta,
self.goal_theta)
if rho < self.turning_goal_distance or alpha > math.pi / 2 and alpha < -math.pi / 2:
# we're at a turning goal, so we should just turn
# or we're not in the stable region of the controller - rotate to face the goal
# better would be to reverse the direction of motion, but this would break along-path localisation
v = 0
omega = self.gain_theta * wrapToPi(self.goal_theta - theta)
else:
# we're in the stable region of the controller
v = self.gain_rho * rho
omega = self.gain_alpha * alpha + self.gain_beta * beta
v, omega = self.scale_velocities(v, omega, self.stop_at_goal)
if self.at_stopping_goal(rho, self.goal_theta - theta):
self.goal_pos = None
v = 0
omega = 0
self.mutex.release()
motor_command = Twist()
motor_command.linear.x = v
motor_command.angular.z = omega
self.pub_cmd_vel.publish(motor_command)
def np_to_frame(pose_tuple):
return tf_conversions.Frame(
tf_conversions.Rotation.RotZ(pose_tuple[2]),
tf_conversions.Vector(pose_tuple[0], pose_tuple[1], 0))