Exemple #1
0
def control_callback(event):
    
    # Actual control is done here. The 'event' is the rospy.Timer() duration period, can be used 
    # for trubleshooting. To test how often is executed use: $ rostopic hz /mallard/thruster_commands.
    
    global x0,y0,q0,x_goal,y_goal,q_goal,ed
    global time_elapsed,t_goal,t_goal_psi
    global t_ramp,psivel
    twist = Twist()


    #  Get forces in global frame using PD controller
    if(goals_received == True and joy_button_L1 == 0 and joy_button_R1 == 0):

        
        # new code - velocity ramp
        # get current time
        tn = rospy.Time.now()
        time_now = tn.secs + (tn.nsecs * 0.000000001)
        # time elapsed form the start of tracking the goal (equal to t_now in goal selector)
        time_elapsed = time_now - time_begin
        # get max velocities
        xvelmax = abs(kguseful.safe_div((x_goal-x0),t_goal))
        yvelmax = abs(kguseful.safe_div((y_goal-y0),t_goal))
        # get desired position,velocity and acceleration from velocity ramp
        x_des, x_vel_des, x_acc_des = kglocal.velramp(time_elapsed, xvelmax, x0, x_goal, t_ramp,name="x")
        y_des, y_vel_des, y_acc_des = kglocal.velramp(time_elapsed, yvelmax, y0, y_goal, t_ramp,name="y")
        # get desired angle:
        qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, time_elapsed)
        psi_des = tft.euler_from_quaternion(qdes)[2] # Its a list: (roll,pitch,yaw)
        # psi_des = psides[2]
        psi_vel_des = kglocal.desvelpsi_fun(ed, t_goal_psi, time_elapsed,psivel)
        # end new code
        
        
        # PD control
        # x_global_ctrl   = control.proportional(x, x_goal, x_vel, x_vel_goal, param['kp'], param['kd'], param['lim'])
        # y_global_ctrl   = control.proportional(y, y_goal, y_vel, y_vel_goal, param['kp'], param['kd'], param['lim'])
        psi_global_ctrl = control.proportional_angle(psi, psi_des,psi_vel,psi_vel_des, param['kp_psi'], param['kd_psi'], param['lim_psi'])
        # PD body control
        # x_PD_body = math.cos(psi)*x_global_ctrl + math.sin(psi)*y_global_ctrl
        # y_PD_body =-math.sin(psi)*x_global_ctrl + math.cos(psi)*y_global_ctrl 

        
        # ----- Model control -----        
        ax = control.acc_ctrl(x, x_des, x_vel, x_vel_des,x_acc_des,param_model_x['kp'], param_model_x['kd'], param_model_x['lim'])
        ay = control.acc_ctrl(y, y_des, y_vel, y_vel_des,y_acc_des,param_model_y['kp'], param_model_y['kd'], param_model_y['lim'])
        # Model body control:
        # aqx = Rt * [ax,ay]t
        # vqx = Rt * [x_vel,y_vel]t
        aqx =  math.cos(psi)*ax + math.sin(psi)*ay
        aqy = -math.sin(psi)*ax + math.cos(psi)*ay
        vqx =  math.cos(psi)*x_vel + math.sin(psi)*y_vel
        vqy = -math.sin(psi)*x_vel + math.cos(psi)*y_vel
        # print("X-velocity: " + str(round(vqx,4)))
        x_body_model_ctrl = Mx*aqx + R1_x*vqx + R2_x*(vqx*abs(vqx))
        # x_body_model_ctrl = Mx*aqx + R2_x*(vqx*abs(vqx))
        y_body_model_ctrl = My*aqy + R1_y*vqy + R2_y*(vqy*abs(vqy))
        # y_body_model_ctrl = My*aqy + R2_y*(vqy*abs(vqy))

        # twist.angular.y = data.buttons[4]
        # vector forces scaled in body frame
        twist.linear.x   = x_body_model_ctrl
        # twist.linear.x  = x_PD_body
        twist.linear.y  = y_body_model_ctrl
        # twist.linear.y  = y_PD_body
        twist.angular.z = -psi_global_ctrl

        # Test uniformity of the timer 
        now = rospy.Time.now()
        # twist.angular.x = now.secs
        # twist.angular.y = now.nsecs
        
        # Publish forces to simulation (joint_state_publisher message)
        pub_velocity.publish(twist)

        # send [time,position,velocity,goal_position,goal_velocity,control input]
        array_data = [now.secs,now.nsecs,\
                      x,y,psi,x_vel,y_vel,psi_vel,\
                      x_des,y_des,psi_des,x_vel_des,y_vel_des,psi_vel_des,\
                      x_body_model_ctrl,y_body_model_ctrl,psi_global_ctrl]
                      
        data_to_send = Float64MultiArray(data = array_data)
        pub_data.publish(data_to_send)

        # goals_received, xdes,ydes,psides[2],\
            #  xveldes,yveldes,psiveldes,ax,ay]

    elif(joy_button_L1 == 1 or joy_button_R1 == 1):
        twist.linear.x = joy_x
        twist.linear.y = joy_y
        twist.angular.z = joy_z
        pub_velocity.publish(twist)   
    else:
        # ----- idle if no goals -----
        pub_velocity.publish(Twist())
Exemple #2
0
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

    # ---- simulation----
    global linear_scale, angular_scale, thruster_1, thruster_2, thruster_3, thruster_4, a_sim, b_sim
    #  ------------------

    # 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
    # ---------------------------------------- OBSTACLE removed
    # 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
    # ---------------------------------------- OBSTACLE removed
    # 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

    # ---------------------------------------- OBSTACLE removed
    # if flag_goal_met and not flag_obstacle_close:
    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

    # 'nv' parameter comes into play here
    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
    # xvelmax = abs((x_goal-x0)/t_goal)           # peak velocity in x and y
    xvelmax = abs(kguseful.safe_div((x_goal - x0), t_goal))
    yvelmax = abs(kguseful.safe_div((y_goal - y0), t_goal))
    xdes, xveldes = kglocal.velramp(t_now, xvelmax, x0, x_goal,
                                    param['t_ramp'])
    ydes, yveldes = kglocal.velramp(t_now, yvelmax, y0, y_goal,
                                    param['t_ramp'])
    # xdes = kglocal.desxy_fun(x_goal, x0, t_goal, t_now)
    # ydes = kglocal.desxy_fun(y_goal, y0, t_goal, t_now)
    # xveldes = kglocal.desvel_fun(x_goal, x0, t_goal, t_now)
    # yveldes = kglocal.desvel_fun(y_goal, y0, t_goal, t_now)
    qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, 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)

    # ------- Simulation ---------
    x_sim = (f_body[0]) * linear_scale
    y_sim = (f_body[1]) * linear_scale
    psi_sim = (-psif_nav) * angular_scale

    thruster_1 = 0 + 0.5 * x_sim + a_sim * psi_sim
    thruster_2 = 0 + 0.5 * x_sim - a_sim * psi_sim
    thruster_3 = 0 - 0.5 * y_sim + b_sim * psi_sim
    thruster_4 = 0 - 0.5 * y_sim - b_sim * psi_sim
    # ------- end simulation -------------

    # 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

        # -------- Simulation  --------

        pub_velocity.publish(thruster_ctrl_msg())

        # ------- end simulation -------------

    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
def slam_callback(data, paramf):
    global dtv, dxv, dyv, tp, xp, yp, qp, ed
    global flag_first, flag_goal_met, flag_end, n_safe, n_goals, goals_received
    global x_goal, y_goal, q_goal, t_goal, t_goal_psi, x0, y0, q0, t0, goal_array,psides
    global back_and_forth,single_goal,counter

    

    # if no goal positions exist, then exit this callback!!!
    if len(goal_array) == 0:
        data_to_send = Float64MultiArray(data = [goals_received])
        pub_goal.publish(data_to_send)
        return

    q_now = [data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w]

    # for first run get current position and load first goals:
    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 goal has been met assign new goals:
    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
        # --------------------------------------
        # to go back nad forth between two goals
        if(n_goals > 1 and back_and_forth):
            n_goals = 0
        elif(single_goal):
            
            if(counter <= 1000 and n_goals == 1):
                if(counter % 100 == 0): print("wait for 10 seconds; counting " + str(counter/100))
                n_goals = 0
                counter += 1
                # print("n_goals: " + str(n_goals))
            else: # maitain the goal
                if(n_goals == 2): print("GOAL REACHED")
                n_goals = 1
                counter = 0

                # print("n_goals: " + str(n_goals))
                
        # --------------------------------------
        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 and distance 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'])  
        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)
        

    # time since start of the goal:
    t_now = (data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001) - t0
    
    # GOALS - get desired linear positions and velocities
    xvelmax = abs(kguseful.safe_div((x_goal-x0), t_goal))
    yvelmax = abs(kguseful.safe_div((y_goal-y0), t_goal))

    name = "x"
    # print(name, " velocity: ",xvelmax)
    xdes, xveldes,ax = kglocal.velramp(t_now, xvelmax, x0, x_goal, param['t_ramp'],name)
    name = "y"
    # print(name, " velocity: ",yvelmax)
    ydes, yveldes,ay = kglocal.velramp(t_now, yvelmax, y0, y_goal, param['t_ramp'],name)
    # print("ax: ", ax, "xveldes: ",xveldes, " xdes: ",xdes)
    # get desired angular positions and velocities
    qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, t_now)

    # Angle (Psides) and angular velocity (psiveldes) in euler:
    psides = tft.euler_from_quaternion(qdes) # Its a list: (roll,pitch,yaw)
    psiveldes = kglocal.desvelpsi_fun(ed, t_goal_psi, t_now, paramf['psivel'])
    
    # VELOCITIES - calculate velocities from current and previous positions
    # 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))

    # Test if goal has been met:
    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 no more goals
                        print 'final goal met - holding position'

    #  --------- Publish goals ---------
    # publish goal array
    array = [goals_received, xdes,ydes,psides[2],\
             xveldes,yveldes,psiveldes,ax,ay]
    data_to_send = Float64MultiArray(data = array)
    pub_goal.publish(data_to_send)

    # 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'])
    # pub_goal.publish(goals_stamped)
    

    # PREVIOUS VALUES - change current to previous values
    tp = t_now
    xp = data.pose.position.x
    yp = data.pose.position.y
    qp = q_now
def control_callback(event):
    
    # Actual control is done here. The 'event' is the rospy.Timer() duration period, can be used 
    # for trubleshooting. To test how often is executed use: $ rostopic hz /mallard/thruster_commands.
    global x0,y0,q0,x_goal,y_goal,q_goal,ed
    global time_elapsed,t_goal,t_goal_psi
    global t_ramp,psivel  
    twist = Twist()

    #  Get forces in global frame using PD controller
    if(goals_received == True and joy_button_L1 == 0 and joy_button_R1 == 0):
        
        # new code - velocity ramp
        # get current time
        tn = rospy.Time.now()
        time_now = tn.secs + (tn.nsecs * 0.000000001)
        # time elapsed form the start of tracking the goal (equal to t_now in goal selector)
        time_elapsed = time_now - time_begin
        # get max velocities
        xvelmax = abs(kguseful.safe_div((x_goal-x0),t_goal))
        yvelmax = abs(kguseful.safe_div((y_goal-y0),t_goal))
        # get desired position,velocity and acceleration from velocity ramp
        x_des, x_vel_des, x_acc_des = kglocal.velramp(time_elapsed, xvelmax, x0, x_goal, t_ramp,name="x")
        y_des, y_vel_des, y_acc_des = kglocal.velramp(time_elapsed, yvelmax, y0, y_goal, t_ramp,name="y")
        # get desired angle:
        qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, time_elapsed)
        psi_des = tft.euler_from_quaternion(qdes)[2] # Its a list: (roll,pitch,yaw)
        psi_vel_des = kglocal.desvelpsi_fun(ed, t_goal_psi, time_elapsed,psivel)
        # end new code
         
        # PD global control
        x_global_ctrl   = control.proportional(x, x_goal, x_vel, x_vel_goal, param['kp'], param['kd'], param['lim'])
        y_global_ctrl   = control.proportional(y, y_goal, y_vel, y_vel_goal, param['kp'], param['kd'], param['lim'])
        psi_global_ctrl = control.proportional_angle(psi, psi_goal,psi_vel,psi_vel_goal, param['kp_psi'], param['kd_psi'], param['lim_psi'])
        # Convert to body coordiante
        x_PD_body = math.cos(psi)*x_global_ctrl + math.sin(psi)*y_global_ctrl
        y_PD_body =-math.sin(psi)*x_global_ctrl + math.cos(psi)*y_global_ctrl 

        # Publish to twist velocitycommand
        twist.linear.x  = x_PD_body
        twist.linear.y  = y_PD_body
        twist.angular.z = -psi_global_ctrl

        # Generate time instance
        now = rospy.Time.now()
        twist.angular.x = now.secs
        twist.angular.y = now.nsecs

        # Publish forces to simulation (joint_state_publisher message)
        pub_velocity.publish(twist)

        # send [time,position,velocity,goal_position,goal_velocity,control input]
        array_data = [now.secs,now.nsecs,\  
                      x,y,psi,x_vel,y_vel,psi_vel,\
                      x_des,y_des,psi_des,x_vel_des,y_vel_des,psi_vel_des,\
                      x_body_model_ctrl,y_body_model_ctrl,psi_global_ctrl]

        data_to_send = Float64MultiArray(data = array_data)
        pub_data.publish(data_to_send)

    elif(joy_button_L1 == 1 or joy_button_R1 == 1):
        twist.linear.x = joy_x
        twist.linear.y = joy_y
        twist.angular.z = joy_z
        pub_velocity.publish(twist)   
    else:
        # ----- idle if no goals -----
        pub_velocity.publish(Twist())