def control(self, event):
command = Command()
command.mode = command.MODE_PASS_THROUGH
command.ignore = command.IGNORE_NONE
# edit starting right here!!!!
# you can get GPS sensor information:
# self.currentGPS.latitude # Deg
# self.currentGPS.longitude # Deg
# self.currentGPS.altitude # m
# self.currentGPS.speed # m/s
# self.currentGPS.ground_course # atan2(east/north)
# code below computes ground course in rad clockwise from the north
# ground_course = self.currentGPS.ground_course
# if ground_course < 0:
# ground_course += 2*pi
command.F = 0.75 # throttle command 0.0 to 1.0
command.x = 0.0 # aileron servo command -1.0 to 1.0 positive rolls to right
command.y = -0.03 # elevator servo command -1.0 to 1.0 positive pitches up
command.z = 0.0 # rudder servo command -1.0 to 1.0 positive yaws to left
self.command_publisher.publish(command)
# add aileron control
del_a = # ?
# add elevator control here
del_e = # ?
# add rudder control here
del_r
# add thrust control here
del_f
if __name__ == '__main__':
# this section communitcates with gazebo through ROS you do not need to modify
# this section
sub = rospy.Subscriber('/dflat', Dflat, control)
pub = rospy.Publisher('/junker/command', Command, queue_size = 10)
rospy.init_node('att_contr', anonymous=True)
rate = rospy.Rate(150)
while not rospy.is_shutdown():
cmd = Command()
cmd.mode = Command.MODE_PASS_THROUGH
cmd.ignore = Command.IGNORE_NONE
cmd.x = del_a
cmd.y = del_e
cmd.z = del_r
cmd.F = del_f
pub.publish(cmd)
rate.sleep()
command_pub = rospy.Publisher('high_level_command', Command, queue_size=1)
sleep_time = 0.05
sleep_rate = 1.0 / sleep_time
rate = rospy.Rate(sleep_rate)
# retrieve benchmarks
bench_times = rospy.get_param('~bench_times')
bench_types = rospy.get_param('~bench_types')
bench_com_x = rospy.get_param('~bench_com_x')
bench_com_y = rospy.get_param('~bench_com_y')
bench_com_z = rospy.get_param('~bench_com_z')
bench_com_F = rospy.get_param('~bench_com_F')
command_k = Command()
command_k.mode = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE
command_k.ignore = 8
command_k.F = 0.0
command_k.x = 0.0
command_k.y = 0.0
command_k.z = 0.0
time_k = bench_times.pop(0)
type_k = getCommandType(bench_types.pop(0))
comx_k = bench_com_x.pop(0)
comy_k = bench_com_y.pop(0)
comz_k = bench_com_z.pop(0)
comF_k = bench_com_F.pop(0)
idx = 0
while not rospy.is_shutdown():
command_k.header.stamp = rospy.Time.now()
