def callback(data, paramf): global dtv, dxv, dyv, tp, xp, yp, qp, ed global flag_first, flag_goal_met, flag_end, n_safe, n_goals global x_goal, y_goal, q_goal, t_goal, t_goal_psi, x0, y0, q0, t0, goal_array pub = rospy.Publisher('/mallard/cmd_vel', Twist, queue_size=10) twist = Twist() q_now = [data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w] # if it's the first run then zero is current position if flag_first: x0 = data.pose.position.x y0 = data.pose.position.y q0 = q_now x_goal = goal_array[n_goals, 0] y_goal = goal_array[n_goals, 1] q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) # if a goal has been met then increment the goal if flag_goal_met: x0 = goal_array[n_goals, 0] y0 = goal_array[n_goals, 1] q0 = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) n_goals = n_goals + 1 x_goal = goal_array[n_goals, 0] y_goal = goal_array[n_goals, 1] q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) # work out time it will take to get to new goal, xy and psi if flag_first or flag_goal_met: t0 = data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001 dist = math.sqrt(pow((x_goal - x0), 2) + pow((y_goal - y0), 2)) t_goal = kguseful.safe_div(dist, paramf['vel']) # avoid zero division stability ed = kguseful.err_psi_fun(q0, q_goal) t_goal_psi = abs(kguseful.safe_div(ed, paramf['psivel'])) flag_first = False flag_goal_met = False rospy.loginfo("t_goal_psi: %s, t_goal: %s", t_goal_psi, t_goal) rospy.loginfo("x: %s, y: %s", data.pose.position.x, data.pose.position.y) rospy.loginfo("xg: %s, yg: %s", x_goal, y_goal) rospy.loginfo("goal number: %s, end goal: %s", n_goals + 1, goal_array.shape[0]) # build difference history buffers and calculate velocities t_now = (data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001) - t0 # time since start of goal dtv.appendleft(t_now-tp) # time difference vector dxv.appendleft(data.pose.position.x-xp) # x difference vector dyv.appendleft(data.pose.position.y-yp) # y difference vector dpsi = kguseful.err_psi_fun(qp, q_now) dpsiv.appendleft(dpsi) # psi difference vector xvel = kglocal.vel_fun(list(dxv), list(dtv)) # velocity vectors x, y and psi yvel = kglocal.vel_fun(list(dyv), list(dtv)) psivel = kglocal.vel_fun(list(dpsiv), list(dtv)) # get current desired positions and velocities xdes = kglocal.desxy_fun(x_goal, x0, t_goal, t_now) ydes = kglocal.desxy_fun(y_goal, y0, t_goal, t_now) qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, t_now) xveldes = kglocal.desvel_fun(x_goal, x0, t_goal, t_now) yveldes = kglocal.desvel_fun(y_goal, y0, t_goal, t_now) psiveldes = kglocal.desvelpsi_fun(ed, t_goal_psi, t_now, paramf['psivel']) # Get forces in nav frame using PD controller xf_nav = kglocal.cont_fun(data.pose.position.x, xdes, xvel, xveldes, paramf['kp'], paramf['kd'], paramf['lim']) yf_nav = kglocal.cont_fun(data.pose.position.y, ydes, yvel, yveldes, paramf['kp'], paramf['kd'], paramf['lim']) psif_nav = kglocal.contpsi_fun(q_now, qdes, psivel, psiveldes, paramf['kp_psi'], paramf['kd_psi'], paramf['lim_psi']) # put xy forces into body frame f_body = kguseful.quat_rot([xf_nav, yf_nav, 0], [-q_now[0], -q_now[1], -q_now[2], q_now[3]]) # put forces into twist structure and publish twist.linear.x = f_body[0] twist.linear.y = f_body[1] twist.angular.z = -psif_nav # minus is a fix to account for wrong direction on el_mal if n_safe > paramf['nv'] + 5: # stop output while deque buffers are filling pub.publish(twist) # publish twist command n_safe = n_safe + 1 # if goal is met then move to next goal if abs(x_goal - data.pose.position.x) <= paramf['goal_tol']: if abs(y_goal - data.pose.position.y) <= paramf['goal_tol']: e_psi = kguseful.err_psi_fun(q_now, q_goal) if abs(e_psi) <= paramf['goal_tol_psi']: if goal_array.shape[0] != n_goals + 1: # if there are more goals print 'goal met' flag_goal_met = True # set flag to move to next goal if goal_array.shape[0] == n_goals + 1: # if there are more goals print 'final goal met - holding position' # change current to previous values tp = t_now xp = data.pose.position.x yp = data.pose.position.y qp = q_now
def callback(data, paramf): global dtv, dxv, dyv, tp, xp, yp, qp, ed global flag_first, flag_goal_met, flag_end, n_safe, n_goals global x_goal, y_goal, q_goal, t_goal, t_goal_psi, x0, y0, q0, t0, goal_array global flag_obstacle_close, flag_obstacle_prev, fobs, t_stall # THIS CODE ADDED FOR RVIZ COVERAGE SELECTION # ------------------ # if no goal positions exist in goal_array, then exit this callback by using return # no further computation is undertaken if len(goal_array) == 0: return # ------------------ # pub = rospy.Publisher('/mallard/cmd_vel', Twist, queue_size=10) twist = Twist() q_now = [ data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w ] ##### code added for obstacle avoidance global param_obs, lidar_data if lidar_data is not None: flag_obstacle_close, fobs = detect_obstacle(lidar_data, param_obs) # rospy.loginfo("obstacle avoidance forces: %s", fobs) # rospy.loginfo("obstacle close flag: %s", flag_obstacle_close) else: flag_obstacle_close = False fobs = (0, 0) # Force obstacle avoidance to be null # --------------------------------- # Remove these two lines to use obstacle avoidance # flag_obstacle_close = False fobs = (0, 0) # ---------------------------------- # if it's the first run then zero is current position if flag_first: x0 = data.pose.position.x y0 = data.pose.position.y q0 = q_now x_goal = goal_array[n_goals, 0] y_goal = goal_array[n_goals, 1] q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) # if a obstacle is to close flag is set if flag_obstacle_close != flag_obstacle_prev: if flag_obstacle_close: t_stall = data.header.stamp.secs x0 = data.pose.position.x + 1e-9 y0 = data.pose.position.y + 1e-9 q0 = q_now x_goal = data.pose.position.x y_goal = data.pose.position.y q_goal = q_now print 'STOPPING!' else: t_stall = data.header.stamp.secs - t_stall x0 = data.pose.position.x y0 = data.pose.position.y q0 = q_now x_goal = goal_array[n_goals, 0] y_goal = goal_array[n_goals, 1] q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) print 'RESUMING' # if a goal has been met then increment the goal if flag_goal_met and not flag_obstacle_close: x0 = goal_array[n_goals, 0] y0 = goal_array[n_goals, 1] q0 = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) n_goals = n_goals + 1 x_goal = goal_array[n_goals, 0] y_goal = goal_array[n_goals, 1] q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) # work out time it will take to get to new goal, xy and psi if flag_first or flag_goal_met: t0 = data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001 dist = math.sqrt(pow((x_goal - x0), 2) + pow((y_goal - y0), 2)) t_goal = kguseful.safe_div( dist, paramf['vel']) # avoid zero division stability ed = kguseful.err_psi_fun(q0, q_goal) t_goal_psi = abs(kguseful.safe_div(ed, paramf['psivel'])) flag_first = False flag_goal_met = False rospy.loginfo("t_goal_psi: %s, t_goal: %s", t_goal_psi, t_goal) rospy.loginfo("x: %s, y: %s", data.pose.position.x, data.pose.position.y) rospy.loginfo("xg: %s, yg: %s", x_goal, y_goal) rospy.loginfo("goal number: %s, end goal: %s", n_goals + 1, goal_array.shape[0]) # build difference history buffers and calculate velocities t_now = (data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001) - t0 # time since start of goal dtv.appendleft(t_now - tp) # time difference vector dxv.appendleft(data.pose.position.x - xp) # x difference vector dyv.appendleft(data.pose.position.y - yp) # y difference vector dpsi = kguseful.err_psi_fun(qp, q_now) dpsiv.appendleft(dpsi) # psi difference vector xvel = kglocal.vel_fun(list(dxv), list(dtv)) # velocity vectors x, y and psi yvel = kglocal.vel_fun(list(dyv), list(dtv)) psivel = kglocal.vel_fun(list(dpsiv), list(dtv)) # get current desired positions and velocities xdes = kglocal.desxy_fun(x_goal, x0, t_goal, t_now) ydes = kglocal.desxy_fun(y_goal, y0, t_goal, t_now) qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, t_now) xveldes = kglocal.desvel_fun(x_goal, x0, t_goal, t_now) yveldes = kglocal.desvel_fun(y_goal, y0, t_goal, t_now) psiveldes = kglocal.desvelpsi_fun(ed, t_goal_psi, t_now, paramf['psivel']) # Get forces in nav frame using PD controller xf_nav = kglocal.cont_fun(data.pose.position.x, xdes, xvel, xveldes, paramf['kp'], paramf['kd'], paramf['lim']) yf_nav = kglocal.cont_fun(data.pose.position.y, ydes, yvel, yveldes, paramf['kp'], paramf['kd'], paramf['lim']) psif_nav = kglocal.contpsi_fun(q_now, qdes, psivel, psiveldes, paramf['kp_psi'], paramf['kd_psi'], paramf['lim_psi']) # put xy forces into body frame f_body = kguseful.quat_rot([xf_nav, yf_nav, 0], [-q_now[0], -q_now[1], -q_now[2], q_now[3]]) # print flag_obstacle_close, np.round(fobs,4) # put forces into twist structure and publish twist.linear.x = f_body[0] + fobs[0] twist.linear.y = f_body[1] + fobs[1] twist.angular.z = -psif_nav # minus is a fix to account for wrong direction on el_mal if n_safe > paramf['nv'] + 5: # stop output while deque buffers are filling pub.publish(twist) # publish twist command n_safe = n_safe + 1 # if goal is met then move to next goal if not flag_obstacle_close: if abs(x_goal - data.pose.position.x) <= paramf['goal_tol']: if abs(y_goal - data.pose.position.y) <= paramf['goal_tol']: e_psi = kguseful.err_psi_fun(q_now, q_goal) if abs(e_psi) <= paramf['goal_tol_psi']: if goal_array.shape[ 0] != n_goals + 1: # if there are more goals print 'goal met' flag_goal_met = True # set flag to move to next goal if goal_array.shape[ 0] == n_goals + 1: # if there are more goals print 'final goal met - holding position' # change current to previous values tp = t_now xp = data.pose.position.x yp = data.pose.position.y qp = q_now flag_obstacle_prev = flag_obstacle_close