def odometryCallback(self, msg):
# Get error between waypoint and current state
current_waypoint = np.array(
self.waypoint_list[self.current_waypoint_index])
current_position = np.array([
msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z
])
# orientation in quaternion form
qw = msg.pose.pose.orientation.w
qx = msg.pose.pose.orientation.x
qy = msg.pose.pose.orientation.y
qz = msg.pose.pose.orientation.z
# yaw from quaternion
y = np.arctan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy**2 + qz**2))
error = current_position - current_waypoint[0:3]
# replace next line with rotation matrix
# i_to_b = quaternion_matrix([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
i_to_b = np.eye(3)
print("i_to_b needs to be fixed ---- currently set to be identity",
i_to_b)
error_b = i_to_b * error
if np.linalg.norm(error) < self.threshold:
# Get new waypoint index
self.current_waypoint_index += 1
if self.cyclical_path:
self.current_waypoint_index %= len(self.waypoint_list)
else:
if self.current_waypoint_index > len(self.waypoint_list):
self.current_waypoint_index -= 1
next_waypoint = np.array(
self.waypoint_list[self.current_waypoint_index])
command_msg = Command()
command_msg.x = next_waypoint[0]
command_msg.y = next_waypoint[1]
command_msg.F = next_waypoint[2]
if len(current_waypoint) <= 3:
next_point = self.waypoint_list[(self.current_waypoint_index +
1) % len(self.waypoint_list)]
delta = next_point - current_waypoint
current_waypoint.append(np.atan2(delta[1], delta[0]))
# altitude send directly
command_msg.F = next_waypoint[2]
# velocity commands, Proportional gain and saturation
command_msg.x = saturate(2.0 * error_b[0], 2.0, -2.0)
command_msg.y = saturate(2.0 * error_b[0], 2.0, -2.0)
command_msg.z = saturate(1.507 * (current_waypoint[3] - y), 1.507,
-1.507)
command_msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE
self.waypoint_pub_.publish(command_msg)
def odometryCallback(self, msg):
# Get error between waypoint and current state
current_waypoint = np.array(
self.waypoint_list[self.current_waypoint_index])
(r, p, y) = tf.transformations.euler_from_quaternion([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
current_position = np.array([
msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z
])
error = current_position - current_waypoint[0:3]
i_to_b = tf.transformations.quaternion_matrix([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
print "i_to_b", i_to_b
error_b = i_to_b * error
if np.linalg.norm(error) < self.threshold:
# Get new waypoint index
self.current_waypoint_index += 1
if self.cyclical_path:
self.current_waypoint_index %= len(self.waypoint_list)
else:
if self.current_waypoint_index > len(self.waypoint_list):
self.current_waypoint_index -= 1
next_waypoint = np.array(
self.waypoint_list[self.current_waypoint_index])
command_msg = Command()
command_msg.x = next_waypoint[0]
command_msg.y = next_waypoint[1]
command_msg.F = next_waypoint[2]
if len(current_waypoint) <= 3:
next_point = self.waypoint_list[(self.current_waypoint_index +
1) % len(self.waypoint_list)]
delta = next_point - current_waypoint
current_waypoint.append(math.atan2(delta[1], delta[0]))
# altitude send directly
command_msg.F = next_waypoint[2]
# velocity commands, Proportional gain and saturation
command_msg.x = saturate(2.0 * error_b[0], 2.0, -2.0)
command_msg.y = saturate(2.0 * error_b[0], 2.0, -2.0)
command_msg.z = saturate(1.507 * (current_waypoint[3] - y), 1.507,
-1.507)
command_msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE
self.waypoint_pub_.publish(command_msg)
def send_waypoint_command(self):
if self.mode_flag == 'mavros':
waypoint_command_msg = PositionTarget()
waypoint_command_msg.header.stamp = rospy.get_rostime()
waypoint_command_msg.coordinate_frame = waypoint_command_msg.FRAME_LOCAL_NED
waypoint_command_msg.type_mask = self.ned_pos_mask
waypoint_command_msg.position.x = self.wp_E # E
waypoint_command_msg.position.y = self.wp_N # N
waypoint_command_msg.position.z = self.rendezvous_height # U
# This converts an NED heading command into an ENU heading command
yaw = -self.heading_command + np.radians(90.0)
while yaw > np.radians(180.0):
yaw = yaw - np.radians(360.0)
while yaw < np.radians(-180.0):
yaw = yaw + np.radians(360.0)
waypoint_command_msg.yaw = yaw
# Publish.
self.command_pub_mavros.publish(waypoint_command_msg)
else:
wp_command_msg = Command()
wp_command_msg.x = self.wp_N
wp_command_msg.y = self.wp_E
wp_command_msg.F = -self.rendezvous_height
wp_command_msg.z = self.heading_command
wp_command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
# Publish.
self.command_pub_roscopter.publish(wp_command_msg)
def __init__(self):
# get parameters
try:
self.waypoint_list = rospy.get_param('~waypoints')
except KeyError:
rospy.logfatal('waypoints not set')
rospy.signal_shutdown('Parameters not set')
# how close does the MAV need to get before going to the next waypoint?
self.threshold = rospy.get_param('~threshold', 5)
self.cyclical_path = rospy.get_param('~cycle', True)
self.prev_time = rospy.Time.now()
# set up Services
self.add_waypoint_service = rospy.Service('add_waypoint', AddWaypoint,
self.addWaypointCallback)
self.remove_waypoint_service = rospy.Service('remove_waypoint',
RemoveWaypoint,
self.addWaypointCallback)
self.set_waypoint_from_file_service = rospy.Service(
'set_waypoints_from_file', SetWaypointsFromFile,
self.addWaypointCallback)
# Set Up Publishers and Subscribers
self.xhat_sub_ = rospy.Subscriber('state',
Odometry,
self.odometryCallback,
queue_size=5)
self.waypoint_pub_ = rospy.Publisher('high_level_command',
Command,
queue_size=5,
latch=True)
self.current_waypoint_index = 0
command_msg = Command()
current_waypoint = np.array(self.waypoint_list[0])
command_msg.x = current_waypoint[0]
command_msg.y = current_waypoint[1]
command_msg.F = current_waypoint[2]
if len(current_waypoint) > 3:
command_msg.z = current_waypoint[3]
else:
next_point = self.waypoint_list[(self.current_waypoint_index + 1) %
len(self.waypoint_list)]
delta = next_point - current_waypoint
command_msg.z = math.atan2(delta[1], delta[0])
command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
command_msg.header = Header()
command_msg.header.stamp = rospy.Time.now()
self.waypoint_pub_.publish(command_msg)
while not rospy.is_shutdown():
# wait for new messages and call the callback when they arrive
rospy.spin()
def execute_landing(self):
if self.mode_flag == 'mavros':
# Update the status flag.
self.status_flag = 'LAND'
# Ramp down the motors.
self.ramp_down_motors(self.landing_thrust0)
# rospy.wait_for_service('/mavros/set_mode')
# try:
# isModeChanged = self.set_mode_srv(custom_mode='AUTO.LAND')
# if isModeChanged:
# self.land_mode_sent = True
# print "mode %s sent." % 'AUTO.LAND'
# self.status_flag = 'LAND'
# except rospy.ServiceException, e:
# print "service call set_mode failed: %s" % e
else:
command_msg = Command()
command_msg.x = 0.0
command_msg.y = 0.0
command_msg.F = 0.0
command_msg.z = 0.0
command_msg.mode = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE
self.command_pub_roscopter.publish(command_msg)
self.status_flag = 'LAND'
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)
def __init__(self):
# get parameters
try:
self.waypoint_list = rospy.get_param('~waypoints')
except KeyError:
rospy.logfatal('waypoints not set')
rospy.signal_shutdown('Parameters not set')
# how close does the MAV need to get before going to the next waypoint?
self.threshold = rospy.get_param('~threshold', 5)
self.cyclical_path = rospy.get_param('~cycle', True)
self.prev_time = rospy.Time.now()
# set up Services
self.add_waypoint_service = rospy.Service('add_waypoint', AddWaypoint, self.addWaypointCallback)
self.remove_waypoint_service = rospy.Service('remove_waypoint', RemoveWaypoint,
self.removeWaypointCallback)
self.set_waypoint_from_file_service = rospy.Service('set_waypoints_from_file', SetWaypointsFromFile,
self.addWaypointCallback)
# Set Up Publishers and Subscribers
self.xhat_sub_ = rospy.Subscriber('state', Odometry, self.odometryCallback, queue_size=5)
self.obstacle_sub_ = rospy.Subscriber('nearest_obstacle',Float32, self.obstacleCallback, queue_size=5)
self.waypoint_pub_ = rospy.Publisher('high_level_command', Command, queue_size=5, latch=True)
self.end_waypoint_pub_ = rospy.Publisher('last_waypoint', Int16 , queue_size = 5, latch = True)
self.check_path_pub_ = rospy.Publisher('check_path',Int16, queue_size = 5, latch = True)
# Initialize other variables
self.current_waypoint_index = 0
self.current_yaw = 0.0 #when making a new waypoint, after clearing old waypoints,
# use same yaw
self.planning_flag = 1 #this flag is published and used by path planner to decide
# how to plan next path. 1 = visual planning,
# 2 = no visualization
self.obstacle_angle = 0 #angle of current nearest obstacle detected
self.obstacle_offset = 0.4 #how far to move away from obstacle after it trips threshold
self.new_waypoint = rospy.ServiceProxy('/slammer/add_waypoint', AddWaypoint)
command_msg = Command()
current_waypoint = np.array(self.waypoint_list[0])
command_msg.x = current_waypoint[0]
command_msg.y = current_waypoint[1]
command_msg.F = current_waypoint[2]
if len(current_waypoint) > 3:
command_msg.z = current_waypoint[3]
else:
next_point = self.waypoint_list[(self.current_waypoint_index + 1) % len(self.waypoint_list)]
delta = next_point - current_waypoint
command_msg.z = np.atan2(delta[1], delta[0])
command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
self.waypoint_pub_.publish(command_msg)
while not rospy.is_shutdown():
# wait for new messages and call the callback when they arrive
rospy.spin()
def __init__(self):
# get parameters
try:
self.waypoint_list = rospy.get_param('~waypoints')
except KeyError:
rospy.logfatal('waypoints not set')
rospy.signal_shutdown('Parameters not set')
# how close does the MAV need to get before going to the next waypoint?
self.threshold = rospy.get_param('~threshold', 5)
self.cyclical_path = rospy.get_param('~cycle', True)
self.prev_time = rospy.Time.now()
# set up Services
self.add_waypoint_service = rospy.Service('add_waypoint', AddWaypoint, self.addWaypointCallback)
self.remove_waypoint_service = rospy.Service('remove_waypoint', RemoveWaypoint, self.addWaypointCallback)
self.set_waypoint_from_file_service = rospy.Service('set_waypoints_from_file', SetWaypointsFromFile, self.addWaypointCallback)
# Wait a second before we publish the first waypoint
while (rospy.Time.now() < rospy.Time(2.)):
pass
# Set Up Publishers and Subscribers
self.xhat_sub_ = rospy.Subscriber('state', Odometry, self.odometryCallback, queue_size=5)
self.waypoint_pub_ = rospy.Publisher('high_level_command', Command, queue_size=5, latch=True)
self.relPose_pub_ = rospy.Publisher('relative_pose', RelativePose, queue_size=5, latch=True)
#Create the initial relPose estimate message
relativePose_msg = RelativePose()
relativePose_msg.x = 0
relativePose_msg.y = 0
relativePose_msg.z = 0
relativePose_msg.F = 0
self.relPose_pub_.publish(relativePose_msg)
#Create the initial command message
self.current_waypoint_index = 0
command_msg = Command()
current_waypoint = np.array(self.waypoint_list[0])
command_msg.header.stamp = rospy.Time.now()
command_msg.x = current_waypoint[0]
command_msg.y = current_waypoint[1]
command_msg.F = current_waypoint[2]
if len(current_waypoint) > 3:
command_msg.z = current_waypoint[3]
else:
command_msg.z = 0.
command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
self.waypoint_pub_.publish(command_msg)
while not rospy.is_shutdown():
# wait for new messages and call the callback when they arrive
rospy.spin()
def odometryCallback(self, msg):
# Get error between waypoint and current state
if len(self.waypoint_list)==0:
current_waypoint = [np.inf,np.inf,np.inf]
else:
current_waypoint = np.array(self.waypoint_list[self.current_waypoint_index])
(r, p, y) = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
current_position = np.array([msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z])
self.pos = current_position
error = np.linalg.norm(current_position - current_waypoint[0:3])
if error < self.threshold:
# Get new waypoint index
self.current_waypoint_index += 1
if self.cyclical_path:
self.current_waypoint_index %= len(self.waypoint_list)
else:
# When at the last waypoint, publish the replan flag once
if self.current_waypoint_index >= len(self.waypoint_list):
self.current_waypoint_index -=1
if not self.last_wp_published:
obstacle_flag = Int16()
obstacle_flag.data = self.planning_flag
self.end_waypoint_pub_.publish(obstacle_flag)
self.planning_flag = 1
self.last_wp_published = True
else:
self.last_wp_published = False
#send flag to check if path is still feasible
check_path_flag = Int16()
check_path_flag.data = self.current_waypoint_index
self.check_path_pub_.publish(check_path_flag)
next_waypoint = np.array(self.waypoint_list[self.current_waypoint_index])
command_msg = Command()
command_msg.x = next_waypoint[0]
command_msg.y = next_waypoint[1]
command_msg.F = next_waypoint[2]
if len(current_waypoint) > 3:
command_msg.z = current_waypoint[3]
self.current_yaw = command_msg.z
else:
next_point = self.waypoint_list[(self.current_waypoint_index + 1) % len(self.waypoint_list)]
delta = next_point - current_waypoint
command_msg.z = np.atan2(delta[1], delta[0])
command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
self.waypoint_pub_.publish(command_msg)
def resetWaypoints(self):
command_msg = Command()
command_msg.x = self.pos[0]-self.obstacle_offset*np.cos(self.obstacle_angle)
command_msg.y = self.pos[1]-self.obstacle_offset*np.sin(self.obstacle_angle)
command_msg.F = self.pos[2]
command_msg.z = self.current_yaw
command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
self.waypoint_pub_.publish(command_msg)
rospy.wait_for_service('/slammer/add_waypoint')
try:
success = self.new_waypoint(x=self.pos[0],y=self.pos[1],
z=self.pos[2],yaw=self.current_yaw,
radius = 0.1, difficulty = 1.0)
if success:
pass
except rospy.ServiceException,e:
print "service call add_waypoint failed: %s" %e
def odometryCallback(self, msg):
# Get error between waypoint and current state
current_waypoint = np.array(self.waypoint_list[self.current_waypoint_index])
current_position = np.array([msg.pose.pose.position.x,
msg.pose.pose.position.y,
-msg.pose.pose.position.z])
# orientation in quaternion form
qw = msg.pose.pose.orientation.w
qx = msg.pose.pose.orientation.x
qy = msg.pose.pose.orientation.y
qz = msg.pose.pose.orientation.z
# yaw from quaternion
y = np.arctan2(2*(qw*qz + qx*qy), 1 - 2*(qy**2 + qz**2))
error = np.linalg.norm(current_position - current_waypoint[0:3])
#publish the relative pose estimate
relativePose_msg = RelativePose()
relativePose_msg.x = current_position[0]
relativePose_msg.y = current_position[1]
relativePose_msg.z = y
relativePose_msg.F = current_position[2]
self.relPose_pub_.publish(relativePose_msg)
if error < self.threshold:
# Get new waypoint index
self.current_waypoint_index += 1
if self.cyclical_path:
self.current_waypoint_index %= len(self.waypoint_list)
else:
if self.current_waypoint_index > len(self.waypoint_list):
self.current_waypoint_index -=1
next_waypoint = np.array(self.waypoint_list[self.current_waypoint_index])
command_msg = Command()
command_msg.header.stamp = rospy.Time.now()
command_msg.x = next_waypoint[0]
command_msg.y = next_waypoint[1]
command_msg.F = next_waypoint[2]
if len(next_waypoint) > 3:
command_msg.z = next_waypoint[3]
else:
command_msg.z = 0.
command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
self.waypoint_pub_.publish(command_msg)
def odometryCallback(self, msg):
# Get error between waypoint and current state
current_waypoint = np.array(
self.waypoint_list[self.current_waypoint_index])
(r, p, y) = tf.transformations.euler_from_quaternion([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
current_position = np.array([
msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z
])
error = np.linalg.norm(current_position - current_waypoint[0:3])
if error < self.threshold:
# Get new waypoint index
self.current_waypoint_index += 1
if self.cyclical_path:
self.current_waypoint_index %= len(self.waypoint_list)
else:
if self.current_waypoint_index > len(self.waypoint_list):
self.current_waypoint_index -= 1
next_waypoint = np.array(
self.waypoint_list[self.current_waypoint_index])
command_msg = Command()
command_msg.x = next_waypoint[0]
command_msg.y = next_waypoint[1]
command_msg.F = next_waypoint[2]
if len(current_waypoint) > 3:
command_msg.z = current_waypoint[3]
else:
next_point = self.waypoint_list[(self.current_waypoint_index +
1) % len(self.waypoint_list)]
delta = next_point - current_waypoint
command_msg.z = math.atan2(delta[1], delta[0])
command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
command_msg.header = Header()
command_msg.header.stamp = rospy.Time.now()
self.waypoint_pub_.publish(command_msg)
# 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()
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()
time_i = idx * sleep_time
rospy.wait_for_service('/mavros/set_mode')
try:
isModeChanged = self.set_mode_srv(custom_mode='AUTO.LAND')
if isModeChanged:
self.land_mode_sent = True
print "mode %s sent." % 'AUTO.LAND'
self.status_flag = 'LAND'
except rospy.ServiceException, e:
print "service call set_mode failed: %s" % e
else:
command_msg = Command()
command_msg.x = 0.0
command_msg.y = 0.0
command_msg.F = 0.0
command_msg.z = 0.0
command_msg.mode = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE
self.command_pub_roscopter.publish(command_msg)
self.status_flag = 'LAND'
def send_ibvs_command(self, flag):
if self.mode_flag == 'mavros':
if flag == 'inner':
ibvs_command_msg = PositionTarget()
ibvs_command_msg.header.stamp = rospy.get_rostime()