Example #1
0
    def timer_callback(self, event):
        #print(self.alt_height)
        # Pass wished yaw position (must be between -1 and 1) and measured yaw
        _, _, set_y = get_rpy_orientation(
            self.current_uav_setpoint.orientation)

        self.yaw_setpoint_pub.publish(set_y)
        self.yaw_state_pub.publish(self.yaw)

        # Pass wished altitude and measured altitude
        self.altitude_setpoint_pub.publish(
            self.current_uav_setpoint.position.z)
        self.altitude_state_pub.publish(self.alt_height)

        # Pass wished alt-rate and estimated speed
        self.altitude_rate_setpoint_pub.publish(self.alt_effort)
        #self.altitude_rate_setpoint_pub.publish(self.current_uav_setpoint.position.z)
        self.altitude_rate_state_pub.publish(self.alt_est_speed)

        rpyt = RollPitchYawrateThrust()
        # Define standard thrust, this should be roughly when the drone is standing still in the air.
        thrust_ref = 7.1125
        output = thrust_ref + self.alt_rate_effort
        # Pass thrust to uav
        rpyt.thrust.z = thrust_ref + self.alt_rate_effort
        # Pass pitch to uav
        rpyt.pitch = self.current_uav_setpoint.orientation.y
        # Pass roll to uav
        rpyt.roll = self.current_uav_setpoint.orientation.x
        # Pass yaw-rate to uav
        rpyt.yaw_rate = self.yaw_effort
        self.rpyt_pub.publish(rpyt)
 def sendCommand(self):
     msg = RollPitchYawrateThrust()
     msg.roll = self.cmd[0]
     msg.pitch = self.cmd[1]
     msg.yaw_rate = self.cmd[2]
     msg.thrust.z = self.cmd[3]
     self.pubcmd.publish(msg)
Example #3
0
 def pidaltitude(self, data):
     # We wish that our drones current position error is 0, the setpoint is the distance to the object / position we-
     # want to go to therefore we publish 0 to the PID state.
     self.PIDyaw_state.publish(0)
     self.PIDpitch_state.publish(0)
     self.PIDroll_state.publish(0)
     rotor_speed = RollPitchYawrateThrust()
     rotor_speed.roll = self.roll_control_effort.data
     rotor_speed.yaw_rate = self.yaw_control_effort.data
     rotor_speed.pitch = self.pitch_control_effort.data
     rotor_speed.thrust.z = float(data.data / 10.99934) + 7.31
     self.humm_rotor_pub.publish(rotor_speed)
    def timer_callback(self, event):
        '''
        Pass the variables to the relevant publishers.self

        yaw PID:
            input.orientation.z -> setpoint
            measured_yaw -> state

        Altitude PID:
            input.position.z (how high we want to go) -> setpoint
            measured_height -> state

        Altitude-rate PID:
            altitude_PID_output -> setpoint
            estimated_speed -> state

        RollPitchYawrateThrust (input to UAV):
            input.pitch -> pitch
            input.roll -> roll
            yaw_PID_output -> yaw-rate
            altitude_rate_PID_output -> thrust
        '''

        # Pass wished yaw position (must be between -1 and 1) and measured yaw
        self.yaw_setpoint_pub.publish(self.current_uav_setpoint.orientation.z)
        self.yaw_state_pub.publish(self.yaw)

        # Pass wished altitude and measured altitude
        self.altitude_setpoint_pub.publish(
            self.current_uav_setpoint.position.z)
        self.altitude_state_pub.publish(self.alt_height)

        # Pass wished alt-rate and estimated speed
        self.altitude_rate_setpoint_pub.publish(self.alt_effort)
        #self.altitude_rate_setpoint_pub.publish(self.current_uav_setpoint.position.z)
        self.altitude_rate_state_pub.publish(self.alt_est_speed)

        rpyt = RollPitchYawrateThrust()
        # Define standard thrust, this should be roughly when the drone is standing still in the air.
        thrust_ref = 7.11
        output = thrust_ref + self.alt_rate_effort
        # Pass thrust to uav
        rpyt.thrust.z = thrust_ref + self.alt_rate_effort
        # Pass pitch to uav
        rpyt.pitch = self.current_uav_setpoint.orientation.y
        # Pass roll to uav
        rpyt.roll = self.current_uav_setpoint.orientation.x
        # Pass yaw-rate to uav
        rpyt.yaw_rate = self.yaw_effort
        self.rpyt_pub.publish(rpyt)
Example #5
0
    def callbackCmdVel(self, data):
        if self.pub is None:
            return

        vel = data

        cmd = RollPitchYawrateThrust()
        cmd.header.stamp = rospy.Time.now()
        #  cmd.header.frame_id = "X3/base_link"
        cmd.roll = -vel.linear.y
        cmd.pitch = vel.linear.x
        cmd.yaw_rate = vel.angular.z
        cmd.thrust.z = vel.linear.z
        self.pub.publish(cmd)
Example #6
0
def main():
    print "Calibration uwp"
    rospy.init_node('calibration_unity')

    status = 0
    roll = 0.0
    pitch = 0.0
    yaw = 0.0
    thrust = Vector3()
    rpy = RollPitchYawrateThrust()
    while (1):
        key = getKey()
        status = status + 1
        if key == 'pageup':
            roll = roll + 0.1
        elif key == 'pagedown':
            roll = roll - 0.1
        elif key == 'up':
            pitch = pitch + 0.1
        elif key == 'down':
            pitch = pitch - 0.1
        elif key == 'right':
            yaw = yaw + 0.1
        elif key == 'left':
            yaw = yaw - 0.1
        else:
            if (key == '\x03'):
                break

        if status == 20:
            print roll + ", " + pitch + ", " + yaw
            status = 0

        rpy.roll = roll
        rpy.pitch = pitch
        rpy.yaw_rate = yaw
        rpy.thrust = thrust
        pub.publish(rpy)
Example #7
0
def do_control():

    odom_sub = rospy.Subscriber(model + '/ground_truth/odometry',
                                Odometry,
                                odom_cb,
                                queue_size=100)
    mpc_sub = rospy.Subscriber(model + '/rpyth',
                               RollPitchYawrateThrust,
                               mpc_cb,
                               queue_size=100)
    target_pose_sub = rospy.Subscriber(model + '/command/pose',
                                       PoseStamped,
                                       target_cb,
                                       queue_size=100)

    motor_speed_pub = rospy.Publisher(model +
                                      "/command/roll_pitch_yawrate_thrust",
                                      RollPitchYawrateThrust,
                                      queue_size=100)

    rospy.init_node('controller', anonymous=True)
    rate = rospy.Rate(50.0)

    while not rospy.is_shutdown():

        x_f = odom.pose.pose.position.x
        y_f = odom.pose.pose.position.y
        z_f = odom.pose.pose.position.z

        vx_f = odom.twist.twist.linear.x
        vy_f = odom.twist.twist.linear.y
        vz_f = odom.twist.twist.linear.z

        (roll, pitch, yaw) = quaternion_to_euler_angle(
            odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,
            odom.pose.pose.orientation.y, odom.pose.pose.orientation.z)
        r_f = math.radians(roll)
        p_f = math.radians(pitch)
        yaw_f = math.radians(yaw)

        rs_f = (float(odom.twist.twist.angular.x))
        ps_f = (float(odom.twist.twist.angular.y))
        ys_f = (float(odom.twist.twist.angular.z))

        target = np.array([
            target_pose.pose.position.x, target_pose.pose.position.y,
            target_pose.pose.position.z, 0., 0., 0., 0., 0., 0., 0.
        ],
                          ndmin=2)
        # target = np.array([1.,1.,1., 0.,0.,0., 0.,0., 0.,0.],ndmin=2)
        state = np.array(
            [x_f, y_f, z_f, vx_f, vy_f, vz_f, r_f, p_f, rs_f, ps_f], ndmin=2)

        inputs = state - target

        # Xtr = [[0.14604905, 0.51560138, 0.27780092, 0.8901413 , 0.90359776, 0.20449533, 0.27998261, 0.48336262, 0.36420253, 0.37711515]]
        # Xtr = np.reshape(Xtr, (1,10))

        inputs = (inputs - mean) / std
        motor_controls = np.ravel(get_actions(inputs))

        actuator = RollPitchYawrateThrust()
        actuator.header.stamp = rospy.Time.now()
        actuator.roll = motor_controls[0]
        actuator.pitch = motor_controls[1]
        # actuator.yaw_rate = motor_controls[2]
        actuator.thrust.z = (motor_controls[3] * 10.34) + (10.34)

        # actuator.roll = mpc_u.roll
        # actuator.pitch = mpc_u.pitch
        actuator.yaw_rate = mpc_u.yaw_rate
        # actuator.thrust.z = mpc_u.thrust.z

        print('roll: {:.5f}, pitch: {}, yaw_rate: {}, thrust: {}'.format(
            actuator.roll, actuator.pitch, actuator.yaw_rate,
            actuator.thrust.z))

        motor_speed_pub.publish(actuator)

        rate.sleep()
Example #8
0
def do_control():

    odom_sub = rospy.Subscriber('/f_450/odometry',
                                Odometry,
                                odom_cb,
                                queue_size=100)
    vrpn_sub = rospy.Subscriber('/f_450/vrpn_client_node/f_450/twist',
                                TwistStamped,
                                vrpn_cb,
                                queue_size=100)
    state_sub = rospy.Subscriber('/f_450/state_machine/state_info',
                                 String,
                                 state_cb,
                                 queue_size=100)
    target_pose_sub = rospy.Subscriber('/f_450/command/current_reference',
                                       MultiDOFJointTrajectory,
                                       target_cb,
                                       queue_size=100)
    mpc_sub = rospy.Subscriber('/f_450/command/roll_pitch_yawrate_thrust',
                               RollPitchYawrateThrust,
                               mpc_cb,
                               queue_size=100)

    control_pub = rospy.Publisher(
        "/f_450/mavros/setpoint_raw/roll_pitch_yawrate_thrust",
        RollPitchYawrateThrust,
        queue_size=100)

    rospy.init_node('controller', anonymous=True)
    rate = rospy.Rate(50.0)
    global target_flag_pub
    target = np.array([-1.6, -0.5, 0.5, 0., 0., 0., 0., 0.], ndmin=2)

    while not rospy.is_shutdown():

        x_f = odom.pose.pose.position.x
        y_f = odom.pose.pose.position.y
        z_f = odom.pose.pose.position.z

        vx_f = odom.twist.twist.linear.x
        vy_f = odom.twist.twist.linear.y
        vz_f = odom.twist.twist.linear.z

        (roll, pitch, yaw) = quaternion_to_euler_angle(
            odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,
            odom.pose.pose.orientation.y, odom.pose.pose.orientation.z)
        r_f = math.radians(roll)
        p_f = math.radians(pitch)
        yaw_f = math.radians(yaw)

        rs_f = float(vrpn.twist.angular.x)
        ps_f = float(vrpn.twist.angular.y)
        ys_f = float(vrpn.twist.angular.z)

        # target = np.array([target_pose.pose.position.x ,target_pose.pose.position.y, target_pose.pose.position.z, 0.,0.,0., 0.,0.,0., 0.,0.,0.],ndmin=2)
        # state = np.array([x_f,y_f,z_f, vx_f,vy_f,vz_f, r_f,p_f,yaw_f, rs_f,ps_f,ys_f],ndmin=2)

        if target_flag_pub == True:
            target = np.array([
                target_pose.points[0].transforms[0].translation.x,
                target_pose.points[0].transforms[0].translation.y,
                target_pose.points[0].transforms[0].translation.z, 0., 0., 0.,
                0., 0.
            ],
                              ndmin=2)

        target_flag_pub = False

        state = np.array([x_f, y_f, z_f, vx_f, vy_f, vz_f, r_f, p_f], ndmin=2)

        inputs = state - target
        inputs = (inputs - mean) / std

        print(str(state_machine.data))

        if str(state_machine.data) == 'PositionHold':

            print('MPC: ')
            controls = np.ravel(get_actions(inputs))
            rpyth_mpc = RollPitchYawrateThrust()

            # rpyth_mpc.header.stamp = rospy.Time.now()
            # rpyth_mpc.roll = controls[0]
            # rpyth_mpc.pitch = controls[1]
            # rpyth_mpc.yaw_rate = mpc_u.yaw_rate
            # rpyth_mpc.thrust.z = (controls[2] * 7.5) + 7.5

            rpyth_mpc.header.stamp = rospy.Time.now()
            rpyth_mpc.roll = mpc_u.roll
            rpyth_mpc.pitch = mpc_u.pitch
            rpyth_mpc.yaw_rate = mpc_u.yaw_rate
            rpyth_mpc.thrust.z = mpc_u.thrust.z

            control_pub.publish(rpyth_mpc)

        elif str(state_machine.data) == 'RcTeleOp' or str(
                state_machine.data) == 'RemoteControl' or str(
                    state_machine.data) == 'HaveOdometry':
            print('MPC')
            rpyth_mpc = RollPitchYawrateThrust()

            rpyth_mpc.header.stamp = rospy.Time.now()
            rpyth_mpc.roll = mpc_u.roll
            rpyth_mpc.pitch = mpc_u.pitch
            rpyth_mpc.yaw_rate = mpc_u.yaw_rate
            rpyth_mpc.thrust.z = mpc_u.thrust.z

            control_pub.publish(rpyth_mpc)

        rate.sleep()
Example #9
0
def do_control( ):
	
	odom_sub = rospy.Subscriber('/'+model+'/ground_truth/odometry', Odometry, odom_cb, queue_size=100)
	target_pose_sub = rospy.Subscriber('/'+ model + '/command/current_reference', MultiDOFJointTrajectory ,target_cb, queue_size=100)
	mpc_sub = rospy.Subscriber('/' + model + '/rpyth', RollPitchYawrateThrust,mpc_cb, queue_size=100)

	control_pub = rospy.Publisher(model + "/command/roll_pitch_yawrate_thrust", RollPitchYawrateThrust,queue_size=100)

	rospy.init_node('controller',anonymous=True)
	rate =  rospy.Rate(50.0)
	global target_flag_pub	
	target = np.array([-1.6, -0.3,0.5, 0.,0.,0., 0.,0.],ndmin=2)
	

	while not rospy.is_shutdown():

		x_f = odom.pose.pose.position.x
		y_f = odom.pose.pose.position.y
		z_f = odom.pose.pose.position.z

		vx_f = odom.twist.twist.linear.x
		vy_f = odom.twist.twist.linear.y
		vz_f = odom.twist.twist.linear.z

		(roll,pitch, yaw) = quaternion_to_euler_angle(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x , odom.pose.pose.orientation.y, odom.pose.pose.orientation.z)
		r_f = math.radians(roll)
		p_f = math.radians(pitch)
		yaw_f = math.radians(yaw)

		rs_f = (float(odom.twist.twist.angular.x))
		ps_f = (float(odom.twist.twist.angular.y))
		ys_f = (float(odom.twist.twist.angular.z))

		# target = np.array([target_pose.pose.position.x ,target_pose.pose.position.y, target_pose.pose.position.z, 0.,0.,0., 0.,0.,0., 0.,0.,0.],ndmin=2)
		# state = np.array([x_f,y_f,z_f, vx_f,vy_f,vz_f, r_f,p_f,yaw_f, rs_f,ps_f,ys_f],ndmin=2)

		if target_flag_pub == True:
			target = np.array([target_pose.points[0].transforms[0].translation.x , target_pose.points[0].transforms[0].translation.y,target_pose.points[0].transforms[0].translation.z, 0.,0.,0., 0.,0., 0.,0.],ndmin=2)
		
		
		# print(target_flag_pub)
		target_flag_pub = False
		
		


		state = np.array([x_f,y_f,z_f, vx_f,vy_f,vz_f, r_f,p_f, rs_f,ps_f],ndmin=2)

		inputs = state - target
		inputs = (inputs-mean)/std

		controls = np.ravel(get_actions(inputs))
		rpyth_mpc = RollPitchYawrateThrust()

		rpyth_mpc.header.stamp = rospy.Time.now()

		if(controller_type == 'dnn'):
			
			rpyth_mpc.roll = controls[0]
			rpyth_mpc.pitch = controls[1]
			rpyth_mpc.yaw_rate = mpc_u.yaw_rate
			rpyth_mpc.thrust.z = (controls[3] * constant_thrust) + constant_thrust
			print('DNN: roll: {:.3f} pitch: {:.3f} thrust: {:3f}'.format(rpyth_mpc.roll, rpyth_mpc.pitch, rpyth_mpc.thrust.z))
			
		
		else:
			rpyth_mpc.roll = mpc_u.roll
			rpyth_mpc.pitch = mpc_u.pitch
			rpyth_mpc.yaw_rate = mpc_u.yaw_rate
			rpyth_mpc.thrust.z = mpc_u.thrust.z
			print('MPC: roll: {:.3f} pitch: {:.3f} thrust: {:3f}'.format(rpyth_mpc.roll, rpyth_mpc.pitch, rpyth_mpc.thrust.z))

		print('Thrust Error: {:.3f}'.format(abs(z_f - target_pose.points[0].transforms[0].translation.z)))
		control_pub.publish(rpyth_mpc)



		rate.sleep()