def __init__(self):
self.status = ""
rospy.init_node('forward', anonymous=False)
self.navdata = Navdata()
self.odom = Odometry()
self.mark = Marker()
self.last_time = time.time()
self.state = "hover"
self.speed = 0.0
self.count = 0
self.rate = rospy.Rate(100.0) # self.rate.sleep_dur.nsecs/1000000
self.period_s = self.rate.sleep_dur.nsecs/1000000000.0 # second
#self.period_s = self.rate.sleep_dur.nsecs/1000000.0 # milisecons
self.pidAlt = PIDControl(self.period_s, 1, 0, 0, 'Altitude control')
self.pubTakeoff = rospy.Publisher("ardrone/takeoff",Empty, queue_size=10)
self.pubLand = rospy.Publisher("ardrone/land",Empty, queue_size=10)
self.pubLand = rospy.Publisher("ardrone/land",Empty, queue_size=10)
self.pubCommand = rospy.Publisher('cmd_vel',Twist, queue_size=10)
self.command = Twist()
rospy.Subscriber("/ardrone/navdata", Navdata, self.cbNavdata)
# rospy.Subscriber("/ardrone/odometry", Odometry, self.cbOdom)
rospy.Subscriber("/ground_truth/state", Odometry, self.cbOdom)
rospy.Subscriber("/visualization_marker", Marker, self.cbMarker)
self.state_change_time = rospy.Time.now()
rospy.on_shutdown(self.SendLand)
def __init__(self):
self.status = ""
rospy.init_node('forward', anonymous=False)
self.pid = {
# 'az': PID(T * 0.001, (1.0/180.0), 0, 0, "Orientation"),
'az': PID(T / 1000.0, 2.0, 0.0, 1.0, "Orientation"),
'z': PID(T / 1000.0, 0.007, 0.000, 0.001, "Altitude"),
'x': PID(T / 1000.0, 0.007, 0.000, 0.001, "X Position"),
'y': PID(T / 1000.0, 0.007, 0.000, 0.001, "Y Position")
}
self.navdata = Navdata()
self.odom = Odometry()
self.mark = Marker()
self.rate = rospy.Rate(10)
self.pose = {'x': 1.0, 'y': -1.0, 'z': 1.0, 'w': 0}
self.pubTakeoff = rospy.Publisher("ardrone/takeoff",
Empty,
queue_size=10)
self.pubLand = rospy.Publisher("ardrone/land", Empty, queue_size=10)
self.pubLand = rospy.Publisher("ardrone/land", Empty, queue_size=10)
self.pubCommand = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.command = Twist()
rospy.Subscriber("/ardrone/navdata", Navdata, self.cbNavdata)
# rospy.Subscriber("/ardrone/odometry", Odometry, self.cbOdom)
rospy.Subscriber("/ground_truth/state", Odometry, self.cbOdom)
rospy.Subscriber("/visualization_marker", Marker, self.cbMarker)
self.state_change_time = rospy.Time.now()
rospy.on_shutdown(self.SendLand)
def move2alt(self):
goal_alt = Navdata()
goal_alt = input("Set your alt:")
altitude_tolerance = input("Set your tolerance:")
vel_msg = Twist()
# while sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2)) >= distance_tolerance:
while goal_alt - self.navdata.altd >= altitude_tolerance:
#Porportional Controller
#linear velocity in the x-axis:
# vel_msg.linear.x = 1.5 * sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2))
vel_msg.linear.x = 0
vel_msg.linear.y = 0
vel_msg.linear.z = goal_alt - self.navdata.altd
#angular velocity in the z-axis:
vel_msg.angular.x = 0
vel_msg.angular.y = 0
# vel_msg.angular.z = 4 * (atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) - self.pose.theta)
vel_msg.angular.z = 0
#Publishing our vel_msg
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
#Stopping our robot after the movement is over
# vel_msg.linear.x = 0
vel_msg.linear.z = 0
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)
rospy.spin()
def __init__(self, max_speed=0.5, cmd_period=100):
self._current_state = dict(roll=0,
pitch=0,
z_velocity=0,
yaw_velocity=0)
self._last_state = self._current_state.copy()
self._queue = []
self.navdata = Navdata()
self.subNavdata = rospy.Subscriber(
'/ardrone/navdata', Navdata,
lambda data: setattr(self, 'navdata', data))
self.pubLand = rospy.Publisher('/ardrone/land', Empty, queue_size=1)
self.pubTakeoff = rospy.Publisher('/ardrone/takeoff',
Empty,
queue_size=1)
self.pubReset = rospy.Publisher('/ardrone/reset', Empty, queue_size=1)
self.pubCommand = rospy.Publisher('/cmd_vel', Twist, queue_size=30)
self.commandTimer = rospy.Timer(rospy.Duration(cmd_period / 1000.0),
self.__PublishCommand)
self.max_speed = min(abs(max_speed), MAX_SPEED)
rospy.on_shutdown(self.SendLand)
def __init__(self):
self.pubPoseTrokut = rospy.Publisher("/camera/tezej", Pose)
self.position = Pose()
self.pubPoseNorth = rospy.Publisher("/camera/north", Pose)
self.north = Pose()
self.pubPoseYM = rospy.Publisher("/camera/marker/yellow", Pose)
self.yellowMarker = Pose()
self.pubPoseOM = rospy.Publisher("/camera/marker/orange", Pose)
self.orangeMarker = Pose()
self.pubPoseBM = rospy.Publisher("/camera/marker/blue", Pose)
self.blueMarker = Pose()
self.pubPosePM = rospy.Publisher("/camera/marker/purple", Pose)
self.purpleMarker = Pose()
self.pubPoseExit = rospy.Publisher("/camera/exit", Pose)
self.exitMarker = Pose()
self.navdata = Navdata()
#self.subNavdata = rospy.Subscriber("/ardrone/navdata", Navdata, self.setNav)
self.pubPoseGlobal = rospy.Publisher("/globus", Pose)
self.globalCoords = Pose()
def __init__(self):
#Creating our node,publisher and subscriber
rospy.init_node('qc_controller', anonymous=True)
self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1000)
self.navdata_subscriber = rospy.Subscriber('/ardrone/navdata', Navdata, self.callback)
self.navdata = Navdata()
self.rate = rospy.Rate(100)
def __init__(self):
self.lastNavdata = Navdata()
self.lastImu = Imu()
self.lastMag = Vector3Stamped()
self.current_pose = PoseStamped()
self.current_odom = Odometry()
self.lastState = State.Unknown
self.command = Twist()
self.drone_msg = ARDroneData()
self.cmd_freq = 1.0 / 200.0
self.drone_freq = 1.0 / 200.0
self.action = Controller.ActionTakeOff
self.previousDebugTime = rospy.get_time()
self.pose_error = [0, 0, 0, 0]
self.pidX = PID(0.35, 0.15, 0.025, -1, 1, "x")
self.pidY = PID(0.35, 0.15, 0.025, -1, 1, "y")
self.pidZ = PID(1.25, 0.1, 0.25, -1.0, 1.0, "z")
self.pidYaw = PID(0.75, 0.1, 0.2, -1.0, 1.0, "yaw")
self.scale = 1.0
self.goal = [-1, 0, 0, height, 0] #set it to center to start
self.goal_rate = [0, 0, 0, 0, 0] # Use the update the goal on time
self.achieved_goal = Goal(
) # Use this to store recently achieved goal, reference time-dependent
self.next_goal = Goal() # next goal
self.goal_done = False
self.waypoints = None
rospy.on_shutdown(self.on_shutdown)
rospy.Subscriber("ardrone/navdata", Navdata, self.on_navdata)
rospy.Subscriber("ardrone/imu", Imu, self.on_imu)
rospy.Subscriber("ardrone/mag", Vector3Stamped, self.on_mag)
rospy.Subscriber("arcontroller/goal", Goal, self.on_goal)
rospy.Subscriber("arcontroller/waypoints", Waypoints,
self.on_waypoints)
rospy.Subscriber("qualisys/ARDrone/pose", PoseStamped,
self.get_current_pose)
rospy.Subscriber("qualisys/ARDrone/odom", Odometry,
self.get_current_odom)
self.pubTakeoff = rospy.Publisher('ardrone/takeoff',
Empty,
queue_size=1)
self.pubLand = rospy.Publisher('ardrone/land', Empty, queue_size=1)
self.pubCmd = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.pubDroneData = rospy.Publisher('droneData',
ARDroneData,
queue_size=1)
self.pubGoal = rospy.Publisher('current_goal', Goal, queue_size=1)
self.setFlightAnimation = rospy.ServiceProxy(
'ardrone/setflightanimation', FlightAnim)
self.commandTimer = rospy.Timer(rospy.Duration(self.cmd_freq),
self.sendCommand)
#self.droneDataTimer = rospy.Timer(rospy.Duration(self.drone_freq),self.sendDroneData)
#self.goalTimer = rospy.Timer(rospy.Duration(self.drone_freq),self.sendCurrentGoal)
self.listener = TransformListener()
def _publish_navdata(self):
"""Publish Navdata"""
orientation = tf.transformations.euler_from_quaternion(
self._sensors['marg'].get_output())
velocity = self._sensors['velocity'].get_output()
altitude = self._sensors['altimeter'].get_output()
msg = self._stamp_msg(Navdata())
msg.altd = altitude[0] * 1000
msg.vx = velocity[0] * 1000
msg.vy = velocity[1] * 1000
msg.rotX = orientation[0] * 180 / numpy.pi
msg.rotY = orientation[1] * 180 / numpy.pi
msg.rotZ = orientation[2] * 180 / numpy.pi
msg.batteryPercent = self._quadrotor.get_battery()
msg.state = self._quadrotor.get_status()
self._publishers['navdata'].publish(msg)
def __init__(self):
# Subscribe to the /ardrone/navdata topic, of message type navdata, and call self.ReceiveNavdata when a message is received
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata, self.receive_navdata)
self.subOdom = rospy.Subscriber('/ground_truth/state', Odometry, self.odom_callback)
rospy.Subscriber('/vrpn_client_node/TestTed/pose', PoseStamped, self.optictrack_callback)
# Allow the controller to publish to the /ardrone/takeoff, land and reset topics
self.pubLand = rospy.Publisher('/ardrone/land', Empty, queue_size=0)
self.pubTakeoff = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=0)
self.pubReset = rospy.Publisher('/ardrone/reset', Empty, queue_size=1)
# Allow the controller to publish to the /cmd_vel topic and thus control the drone
self.pubCommand = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# Put location into odometry
self.location = Odometry()
self.status = Navdata()
self.real_loc = PoseStamped()
# Gets parameters from param server
self.speed_value = rospy.get_param("/speed_value")
self.run_step = rospy.get_param("/run_step")
self.desired_pose = Pose()
self.desired_pose.position.z = rospy.get_param("/desired_pose/z")
self.desired_pose.position.x = rospy.get_param("/desired_pose/x")
self.desired_pose.position.y = rospy.get_param("/desired_pose/y")
self.max_incl = rospy.get_param("/max_incl")
self.max_altitude = rospy.get_param("/max_altitude")
self.exporation_noise = OUNoise()
self.on_place = 0
# initialize action space
# Forward,Left,Right,Up,Down
#self.action_space = spaces.Discrete(8)
self.action_space = spaces.Box(np.array((-0.5, -0.5, -0.5, -0.5)), np.array((0.5, 0.5, 0.5, 0.5)))
# Gazebo Connection
self.gazebo = GazeboConnection()
self._seed()
# Land the drone if we are shutting down
rospy.on_shutdown(self.send_land)
def __init__(self, rate=200):
self.navdata = Navdata()
self.sub_navdata = rospy.Subscriber('/ardrone/navdata', Navdata, self.navdata_cb)
self.sub_altitude = rospy.Subscriber('/ardrone/navdata_altitude', navdata_altitude, self.altd_cb)
# self.pub_alt = rospy.Publisher('/altd', Float32, queue_size=100)
self.tag_acquired = False
self.theta = 0
# Euler angles
self.roll = 0
self.pitch = 0
# Altitude
self.altitude = 0
# Battery
self.battery = 0
# Magnetometer
self.magX = 0
self.magY = 0
# These are switched for the controller purposes
# Raw values taken from the QC
self.tag_x = 0
self.prev_tag_x = 0
self.tag_vx = 0
self.tag_y = 0
self.prev_tag_y = 0
self.tag_vy = 0
self.rate = rate
# These are switched for the controller purposes
# Calculated, normalized values
self.norm_tag_x = 0
self.norm_prev_tag_x = 0
self.norm_tag_vx = 0
self.norm_tag_y = 0
self.norm_prev_tag_y = 0
self.norm_tag_vy = 0
# For the polar controller
self.r = 0
def __init__(self):
# rospy.init_node('forward', anonymous=False)
rospy.Subscriber("/ardrone/navdata", Navdata, self.cbNavdata)
rospy.Subscriber("/ardrone/odometry", Odometry, self.cbOdom)
# rospy.Subscriber("/ground_truth/state", Odometry, self.cbOdom)
# rospy.Subscriber("/visualization_marker", Marker, self.cbMarker)
rospy.Subscriber("/ar_filter_pose", Marker, self.cbMarker)
self.navdata = Navdata()
self.odom = Odometry()
self.mark = Marker()
self.rate = rospy.Rate(100)
self.pubTakeoff = rospy.Publisher("ardrone/takeoff",
Empty,
queue_size=10)
self.pubLand = rospy.Publisher("ardrone/land", Empty, queue_size=10)
self.pubLand = rospy.Publisher("ardrone/land", Empty, queue_size=10)
self.pubCommand = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.command = Twist()
#self.pid = {
# 'az': PID(T * 0.001, (1.0/360.0), 0, 0, "Orientation"),
# 'z': PID(T * 0.001, 0.20000000000, 0.0, 0.00000, "Altitude"), # 5
# 'x': PID(T * 0.001, 0.20000000000, 0.0, 0.00000, "X Position"), # 2
# 'y': PID(T * 0.001, 0.50000000000, 0.0, 0.00000, "Y Position") # 5
#}
self.pid = {
# 'az': PID(T * 0.001, (1.0/180.0), 0, 0, "Orientation"),
'az': PID(T / 1000.0, 2.0, 0.0, 1.0, "Orientation"),
'z': PID(T / 1000.0, 2.0, 0.0, 1.0, "Altitude"),
'x': PID(T / 1000.0, 0.007, 0.000, 0.001, "X Position"),
'y': PID(T / 1000.0, 0.007, 0.000, 0.001, "Y Position")
}
#self.commandTimer = rospy.Timer(rospy.Duration(COMMAND_PERIOD/1000.0),self.SetCommand)
self.state_change_time = rospy.Time.now()
self.last_stamp = time.time()
rospy.on_shutdown(self.SendLand)
def __init__(self):
# initialise state
# ground truth position
self.x = 0
self.y = 0
self.z = 0
# ground truth velocity
self.x_dot = 0
self.y_dot = 0
self.z_dot = 0
# ground truth quaternion
self.imu_x = 0 # q0
self.imu_y = 0 # q1
self.imu_z = 0 # q2
self.imu_w = 0 # q3
# nav drone angle
self.rot_x = 0 # roll
self.rot_y = 0 # pitch
self.rot_z = 0 # yaw
# Optitrack Information
self.x_real = 0
self.y_real = 0
self.z_real = 0
self.dist = 0
# Subscribe to the /ardrone/navdata topic, of message type navdata, and call self.ReceiveNavdata when a message is received
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.receive_navdata)
self.subOdom = rospy.Subscriber('/ground_truth/state', Odometry,
self.odom_callback)
# rospy.Subscriber('/vrpn_client_node/Rigid_grant/pose', PoseStamped, self.optictrack_callback)
# Allow the controller to publish to the /ardrone/takeoff, land and reset topics
self.pubLand = rospy.Publisher('/ardrone/land', Empty, queue_size=0)
self.pubTakeoff = rospy.Publisher('/ardrone/takeoff',
Empty,
queue_size=0)
self.pubReset = rospy.Publisher('/ardrone/reset', Empty, queue_size=1)
# Allow the controller to publish to the /cmd_vel topic and thus control the drone
self.pubCommand = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# Put location into odometry
self.location = Odometry()
self.status = Navdata()
self.loc_real = PoseStamped()
# Gets parameters from param server
self.speed_value = rospy.get_param("/speed_value")
self.run_step = rospy.get_param("/run_step")
self.target_x = rospy.get_param("/desired_pose/x")
self.target_y = rospy.get_param("/desired_pose/y")
self.target_z = rospy.get_param("/desired_pose/z")
self.desired_pose = np.array(
[self.target_x, self.target_y, self.target_z])
# self.desired_pose = [0, 0, 1.5]
self.max_incl = rospy.get_param("/max_incl")
self.max_altitude = rospy.get_param("/max_altitude")
self.min_altitude = rospy.get_param("/min_altitude")
self.x_max = rospy.get_param("/max_pose/max_x")
self.y_max = rospy.get_param("/max_pose/max_y")
self.on_place = 0
self.count_on_place = 0
# initialize action space
# Forward,Left,Right,Up,Down
# self.action_space = spaces.Discrete(8)
self.action_space = spaces.Discrete(7)
self.up_bound = np.array([np.inf, np.inf, np.inf, np.inf, 1])
self.low_bound = np.array([-np.inf, -np.inf, -np.inf, -np.inf, 0])
self.observation_space = spaces.Box(
self.low_bound,
self.up_bound) # position[x,y,z], linear velocity[x,y,z]
#self.observation_space = spaces.Box(-np.inf, np.inf, (9,))
# Gazebo Connection
self.gazebo = GazeboConnection()
self._seed()
# Land the drone if we are shutting down
rospy.on_shutdown(self.send_land)
#=======================#
# Messages Needed #
#=======================#
from risc_msgs.msg import *
from ardrone_autonomy.msg import Navdata
from geometry_msgs.msg import PointStamped
from std_msgs.msg import Empty
#========================#
# Globals #
#========================#
PI = 3.141592653589793
Threshold = 1000
navdata = Navdata()
states = Cortex()
states.Obj = [States()]*0
traj = Trajectories()
traj.Obj = [Trajectory()]*1
euler_max = 0.349066 #in radians
max_yaw_rate = .3490659 #in radians/sec
max_alt_rate = 1000 # in mm/sec
rate = 200 # Hz
cycles = 2 # number of cycles for
time_past = 0
image = 0
start_time = 0
mode = 0 # mode of 7 listed under cases
cases = ['Takeoff','Fly to Origin','Slanted Figure Eight',\
'Flat Figure Eight','Circle', 'Fly to Origin', 'Land']
from std_msgs.msg import String from std_msgs.msg import Char from std_msgs.msg import Empty from std_msgs.msg import Float64 from sensor_msgs.msg import Imu from geometry_msgs.msg import Twist from geometry_msgs.msg import Vector3 from nav_msgs.msg import Odometry from ardrone_autonomy.msg import Navdata from ar_track_alvar_msgs.msg import AlvarMarkers, AlvarMarker from drone import Drone #Variables initialized cmd_vel = Twist() nav_data = Navdata() imu_data = Imu() odom = Odometry() empty = Empty() ar_data = AlvarMarkers() # queues for keeping center positions to take average mean_center_x = collections.deque(maxlen=20) mean_center_y = collections.deque(maxlen=20) mean_center_z = collections.deque(maxlen=20) mean_center_yaw = collections.deque(maxlen=20) # TODO set it to false if you want to fly the drone testing = True cx = 0.0 cy = 0.0