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 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):
self.mass = rospy.get_param('dynamics/mass')
self.g = rospy.get_param('dynamics/gravity',9.80665)
self.thrust_max = rospy.get_param('dynamics/max_F')
self.thrust_eq = (self.g*self.mass)/(self.thrust_max)
self.phi = 0.0
self.theta = 0.0
self.psi = 0.0
self.p = 0.0
self.q = 0.0
self.r = 0.0
self.pnddot_c = 0.0
self.peddot_c = 0.0
self.pdddot_c = 0.0
self.r_c = 0.0
self.prev_time = rospy.get_time()
self.t = 0
# Set Up Publishers and Subscribers
self.cmd_sub_ = rospy.Subscriber('u_command', Command, self.urawCallback, queue_size=5)
self.xhat_sub_ = rospy.Subscriber('state', Odometry, self.odometryCallback, queue_size=5)
self.command_pub_ = rospy.Publisher('v_command', Command, queue_size=5, latch=True)
self.v_command = Command()
self.control_mode = 0
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 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 __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 initialize():
# CommandPubSub.status_sub = rospy.Subscriber('status', Status, CommandPubSub.status_callback)
CommandPubSub.command = Command()
CommandPubSub.rf_com_pub = rospy.Publisher('command',
Command,
queue_size=1)
CommandPubSub.rc_com_pub = rospy.Publisher('high_level_command',
Command,
queue_size=1)
CommandPubSub.timer = rospy.Timer(rospy.Duration(1.0 / 25.0),
CommandPubSub.update)
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 __init__(self):
# Parameters for feed forward trajectory
self.alpha = 2.5 #amplitude of pn wave
self.beta = 1.25 #amplitude of pe wave
self.eta = -2.0 #pd
self.omega_f = 1.5 #period of cycle final
self.omega_s = 0.05 #period of cycle start
# self.alpha = 3.0
# self.beta = 3.0
# self.eta = -1.0
# self.omega = 2#2.5
self.takeoff_time = 10#30.0 #how long before the trajectory begins(s)
self.g = rospy.get_param('dynamics/gravity',9.80665)
self.mass = rospy.get_param('dynamics/mass')
self.theta_prev = 0.0
self.theta_dot = 0.0
self.prev_time = rospy.get_time()
self.start_time = self.prev_time
# Set Up Publishers and Subscribers
self.xhat_sub_ = rospy.Subscriber('state', Odometry, self.odometryCallback, queue_size=5)
self.traj_pub_ = rospy.Publisher('trajectory', Command, queue_size=5, latch=True)
self.uff_pub_ = rospy.Publisher('u_ff', Command, queue_size=5, latch=True)
self.xdes_pub_ = rospy.Publisher('xdes_vel', Command, queue_size=5, latch=True)
self.cmd = Command()
self.xdes = Command()
self.u_ff = Command()
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 __init__(self):
self.mass = rospy.get_param('dynamics/mass')
self.g = rospy.get_param('dynamics/gravity', 9.80665)
self.thrust_max = rospy.get_param('dynamics/max_F')
self.thrust_eq = (self.g * self.mass) / (self.thrust_max)
self.phi = 0.0
self.theta = 0.0
self.p = 0.0
self.q = 0.0
self.r = 0.0
self.phi_c = 0.0
self.theta_c = 0.0
self.F_c = 0.0
self.r_c = 0.0
self.pd = 0
self.pd_c = 0
self.w = 0
self.control_mode = 0
self.rot_true = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
self.prev_time = rospy.get_time()
# Set Up Publishers and Subscribers
self.cmd_sub_ = rospy.Subscriber('v_command',
Command,
self.vcmdCallback,
queue_size=5)
self.hlc_sub_ = rospy.Subscriber('trajectory',
Command,
self.trajCallback,
queue_size=5)
self.xhat_sub_ = rospy.Subscriber('state',
Odometry,
self.odometryCallback,
queue_size=5)
self.command_pub_ = rospy.Publisher('diff_flat_cmd',
Command,
queue_size=5,
latch=True)
self.df_cmd = Command()
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 __init__(self):
self.traj_follower = self.setup_trajectory_follower()
self.att_controller = self.setup_attitude_stabilizer()
self.state = state.State()
self.traj = trajectory.Trajectory()
self.armed = False
self.t0 = 0.0
self.t_prev = 0.0
# Initialize ROS
rospy.Subscriber("truth/NED", Odometry, self.stateCallback,
queue_size=1, tcp_nodelay=True)
rospy.Subscriber("status", Status, self.statusCallback, queue_size=1)
self.cmd_pub = rospy.Publisher("command", Command, queue_size=1)
rospy.Subscriber("trajectory", Float32MultiArray,
self.trajectoryCallback)
self.cmd_msg = Command()
self.cmd_msg.mode = Command.MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE
def __init__(self):
# Init Params here
self.x_vel = 2.0
self.y_vel = 2.0
self.z_vel = 0.0 # yaw rate
self.alt_command = 10.0
self.delay_time = 5
# Init Publishers
self.high_lvl_commands_pub = rospy.Publisher('/leo/velocity_command', Command, queue_size=10)
# Init the command
self.relative_cmd = Command()
# begin sending commands
self.sendCommands()
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)
def __init__(self):
self.pub = rospy.Publisher('/command',Command, queue_size=10, latch=True)
self.command_msg = Command()
self.command_msg.mode = self.command_msg.MODE_ROLL_PITCH_YAWRATE_THROTTLE
self.count = True # used for initiating time
self.past_start = True # used for initiating time for starting trajectory
rospy.Subscriber('/odom',Odometry,self.callback)
# with open('/home/matiss/demo_ws/src/demo/scripts/demo.yaml','r') as f:
# param = yaml.safe_load(f)
param = rospy.get_param('~')
self.mass = param['dynamics']['mass']
self.g = param['dynamics']['g']
self.controller = controller(param) # initiate controller
# calculate the equilibrium force
self.force_adjust = param['dynamics']['mass']*param['dynamics']['g']/param['controller']['equilibrium_throttle']
def __init__(self):
# Load ROS params.
self.axis = rospy.get_param('~xy_axis', 'X')
self.magnitude = rospy.get_param('~magnitude', 0.5)
self.feed_forward = rospy.get_param('~feed_forward', 0.0)
# Initialize other class variables.
self.vel_x = 0.0
self.vel_y = 0.0
self.phase = 1
self.t_start = rospy.get_time()
# Messages
self.command_msg = Command()
self.ibvs_active_msg = Bool()
self.ibvs_active_msg.data = True
# Initialize publishes
self.command_pub = rospy.Publisher('/quadcopter/high_level_command',
Command,
queue_size=5,
latch=True)
self.ibvs_active_pub = rospy.Publisher('/quadcopter/ibvs_active',
Bool,
queue_size=1)
self.count_rate = 0.25 # seconds
self.count_timer = rospy.Timer(rospy.Duration(1.0 / self.count_rate),
self.generate_velocities)
# Initialize timers.
self.update_rate = 20.0
self.update_timer = rospy.Timer(rospy.Duration(1.0 / self.update_rate),
self.send_commands)
def __init__(self):
self.velocity_controller = self.setup_velocity_controller()
self.att_controller = self.setup_attitude_stabilizer()
self.state = state.State()
self.armed = False
self.t0 = 0.0
self.t_prev = 0.0
# self.vel_des = np.array([0.0, -5.0, 0])
self.vel_des = np.array([5.0, 0.0, 0.0])
self.vel_cmd = np.array([0.0, 0.0, 0.0])
self.vel_opt = np.array([0.0, 0.0, 0.0])
# Initialize ROS
rospy.Subscriber("truth/NED",
Odometry,
self.stateCallback,
queue_size=1,
tcp_nodelay=True)
rospy.Subscriber("status", Status, self.statusCallback, queue_size=1)
self.cmd_pub = rospy.Publisher("command", Command, queue_size=1)
rospy.Subscriber("optical_flow/velocity_cmd", Vector3,
self.velocityCallback)
self.cmd_msg = Command()
self.cmd_msg.mode = Command.MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE
def __init__(self):
self.pn = 0.0
self.pe = 0.0
self.pd = 0.0
self.u = 0.0
self.v = 0.0
self.w = 0.0
self.phi = 0.0
self.theta = 0.0
self.psi = 0.0
self.p = 0.0
self.q = 0.0
self.r = 0.0
self.pndot = 0.0
self.pedot = 0.0
self.pddot = 0.0
self.pn_d = 0.0
self.pe_d = 0.0
self.pd_d = 0.0
self.psi_d = 0.0
self.pndot_d = 0.0
self.pedot_d = 0.0
self.pddot_d = 0.0
self.pnddot_c = 0.0
self.peddot_c = 0.0
self.pdddot_c = 0.0
self.r_c = 0.0
self.control_mode = 0
self.max_roll = rospy.get_param('controller/max_roll', 1.0)
self.max_yaw_rate = rospy.get_param('controller/~max_yaw_rate', 0.785)
self.max_throttle = rospy.get_param('controller/max_throttle', 0.85)
self.gravity = rospy.get_param('dynamics/gravity', 9.80665)
self.mass = rospy.get_param('dynamics/mass', 2.865)
# self.xlim = np.array([0.1,0.28,0.1,0.2,0.5,0.2,0.5])
self.xlim = np.array([0.1, 0.28, 0.1, 0.2, 1.0, 0.2, 0.5])
self.K = self.findLQRGain()
self.prev_time = rospy.get_time()
# Set Up Publishers and Subscribers
self.xhat_sub_ = rospy.Subscriber('state',
Odometry,
self.odometryCallback,
queue_size=5)
self.cmd_sub_ = rospy.Subscriber('trajectory',
Command,
self.trajCallback,
queue_size=5)
self.uff_sub_ = rospy.Subscriber('u_ff',
Command,
self.uffCallback,
queue_size=5)
self.xdes_sub_ = rospy.Subscriber('xdes_vel',
Command,
self.xdesCallback,
queue_size=5)
self.is_flying_sub_ = rospy.Subscriber('is_flying',
Bool,
self.isFlyingCallback,
queue_size=5)
self.command_pub_ = rospy.Publisher('u_command',
Command,
queue_size=5,
latch=True)
self.u_raw = Command()
self.flying = False
# 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()
def __init__(self):
# get parameters
try:
self.waypoint_list = rospy.get_param('~waypoints')
except KeyError:
rospy.logfatal('[waypoint_manager] waypoints not set')
rospy.signal_shutdown('[waypoint_manager] Parameters not set')
self.current_waypoint_index = 0
self.y = 0
self.hold = False
self.no_command = False
if len(self.waypoint_list) == 0:
self.no_command = True
self.halt_waypoint = [0, 0, 0, 0]
# how close does the MAV need to get before going to the next waypoint?
self.pos_threshold = rospy.get_param('~threshold', 5)
self.heading_threshold = rospy.get_param('~heading_threshold',
0.035) # radians
self.cyclical_path = rospy.get_param('~cycle', True)
self.print_wp_reached = rospy.get_param('~print_wp_reached', True)
# 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_waypoints_from_file_service = rospy.Service(
'set_waypoints_from_file', SetWaypointsFromFile,
self.setWaypointsFromFileCallback)
self.list_waypoints_service = rospy.Service('list_waypoints',
ListWaypoints,
self.listWaypointsCallback)
self.clear_waypoints_service = rospy.Service(
'clear_waypoints', ClearWaypoints, self.clearWaypointsCallback)
self.hold_service = rospy.Service('hold', Hold, self.holdCallback)
self.release_service = rospy.Service('release', Release,
self.releaseCallback)
# Set Up Publishers and Subscribers
self.xhat_sub_ = rospy.Subscriber('state',
Odometry,
self.odometryCallback,
queue_size=5)
self.waypoint_cmd_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)
# Wait a second before we publish the first waypoint
rospy.sleep(2)
# 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.cmd_msg = Command()
current_waypoint = np.array(self.waypoint_list[0])
self.publish_command(current_waypoint)
while not rospy.is_shutdown():
# wait for new messages and call the callback when they arrive
rospy.spin()
def __init__(self):
# set up timing variables
self.hz = 100.
self.rate = rospy.Rate(self.hz)
self.Ts = 1/self.hz
self.t = []
self.Va_c=[]
self.theta_c_max = 30*np.pi/180.
self.phi_max = 45*np.pi/180.
self.altitude_takeoff_zone = 10
self.altitude_hold_zone = 5
self.climb_out_throttle = 1.0
self.altitude_state = 0
self.integrator_1 = 0
self.integrator_2 = 0
self.integrator_3 = 0
self.integrator_4 = 0
self.integrator_5 = 0
self.integrator_6 = 0
self.error_1 = 0
self.error_2 = 0
self.error_3 = 0
self.error_4 = 0
self.error_5 = 0
self.error_6 = 0
self.ap_differentiator_ = 0
self.at_differentiator_ = 0
self.hdot = 0
self.hdot_d = 0
self.h_d = 0
self.tau = 5
self.commands = Command()
# load param file
self.P = yaml.load(open('/home/nmd89/git/nathan/flight_dynamics/final_project/rosplane_ws/src/rosplane/rosplane/param/aerosonde.yaml'))
# modified param values so they match the simulation
self.P['AS_PITCH_KP'] = 0.0
self.P['BETA_KP'] = 0.0
self.P['BETA_KI'] = 0.0
self.P['COURSE_KI'] = 0.0
self.P['PITCH_KP'] = 1.0
self.P['PITCH_KD'] = -0.17
# trim values for control surfaces and throttle
self.delta_a = self.P['TRIM_A']
self.delta_e = self.P['TRIM_E']
self.delta_r = self.P['TRIM_R']
self.delta_t = self.P['TRIM_T']
# subscribe to the MAV states and controller_commands
rospy.Subscriber('/fixedwing/truth', State, self.get_states)
rospy.Subscriber('/fixedwing/controller_commands', Controller_Commands, self.get_commands)
# publish the commanded surface deflections
self.pub = rospy.Publisher('/fixedwing/command', Command, queue_size=1)
check=1
while not self.t:
if check:
print 'waiting for states'
check=0
print 'states received'
check=1
while not self.Va_c:
if check:
print 'waiting for commands'
check=0
print 'commands received'
def __init__(self):
# load ROS params
# PID gains
u_P = rospy.get_param('~u_P', 0.2)
u_I = rospy.get_param('~u_I', 0.0)
u_D = rospy.get_param('~u_D', 0.01)
v_P = rospy.get_param('~v_P', 0.2)
v_I = rospy.get_param('~v_I', 0.0)
v_D = rospy.get_param('~v_D', 0.01)
w_P = rospy.get_param('~w_P', 3.0)
w_I = rospy.get_param('~w_I', 0.05)
w_D = rospy.get_param('~w_D', 0.5)
x_P = rospy.get_param('~x_P', 0.5)
x_I = rospy.get_param('~x_I', 0.01)
x_D = rospy.get_param('~x_D', 0.1)
y_P = rospy.get_param('~y_P', 0.5)
y_I = rospy.get_param('~y_I', 0.01)
y_D = rospy.get_param('~y_D', 0.1)
z_P = rospy.get_param('~z_P', 1.0)
z_I = rospy.get_param('~z_I', 0.1)
z_D = rospy.get_param('~z_D', 0.4)
psi_P = rospy.get_param('~psi_P', 0.5)
psi_I = rospy.get_param('~psi_I', 0.0)
psi_D = rospy.get_param('~psi_D', 0.0)
tau = rospy.get_param('~tau', 0.04)
# quadcopter params
self.max_thrust = rospy.get_param('dynamics/max_F', 60.0)
self.mass = rospy.get_param('dynamics/mass', 3.0)
self.thrust_eq = (9.80665 * self.mass) / self.max_thrust
self.drag_constant = rospy.get_param('dynamics/linear_mu', 0.1)
# initialize state variables
self.pn = 0.0
self.pe = 0.0
self.pd = 0.0
self.phi = 0.0
self.theta = 0.0
self.psi = 0.0
self.u = 0.0
self.v = 0.0
self.w = 0.0
self.p = 0.0
self.q = 0.0
self.r = 0.0
self.ax = 0.0
self.ay = 0.0
self.az = 0.0
self.throttle = 0.0
# initialize command variables
self.xc_pn = 0.0
self.xc_pe = 0.0
self.xc_pd = 0.0
self.xc_phi = 0.0
self.xc_theta = 0.0
self.xc_psi = 0.0
self.xc_u = 0.0
self.xc_v = 0.0
self.xc_pd = 0.0
self.xc_r = 0.0
self.xc_ax = 0.0
self.xc_ay = 0.0
self.xc_az = 0.0
self.xc_r = 0.0
self.xc_throttle = 0.0
# initialize saturation values
self.max_roll = rospy.get_param('~max_roll', 0.15)
self.max_pitch = rospy.get_param('~max_pitch', 0.15)
self.max_yaw_rate = rospy.get_param('~max_yaw_rate', np.radians(45.0))
self.max_throttle = rospy.get_param('~max_throttle', 1.0)
self.max_u = rospy.get_param('~max_u', 1.0)
self.max_v = rospy.get_param('~max_v', 1.0)
self.max_w = rospy.get_param('~max_x', 1.0)
# initialize PID controllers
self.PID_u = PID(u_P, u_I, u_D, None, None, tau)
self.PID_v = PID(v_P, v_I, v_D, None, None, tau)
self.PID_w = PID(w_P, w_I, w_D, None, None, tau)
self.PID_x = PID(x_P, x_I, x_D, None, None, tau)
self.PID_y = PID(y_P, y_I, y_D, None, None, tau)
self.PID_z = PID(z_P, z_I, z_D, None, None, tau)
self.PID_psi = PID(psi_P, psi_I, psi_D, None, None, tau)
# initialize other class variables
self.prev_time = 0.0
self.is_flying = False
self.control_mode = 4 # MODE_XPOS_YPOS_YAW_ALTITUDE
self.ibvs_active = False
self.command = Command()
# dynamic reconfigure
self.server = Server(ControllerConfig, self.reconfigure_callback)
# initialize subscribers
self.state_sub = rospy.Subscriber('estimate', Odometry,
self.state_callback)
self.is_flying_sub = rospy.Subscriber('is_flying', Bool,
self.is_flying_callback)
self.cmd_sub = rospy.Subscriber('high_level_command', Command,
self.cmd_callback)
self.ibvs_active_sub = rospy.Subscriber('ibvs_active', Bool,
self.ibvs_active_callback)
# initialize publishers
self.command_pub = rospy.Publisher('command', Command, queue_size=10)
# initialize timer
self.update_rate = 200.0
self.update_timer = rospy.Timer(rospy.Duration(1.0 / self.update_rate),
self.send_command)
rospy.init_node('benchmark_commander')
rospy.loginfo('BENCHMARK COMMANDED ACTIVE')
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
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()
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()
ibvs_command_msg.coordinate_frame = ibvs_command_msg.FRAME_BODY_NED
ibvs_command_msg.type_mask = self.velocity_mask
ibvs_command_msg.velocity.x = self.saturate(
self.ibvs_x_inner, self.u_max_inner,
-self.u_max_inner) + self.target_VE * np.cos(
self.psi) - self.target_VN * np.sin(self.psi)
ibvs_command_msg.velocity.y = self.saturate(
self.ibvs_y_inner, self.v_max_inner,
-self.v_max_inner) + self.target_VN * np.cos(
self.psi) + self.target_VE * np.sin(self.psi)
ibvs_command_msg.velocity.z = self.saturate(
self.ibvs_F_inner, self.w_max_inner, -self.w_max_inner)
ibvs_command_msg.yaw_rate = self.ibvs_z_inner
# Publish.
self.command_pub_mavros.publish(ibvs_command_msg)
elif flag == 'outer':
ibvs_command_msg = PositionTarget()
ibvs_command_msg.header.stamp = rospy.get_rostime()
ibvs_command_msg.coordinate_frame = ibvs_command_msg.FRAME_BODY_NED
ibvs_command_msg.type_mask = self.velocity_mask
ibvs_command_msg.velocity.x = self.saturate(
self.ibvs_x, self.u_max,
-self.u_max) + self.target_VE * np.cos(
self.psi) - self.target_VN * np.sin(self.psi)
ibvs_command_msg.velocity.y = self.saturate(
self.ibvs_y, self.v_max,
-self.v_max) + self.target_VN * np.cos(
self.psi) + self.target_VE * np.sin(self.psi)
ibvs_command_msg.velocity.z = self.saturate(
self.ibvs_F, self.w_max, -self.w_max)
ibvs_command_msg.yaw_rate = self.ibvs_z
# Publish.
self.command_pub_mavros.publish(ibvs_command_msg)
else:
pass
else:
if flag == 'inner':
ibvs_command_msg = Command()
ibvs_command_msg.x = self.saturate(
self.ibvs_x_inner, self.u_max_inner,
-self.u_max_inner) + self.target_VN * np.cos(
self.psi) + self.target_VE * np.sin(self.psi)
ibvs_command_msg.y = self.saturate(
self.ibvs_y_inner, self.v_max_inner,
-self.v_max_inner) + self.target_VE * np.cos(
self.psi) - self.target_VN * np.sin(self.psi)
ibvs_command_msg.F = self.saturate(self.ibvs_F_inner,
self.w_max_inner,
-self.w_max_inner)
ibvs_command_msg.z = self.ibvs_z_inner
ibvs_command_msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE
self.command_pub_roscopter.publish(ibvs_command_msg)
elif flag == 'outer':
ibvs_command_msg = Command()
ibvs_command_msg.x = self.saturate(
self.ibvs_x, self.u_max,
-self.u_max) + self.target_VN * np.cos(
self.psi) + self.target_VE * np.sin(self.psi)
ibvs_command_msg.y = self.saturate(
self.ibvs_y, self.v_max,
-self.v_max) + self.target_VE * np.cos(
self.psi) - self.target_VN * np.sin(self.psi)
ibvs_command_msg.F = self.saturate(self.ibvs_F, self.w_max,
-self.w_max)
ibvs_command_msg.z = self.ibvs_z
ibvs_command_msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE
self.command_pub_roscopter.publish(ibvs_command_msg)
else:
pass
def __init__(self):
self.length = rospy.get_param('~tether_takeoff_length', 0.2)
self.takeoff_alt = rospy.get_param('~takeoff_altitude', 1.0)
self.comm_alt_buf = rospy.get_param('~commanded_altitude_buffer', 0.1)
self.tension_threshold = rospy.get_param('~tension_threshold', 2.0)
self.tensioner_pitch = rospy.get_param('~tensioner_pitch',
0.1) # make a max value?
self.kp_lateral = rospy.get_param('~kp_lateral', 0.5)
self.v_max = rospy.get_param('~v_max', 1.2)
self.commanded_rel_pos_buffer = rospy.get_param(
'~commanded_rel_pos_buffer', 0.1)
self.rel_yaw_home = pi * rospy.get_param('~rel_yaw_home', 0.0) / 180.0
self.rel_yaw_buff = pi * rospy.get_param('~commanded_rel_yaw_buffer',
10.0) / 180.0
self.sweep_angle = pi * rospy.get_param('~sweep_angle', 45.0) / 180.0
self.max_rel_yaw = pi * rospy.get_param('~max_rel_yaw', 90) / 180.0
self.sweep_rel_yaw_lower = max(-self.max_rel_yaw, self.rel_yaw_home -
self.sweep_angle) + self.rel_yaw_buff
self.sweep_rel_yaw_upper = min(self.max_rel_yaw, self.rel_yaw_home +
self.sweep_angle) - self.rel_yaw_buff
self.mcS = rospy.Service('mission_command', MissionCommand,
self.MCCallback)
self.tlS = rospy.Service('tether_length', TetherLength,
self.TLCallback)
self.tltS = rospy.ServiceProxy('tether_length_tether', TetherLength)
self.uaS = rospy.Service('uav_altitude', UavAltitude, self.UACallback)
self.abs_pose = None
self.abs_twist = None
self.rel_pose = None
self.rel_yaw = 0.0
self.rel_twist = None
self.Fext = None
self.shipVel = rospy.get_param('~ship_vel_guess', 1.0)
self.command = Command()
self.command.mode = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE
self.command.ignore = 0
self.command.x = 0.0
self.command.y = 0.0
self.command.z = 0.0
self.command.F = 0.0
self.abs_ekf_sub = rospy.Subscriber("odometry", Odometry,
self.abs_ekf_callback)
self.rel_ekf_sub = rospy.Subscriber("rel_odometry", Odometry,
self.rel_ekf_callback)
self.Fext_sub = rospy.Subscriber("Fext", WrenchStamped,
self.fext_callback)
self.ship_v_sub = rospy.Subscriber("base/navvelned", NavVELNED,
self.ship_v_callback)
self.hlc_com_pub = rospy.Publisher("high_level_command",
Command,
queue_size=1)
self.locked = False
self.checkRate = rospy.Rate(2.0)
self.flying = False
self.inTension = False
self.tensionThread = threading.Thread(target=self.establishTension)
self.command_timer = rospy.Timer(rospy.Duration(1.0 / 20.0),
self.update)
