def __init__(self):
"""ROS interface for controlling the Parrot ARDrone in the Vicon Lab."""
self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32)
#self.random_test = rospy.Publisher('/random_stuff', Twist, queue_size = 32)
self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
TransformStamped, self.update_vicon)
#self.start_value = rospy.Subscriber('/start_execution', String, self.run_stuff)
self.desired_position_data = rospy.Subscriber(
'/desired_positions', PoseStamped, self.update_desired_position)
#self.pub_check = rospy.Publisher('/check_mate', String, queue_size = 100)
self.pub_reached = rospy.Publisher('/check_reach',
String,
queue_size=100)
self.pub_counter = 0
self.translation_x_old = 0.0
self.translation_y_old = 0.0
self.translation_z_old = 0.0
self.z_velocity_old = 0.0
self.translation_x_desired = 0.0
self.translation_y_desired = 0.0
self.translation_z_desired = 0.5
self.translation_x = 0.0
self.translation_y = 0.0
self.translation_z = 0.0
self.rotation_x = 0.0
self.rotation_y = 0.0
self.rotation_z = 0.0
self.rotation_w = 1.0
self.rotation_x_desired = 0.0
self.rotation_y_desired = 0.0
self.rotation_z_desired = 0.0
self.rotation_w_desired = 1.0
self.time_interval = 0
self.current_time = rospy.get_time()
self.old_time = rospy.get_time()
self.postcom = PositionController()
# Run the onboard controller at 100 Hz
self.onboard_loop_frequency = 100.
# Run this ROS node at the onboard loop frequency
self.nutjobcase = rospy.Timer(
rospy.Duration(1. / self.onboard_loop_frequency),
self.process_commands)
self.postcom = PositionController()
"""
def __init__(self):
self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32)
self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
TransformStamped, self.update_vicon)
self.desired_position_data = rospy.Subscriber(
'/desired_positions', PoseStamped, self.update_desired_position)
self.start_stop_command = rospy.Subscriber('/start_stop_toggle',
String, self.update_state)
self.pub_errors = rospy.Publisher('/errors',
PoseStamped,
queue_size=100)
#self.pub_check = rospy.Publisher('/check_mate', String, queue_size = 100)
self.run_state = False
self.translation_x_old = 0.0
self.translation_y_old = 0.0
self.translation_z_old = 0.0
self.z_velocity_old = 0.0
self.translation_x_desired = 0.0
self.translation_y_desired = 0.0
self.translation_z_desired = 0.0
self.translation_x = 0.0
self.translation_y = 0.0
self.translation_z = 0.0
self.rotation_x = 0.0
self.rotation_y = 0.0
self.rotation_z = 0.0
self.rotation_w = 1.0
self.rotation_x_desired = 0.0
self.rotation_y_desired = 0.0
self.rotation_z_desired = 0.0
self.rotation_w_desired = 1.0
self.time_interval = 0
self.current_time = rospy.get_time()
self.old_time = rospy.get_time()
self.postcom = PositionController()
# Run the publish commands at 100 Hz
self.onboard_loop_frequency = 100.
# Run this ROS node at the onboard loop frequency
self.nutjobcase = rospy.Timer(
rospy.Duration(1. / self.onboard_loop_frequency), self.run_process)
self.postcom = PositionController()
def __init__(self):
# Publishers
self.pub_vel_cmd = rospy.Publisher('/cmd_vel_RHC',
Twist,
queue_size=32)
self.pub_flat_state = rospy.Publisher('/aer1217/flat_state',
FlatState,
queue_size=32)
#Subscriber
self.sub_vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
TransformStamped,
self.update_quadrotor_state)
self.sub_flat_in = rospy.Subscriber('/aer1217/flat_in', FlatInput,
self.update_flat_in)
self.sub_flat_state_op = rospy.Subscriber('/aer1217/flat_state_op',
FlatState,
self.update_flat_state_op)
self.sub_desired_pos = rospy.Subscriber('/aer1217/desired_position',
TransformStamped,
self.update_desired_pos)
#initialize variables
self.actual_pos = TransformStamped()
self.actual_pos_prev = TransformStamped()
self.actual_vel = np.array([0, 0, 0, 0])
self.actual_vel_prev = np.array([0, 0, 0, 0])
self.actual_vel_prev2 = np.array([0, 0, 0, 0])
self.actual_vel_prev3 = np.array([0, 0, 0, 0])
self.flat_state_msg = FlatState()
self.desired_pos = TransformStamped()
self.desired_pos.transform.translation.z = 2 #initialize z to 2
self.desired_pos_prev = TransformStamped()
self.desired_vel = np.array([0, 0, 2, 0])
self.flat_state = list(np.zeros(9))
self.flat_input = list(np.zeros(4))
self.flat_state_op = list(np.zeros(9))
#initialize position controller
self.pos_controller = PositionController()
self.flat_input = np.array([0, 0, 0, 0])
#publish vel commands
self.dt = 1 / 300.0
self.freq = 300
#run ros loop
rate = rospy.Rate(self.freq)
while not rospy.is_shutdown():
self.send_vel_cmd()
rate.sleep()
def __init__(self):
"""Initialize the ROSControllerNode class."""
# Publishers
"""
self.pub_cmd_vel = rospy.Publisher('cmd_vel',
Twist,
queue_size = 300)
"""
self.pub_cmd_vel = rospy.Publisher('cmd_vel_RHC',
Twist,
queue_size=300)
self.pubTakePic = rospy.Publisher('/ardrone/take_pic',
String,
queue_size=10)
# Subscribers
self.model_name = 'ARDroneCarre'
self.sub_vicon_data = rospy.Subscriber(
'/vicon/{0}/{0}'.format(self.model_name), TransformStamped,
self.update_vicon_data)
self.sub_des_pos = rospy.Subscriber('des_pos', String,
self.update_desired_position)
# Initialize messages for publishing
self.cmd_vel_msg = Twist()
# Run the onboard controller at 200 Hz
self.onboard_loop_frequency = 200.
# Calling the position controller to pass the data
self.pos_class = PositionController()
# Run this ROS node at the onboard loop frequency
self.run_pub_cmd_vel = rospy.Timer(
rospy.Duration(1. / self.onboard_loop_frequency),
self.update_roll_pitch_yaw)
self.current_point = 1
self.nonunix_time = 0
self.dt = 0
self.num_points_old = 3
self.pic_number = 1
# Keep time for differentiation and integration within the controller
self.old_time = rospy.get_time()
self.start_time = rospy.get_time()
def __init__(self):
# Publishers: rotor velocities
self.pub_rotor_vel = rospy.Publisher('/crazyflie2/command/motor_speed',
Actuators,
queue_size=1)
# Subscribers: desired posn, current posn
#self._goal_msg = MultiDOFJointTrajectory()
#self.sub_goal = rospy.Subscriber('crazyflie2/command/trajectory', MultiDOFJointTrajectory, self._goal_callback)
self._currpos_msg = Odometry()
self.sub_currpos = rospy.Subscriber('/crazyflie2/odometry', Odometry,
self._currpos_callback)
# end of publisher/subscriber
# initialize other parameters
self.data_list = [[
'Time', 'Desired_x', 'Desired_y', 'Desired_z', 'Actual_x',
'Actual_y', 'Actual_z', 'Desired_yaw', 'Desired_roll',
'Desired_pitch', 'Desired_yaw_r', 'Desired_climb_r', 'Actual_roll',
'Actual_pitch', 'Actual_yaw_r', 'Actual_climb_r'
]]
self.start = [1.0, 1.0]
self.wp_id = 1
self.previous_time = 0.0
self.change_time = -100.0
self.controller = PositionController()
self.init_time = rospy.get_time()
# initialize position and roll, pitch, yaw
self._x = 0
self._y = 0
self._z = 0
self.roll = 0
self.pitch = 0
self.yaw = 0
self._z_old = 0.0
self._z_oold = 0.0
self.yaw_old = 0.0
def __init__(self):
# Publishers
self.pub_vel_cmd = rospy.Publisher('/cmd_vel_RHC',
Twist,
queue_size=32)
self.pub_error = rospy.Publisher('/aer1217/position_error',
Twist,
queue_size=32)
#Subscriber
self.sub_vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
TransformStamped,
self.update_quadrotor_state)
self.sub_desired_pos = rospy.Subscriber('/aer1217/desired_position',
TransformStamped,
self.update_desired_pos)
#initialize variables
self.actual_pos = TransformStamped()
self.actual_pos_prev = TransformStamped()
self.actual_vel = np.array([0, 0, 0, 0])
self.desired_pos = TransformStamped()
self.desired_pos.transform.translation.z = 2 #initialize z to 2
self.desired_pos_prev = TransformStamped()
self.desired_vel = np.array([0, 0, 2, 0])
#Define Published msgs
self.pos_error = Twist()
self.vel_cmd_msg = Twist()
#initialize position controller
self.pos_controller = PositionController()
#publish vel commands
self.dt = 1 / 100.0
#press_key_wait = rospy.wait_for_message('/aer1217/move_type', Int16)
self.pub_prop_vel = rospy.Timer(rospy.Duration(self.dt), \
self.send_vel_cmd)
def __init__(self):
"""Initialize the ROSControllerNode class."""
self.model_name = 'ARDroneCarre'
# Publishers
self.pub_cmd_vel = rospy.Publisher('cmd_vel_RHC',
Twist,
queue_size=300)
# Subscribers
self.sub_vicon_data = rospy.Subscriber(
'/vicon/{0}/{0}'.format(self.model_name), TransformStamped,
self.process_vicon_data)
self.sub_des_pos = rospy.Subscriber('des_pos', String,
self.process_desired_position)
# Initialize messages for publishing
self.cmd_vel = Twist()
# Run the onboard controller at 200 Hz
self.onboard_loop_frequency = 200.
# Calling the position controller to pass the data
self.pos_class = PositionController()
# Run this ROS node at the onboard loop frequency
self.run_pub_cmd_vel = rospy.Timer(
rospy.Duration(1. / self.onboard_loop_frequency),
self.process_commands)
# Initialize time
self.nonunix_time = 0
self.old_time = rospy.get_time()
self.start_time = rospy.get_time()
self.dt = 0
# Initialize points
self.current_point = 1
self.num_points_old = 0
def __init__(self):
self.position_controller = PositionController()
self.sub_desired_state = rospy.Subscriber(
'/aer1217_ardrone/desired_state', DesiredStateMsg,
self.update_quadrotor_desired_state)
#######################################################################
# input is the estimated state x,y,z,roll,pitch,yaw from VICON
self.sub_estimated_state_vicon = rospy.Subscriber(
'/vicon/ARDroneCarre/ARDroneCarre', TransformStamped,
self.update_quadrotor_estimated_state_from_vicon)
#######################################################################
# output roll,pitch,yaw_rate,climb_rate commands
self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=30)
self.pub_cmd_vel_RHC = rospy.Publisher('cmd_vel_RHC',
Twist,
queue_size=30)
#######################################################################
self.onboard_loop_frequency = 200.
self.pub_cmd_vel_timer = rospy.Timer(
rospy.Duration(1. / self.onboard_loop_frequency),
self.send_cmd_vel)
def process_commands(self):
self.rotation = np.array([
self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w
])
self.rotation_desired = np.array([
self.rotation_x_desired, self.rotation_y_desired,
self.rotation_z_desired, self.rotation_w_desired
])
current_time = rospy.get_time()
self.time_interval = current_time - self.old_time
self.old_time = current_time
postcom = PositionController(
self.translation_x_old, self.translation_y_old,
self.translation_z_old, self.translation_x, self.translation_y,
self.translation_z, self.rotation, self.translation_x_desired,
self.translation_y_desired, self.translation_z_desired,
self.rotation_desired, self.z_velocity_old, self.time_interval)
returnvalue = postcom.member()
"""
roll = returnvalue[0]
pitch = returnvalue[1]
yaw = returnvalue[2]
z_dot = returnvalue[3]
old_x = returnvalue[4]
old_y = returnvalue[5]
old_z = returnvalue[6]
old_velocity_z = returnvalue[7]
"""
[roll, pitch, yaw, z_dot, old_x, old_y, old_z,
old_velocity_z] = returnvalue
yaw_dot = 0
self.set_vel(roll, pitch, z_dot, yaw)
self.store_old_values(old_x, old_y, old_z, old_velocity_z)
def __init__(self):
self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32)
self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre',
TransformStamped, self.update_vicon)
self.desired_position_data = rospy.Subscriber(
'/desired_positions', PoseStamped, self.update_desired_position)
self.start_stop_command = rospy.Subscriber('/start_stop_toggle',
String, self.update_state)
self.request_pic = rospy.Publisher('/take_pic', String, queue_size=10)
self.request_land = rospy.Publisher('/ask_land', String, queue_size=2)
self.pub_errors = rospy.Publisher('/errors',
PoseStamped,
queue_size=100)
#self.pub_check = rospy.Publisher('/check_mate', String, queue_size = 100)
self.run_state = False
self.translation_x_old = 0.0
self.translation_y_old = 0.0
self.translation_z_old = 0.0
self.z_velocity_old = 0.0
self.translation_x_desired = 0.0
self.translation_y_desired = 0.0
self.translation_z_desired = 0.0
self.translation_x = 0.0
self.translation_y = 0.0
self.translation_z = 0.0
self.rotation_x = 0.0
self.rotation_y = 0.0
self.rotation_z = 0.0
self.rotation_w = 1.0
self.rotation_x_desired = 0.0
self.rotation_y_desired = 0.0
self.rotation_z_desired = 0.0
self.rotation_w_desired = 1.0
self.pic_counter = 0
self.pic_x = [
0, -2, -2, -2, -2, -2, -2, -2, -2, -2, -1, -1, -1, -1, -1, -1, -1,
-1, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2,
2, 2, 2, 2, 2, 2, 2
]
self.pic_y = [
0, -2.0000, -1.5000, -1.0000, -0.5000, 0, 0.5000, 1.0000, 1.5000,
2.0000, 2.0000, 1.5000, 1.0000, 0.5000, 0, -0.5000, -1.0000,
-1.5000, -2.0000, -2.0000, -1.5000, -1.0000, -0.5000, 0, 0.5000,
1.0000, 1.5000, 2.0000, 2.0000, 1.5000, 1.0000, 0.5000, 0, -0.5000,
-1.0000, -1.5000, -2.0000, -2.0000, -1.5000, -1.0000, -0.5000, 0,
0.5000, 1.0000, 1.5000, 2.0000
]
self.pic_z = [
1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3,
1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3,
1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3,
1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3
]
self.waypoint_counter = 0
x = np.loadtxt(
'/home/mason/aer1217/labs/src/aer1217_ardrone_simulator/scripts/order.txt',
delimiter=None)
self.order_pic = x.astype(int)
self.x_position_array = [1, 4.26, 0.88, 4.33, 7.69, 1]
self.y_position_array = [1, 1.23, 5.48, 8.04, 4.24, 1]
self.time_interval = 0
self.current_time = rospy.get_time()
self.old_time = rospy.get_time()
self.postcom = PositionController()
# Run the publish commands at 100 Hz
self.onboard_loop_frequency = 100.
self.pic_taking_speed = 10.
self.waypoint_check_speed = 10.
# Run this ROS node at the onboard loop frequency
self.nutjobcase = rospy.Timer(
rospy.Duration(1. / self.onboard_loop_frequency), self.run_process)
#self.wedfsj = rospy.Timer(rospy.Duration(1. / self.pic_taking_speed), self.pic_initiator)
self.wedfsj = rospy.Timer(
rospy.Duration(1. / self.waypoint_check_speed),
self.mission_planner)
self.postcom = PositionController()
def __init__(self, uav_ids, init, fin, vx_ds, vy_ds, vz_ds, yaw_ds):
self.cf_ids = uav_ids
self.number_of_agents = np.shape(uav_ids)[0]
self.takoff_alt = 1.0 # change?
self._pos = {}
self._vel = {}
self._quat = {}
self._dist_to_goal = {}
self._euler_angles = {}
self._euler_angular_rates = {}
self._z_old = {}
self._z_oold = {}
self.yaw_old = {}
self.radius = 0.15
self.d_star = self.radius
self.MaxVelo = 1.0
# Tune these
self.c1 = 7 * 4.5
self.c2 = 9 * 4.5
self.RepulsiveGradient = 7.5 * 100
self.previous_time = 0.0
self.change_time = -100.0
self.controller = PositionController()
self.init_time = rospy.get_time()
### Publish ###
# waypoint messages
self.goal_msg_0, self.goal_msg_1, self.goal_msg_2, self.goal_msg_3 = PoseStamped(
), PoseStamped(), PoseStamped(), PoseStamped()
self.goal_msgs = {
'0': self.goal_msg_0,
'1': self.goal_msg_1,
'2': self.goal_msg_2,
'3': self.goal_msg_3
}
# rotor velocities messages
# self.cmdV_msg_0, self.cmdV_msg_1, self.cmdV_msg_2, self.cmdV_msg_3 = Actuators(), Actuators(), Actuators(), Actuators()
# self.rotor_vel_msgs = {
# '0' : self.cmdV_msg_0,
# '1' : self.cmdV_msg_1,
# '2' : self.cmdV_msg_2,
# '3' : self.cmdV_msg_3
# }
### Subscribe ###
# odometry messages
self._currpos_callbacks = {
'0': self._currpos_callback_0,
'1': self._currpos_callback_1,
'2': self._currpos_callback_2,
'3': self._currpos_callback_3
}
# set parameters
self.initials, self.finals = {}, {}
self.goal_pubs, self.cmdVtemp_pubs, self.cmdV_pubs, self.takeoffs, self.lands = {}, {}, {}, {}, {}
for index, cf_id in enumerate(self.cf_ids):
self.initials[str(cf_id)] = init[index][:]
self._z_old[str(cf_id)] = 0.0
self._z_oold[str(cf_id)] = 0.0
self.yaw_old[str(cf_id)] = 0.0
self.finals[str(cf_id)] = fin[index][:]
self.goal_pubs[str(cf_id)] = rospy.Publisher('/crazyflie2_' +
str(cf_id) + '/goal',
PoseStamped,
queue_size=1)
self.cmdV_pubs[str(cf_id)] = rospy.Publisher(
'/crazyflie2_' + str(cf_id) + '/command/motor_speed',
Actuators,
queue_size=1)
rospy.Subscriber("/crazyflie2_" + str(cf_id) + "/odometry",
Odometry, self._currpos_callbacks[str(cf_id)])
self.vx_d = vx_ds
self.vy_d = vy_ds
self.vz_d = vz_ds
self.yaw_d = yaw_ds
self.takeoffed = False
self.reached_1st = False
self.flag = {'flying': 0, 'landed': 0, 'preland': 0}
# self.vstate = {
# 'takeoff' : self.do_takeoff,
# 'wpnav' : self.do_wpnav,
# 'land' : self.do_land
# }
# Position controller
self.controller = PositionController()
def __init__(self, **kwargs):
self.mainloop = gobject.MainLoop()
self.pipeline = gst.Pipeline("pipeline")
self.verbose = kwargs.get('verbose', True)
self.debug = kwargs.get('debug', False)
cam_width = kwargs.get('cam_width', 640)
cam_height = kwargs.get('cam_height', 480)
host = kwargs.get('host', '127.0.0.1')
port = kwargs.get('port', 5000)
h264 = kwargs.get('h264', False)
self.marker_spotted = False
self.image_processing = ImageProcessing(area_threshold=10)
self.state_estimate = StateEstimationAltitude()
self.autopilot = AutoPilot(self.state_estimate)
self.position_controller = PositionController(self.autopilot,
self.state_estimate)
if h264:
self.videosrc = gst.parse_launch(
'uvch264_src device=/dev/video0 name=src auto-start=true src.vfsrc'
)
else:
self.videosrc = gst.element_factory_make('v4l2src', 'v4l2src')
fps = 30
self.vfilter = gst.element_factory_make("capsfilter", "vfilter")
self.vfilter.set_property(
'caps',
gst.caps_from_string(
'image/jpeg, width=%s, height=%s, framerate=20/1' %
(str(cam_width), str(cam_height))))
self.queue = gst.element_factory_make("queue", "queue")
self.udpsink = gst.element_factory_make('udpsink', 'udpsink')
self.rtpjpegpay = gst.element_factory_make('rtpjpegpay', 'rtpjpegpay')
self.udpsink.set_property('host', host)
self.udpsink.set_property('port', port)
self.pipeline.add_many(self.videosrc, self.queue, self.vfilter,
self.rtpjpegpay, self.udpsink)
gst.element_link_many(self.videosrc, self.queue, self.vfilter,
self.rtpjpegpay, self.udpsink)
pad = next(self.queue.sink_pads())
# Sending frames to onVideBuffer where openCV can do processing.
pad.add_buffer_probe(self.onVideoBuffer)
self.pipeline.set_state(gst.STATE_PLAYING)
self.i = 0
gobject.threads_init()
context = self.mainloop.get_context()
#previous_update = time.time()
fpstime = time.time()
while True:
try:
context.iteration(False)
self.autopilot._read_sensors()
if self.autopilot.auto_switch > 1700:
self.position_controller.headingHold()
self.position_controller.holdAltitude()
self.autopilot.send_control_commands()
else:
self.position_controller.reset_targets()
print self.autopilot.print_commands()
except KeyboardInterrupt:
fps = self.i / (time.time() - fpstime)
print 'fps %f ' % fps
self.autopilot.dump_log()
self.autopilot.disconnect_from_drone()
return euler_from_quaternion(orient_q)
def get_time(self):
return self._vicon_msg.header.stamp.nsecs
def land(self):
self.pub_land.publish()
def set_vel(self, traj):
self.pub_traj.publish(traj)
if __name__ == '__main__':
# write code to create ROSControllerNode
rospy.init_node("ros_interface", disable_signals=True)
positionCtrl = PositionController()
ardrone = ROSControllerNode()
# ardrone.time_stamp = ardrone.get_time()
#first test
# x_des = 4
# y_des = 4
# z_des = 4
# yaw_des = 1
try:
while not rospy.is_shutdown():
#get position nad orientation from vicon
currentPosition = ardrone.get_pos()
currentOrientation = ardrone.get_orient()
#compute desired pose
