def droneTakeoff(self, alt=1.0):
if self.isAirbourne == False or self.sysState == 'idle':
if not self.droneArmed:
mavCMD.arming(True)
preArmMsgs = self.curPos
preArmMsgs.pose.position.z = alt
for i in range(50):
self._pubMsg(preArmMsgs, self.spLocalPub)
self.setState('takeoff')
self.setMode(0, 'OFFBOARD')
self.setPoint = preArmMsgs
self.isAirbourne = True
self.messageHandlerPub.publish(self.isAirbourne)
# wait until takeoff has occurred
while (self.curPos.pose.position.z <=
(preArmMsgs.pose.position.z - 0.25)):
self._pubMsg(self.setPoint, self.spLocalPub)
else:
self.setState('landing')
self.setMode(0, 'AUTO.LAND')
while self.uavState.armed:
self.rate.sleep()
self.isAirbourne = False
self.setState('idle')
def __init__(self):
# Arm the drone
mavros.set_namespace()
command.arming(True)
self.pub_sp = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
rospy.wait_for_service('mavros/set_mode')
change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)
rospy.init_node('gotowaypoint', anonymous=True)
rate = rospy.Rate(50)
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_cb)
self.waypoint = PoseStamped()
self.waypoint.header.frame_id = 'map'
self.waypoint.pose.position.x = 0
self.waypoint.pose.position.y = 0
self.waypoint.pose.position.z = 5
self.marker_pub = rospy.Publisher('/waypoint_marker', Marker, queue_size=10)
self.waypoint_marker = Marker()
self.waypoints = np.array([[0,0,0],
[0,1,1],
[0,3,3]])
self.publish_waypoint()
while not rospy.is_shutdown():
result_mode = change_mode(0,"OFFBOARD")
self.pub_sp.publish(self.waypoint)
rate.sleep()
def joy_cb(self, data):
if self.state_mach == self.DISARMED and data.buttons[20] == 1:
mavros.set_namespace()
self.set_offboard_mode()
elif self.state_mach == self.OFFBOARD and data.buttons[3] == 1:
command.arming(True)
elif self.state_mach == self.ARMED and data.buttons[4] == 1:
command.arming(False)
def __init__(self):
# Arm the drone
mavros.set_namespace()
command.arming(True)
self.pub_sp = rospy.Publisher('/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=10)
rospy.init_node('gotowaypoint', anonymous=True)
rate = rospy.Rate(60) # 10hz
# Parse input
#parser = argparse.ArgumentParser()
#parser.add_argument("--boundary", help="filepath to the control boundary")
#args = parser.parse_args(rospy.myargv(argsv))
#self.pub_sp = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
rospy.wait_for_service('mavros/set_mode')
self.change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)
data = self.load_config('scripts/boundary.yaml')
self.boundary = Boundary(data)
self.controller = QUAD_position_controller(
self.load_config('scripts/gainsStart.yaml'))
# Set parameters
self.kz = 0.05
self.hoverth = 0.565
# Subscribe to local position
self.local_pose = PoseStamped()
self.velocity = TwistStamped()
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
self._send_command)
rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped,
self._read_velocity)
# Set Waypoint
self.waypoint = PoseStamped()
self.waypoint.pose.position.x = -1.4
self.waypoint.pose.position.y = -5
self.waypoint.pose.position.z = 3
self.marker_pub = rospy.Publisher('/waypoint_marker',
Marker,
queue_size=10)
self.waypoint_marker = Marker()
self.publish_waypoint()
# Define Controller
rospy.spin()
def joy_cb(self, data):
if self.state_mach == self.DISARMED and data.buttons[20] == 1:
mavros.set_namespace()
self.set_offboard_mode()
elif self.state_mach == self.OFFBOARD and data.buttons[3] == 1:
command.arming(True)
elif self.state_mach == self.ARMED and data.buttons[4] == 1:
command.arming(False)
elif self.state_switch_mode == self.UNSWITCHED and data.buttons[7] == 1:
self.state_switch_mode = self.SWITCHED
doS = rospy.ServiceProxy('doSwitch', CommandBool)
doS(True)
elif self.state_switch_mode == self.SWITCHED and data.buttons[8] == 1:
self.state_switch_mode = self.UNSWITCHED
doS = rospy.ServiceProxy('doSwitch', CommandBool)
doS(False)
def __init__(self, argsv):
# Init Node
self.main_loop_rate = 60
self.is_simulation = True
self.p = namedtuple("p", "x y z")
self.q = namedtuple("q", "w x y z")
self.v = namedtuple("v", "x y z")
self.omg = namedtuple("omg", "x y z")
self.p.x, self.p.y, self.p.z, self.q.w, self.q.x, self.q.y, self.q.z, self.v.x, self.v.y, self.v.z, self.omg.x,\
self.omg.y, self.omg.z = 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.
self.u_current = np.zeros((1, 4))
self.T_d = 0.0
self.q_d = namedtuple("q_d", "w x y z")
self.omg_d = namedtuple("omg_d", "x y z")
self.f_d = namedtuple(
"f_d", "x y z") #Variable used to publish desired force commands
self.init_ROS()
stabilize_time = 1.0
state = 'not_launched'
z_th = 0.5
# Armrq
mavros.set_namespace()
command.arming(True)
while (not rospy.is_shutdown()):
# Run State Machine
if (state == 'not_launched'):
if (self.p.z > z_th):
launch_time = rospy.Time().now()
state = 'launched'
print('Launch Detected.')
elif (state == 'launched'):
if ((rospy.Time().now() - launch_time).to_sec() >
stabilize_time):
self.change_mode(0, "POSCTL")
state = 'stabilize'
print('Stabilize mode set.')
# Check altitude
self.rate.sleep()
def main():
rospy.init_node('px4_offboard_python', anonymous=True)
#state_sub = rospy.Subscriber("mavros/state", )
#while not rospy.is_shutdown() and not current_state.connected:
# rospy.Rate(20)
#rospy.loginfo('FCU connection successful')
#rospy.Subscriber("chatter", String, callback)
pub = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
#local_pos_pub = get_pub_position_local(queue_size=10)
#arming_client = rospy.ServiceProxy("/mavros/arming", CommandBool)
set_mode = rospy.ServiceProxy("/mavros/set_mode", SetMode)
rate = rospy.Rate(20)
msg = PoseStamped()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = 0
msg.pose.position.y = 0
msg.pose.position.z = 0.5
#for i in range(100):
# local_pos_pub.Publish(msg)
# rate.sleep()
#offb_set_mode = SetMode()
#offb_set_mode.request.custom_mode('OFFBOARD')
#rospy.loginfo("Drone offboard!")
req = SetModeRequest()
req.custom_mode = 'OFFBOARD'
set_mode.call(req)
print 'mode: ' + State().mode
mavros.set_namespace()
command.arming(True)
rospy.loginfo("Drone armed!")
rospy.spin()
def _arm(self, state):
try:
ret = command.arming(value=state)
except rospy.ServiceException as ex:
fault(ex)
if not ret.success:
fault("Request failed. Check mavros logs")
return ret
def _arm(self, state):
try:
ret = command.arming(value=state)
if not ret.success:
rospy.logerr("Request failed. Check mavros logs")
rospy.loginfo("Command result: %s" % str(ret.result))
except rospy.ServiceException as ex:
rospy.logerr(ex)
def droneTakeoff(self, alt=1.0):
if self.isAirbourne == False or self.sysState == 'idle':
if not self.MAVROS_State.armed:
mavCMD.arming(True)
preArmMsgs = self.uavLocalPos
preArmMsgs.pose.position.z = 1.5
for i in range(50):
self._pubMsg(preArmMsgs, self.spLocalPub)
self._setState('takeoff')
self.setMode(0, 'OFFBOARD')
self.isAirbourne = True
self.enableMHPub.publish(self.isAirbourne)
#wait until takeoff has occurred
while (self.uavLocalPos.pose.position.z <=
(preArmMsgs.pose.position.z - 0.25)):
self._pubMsg(preArmMsgs, self.spLocalPub)
def arm():
try:
ret = command.arming(value=True)
except rospy.ServiceException as ex:
fault(ex)
if not ret.success:
fault('Request failed. Check mavros logs')
return ret
def reset_position(self):
# Arm the drone
mavros.set_namespace()
command.arming(True)
rospy.wait_for_service('mavros/set_mode')
change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)
self.waypoint = PoseStamped()
self.waypoint.header.frame_id = 'map'
self.waypoint.pose.position.x = self.p_init.x
self.waypoint.pose.position.y = self.p_init.y
self.waypoint.pose.position.z = self.p_init.z
while not rospy.is_shutdown() and (np.linalg.norm(
np.array([self.p_init.x - self.p.x,
self.p_init.y - self.p.y])) > 0.2 or abs(self.p_init.z - self.p.z > 0.05)): # or \
#rospy.Duration.from_sec(rospy.get_time() - self.t_last_rl_msg.to_sec()).to_sec() > 0.05):
result_mode = change_mode(0, "OFFBOARD")
self.pub_posmsg.publish(self.waypoint)
self.rate.sleep()
def _arm():
""" for this use case, state is always expected to be 'throttle' """
state = 1 # true, throttle on
try:
ret = command.arming(value=state)
except rospy.ServiceException as ex:
fault(ex)
if not ret.success:
fault("Request failed. Check mavros logs")
#print_if(args.verbose, "Command result:", ret.result)
print("Command result:", ret.result)
return ret
def disarm(self):
if not self.armed:
print("Vehicle Already Disarmed !")
else:
print("Disarming Now...")
try:
ret = command.arming(False)
self.armed = False
if not ret.success:
utils.fault("Request failed. Check mavros logs")
except rospy.ServiceException as ex:
utils.fault(ex)
print("Disarming Command result: %s" % (self.armed == False))
def _arm(args, state):
try:
ret = command.arming(value=state)
except rospy.ServiceException as ex:
fault(ex)
if not ret.success:
fault("Request failed. Check mavros logs")
print_if(args.verbose, "Command result:", ret.result)
return ret
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def __init__(self):
# Arm the drone
mavros.set_namespace()
command.arming(True)
self.pub_sp = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
rospy.wait_for_service('mavros/set_mode')
self.change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)
if rospy.is_shutdown():
rospy.init_node('gotowaypoint', anonymous=True)
self.rate = rospy.Rate(60) # 10hz
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
self._local_pose_cb)
self.local_pose = PoseStamped()
self.waypoint = PoseStamped()
self.waypoint.header.frame_id = 'map'
self.marker_pub = rospy.Publisher('/waypoint_marker',
Marker,
queue_size=10)
self.waypoint_marker = Marker()
self.waypoint_marker.type = Marker.CUBE
self.waypoint_marker.header.frame_id = 'map'
self.waypoint_marker.scale.x = 0.1
self.waypoint_marker.scale.y = 0.1
self.waypoint_marker.scale.z = 0.1
self.waypoint_marker.color.r = 0.1
self.waypoint_marker.color.g = 1
self.waypoint_marker.color.b = 0
self.waypoint_marker.color.a = 0.6
def __init__(self):
rospy.init_node('offboard_test', anonymous=True)
pose_pub = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
tag_pose = rospy.Subscriber('/tag_detections_pose',
PoseArray,
callback=self.tag_pose_cb)
mocap_sub = rospy.Subscriber('/mavros/local_position/pose',
PoseStamped,
callback=self.mocap_cb)
state_sub = rospy.Subscriber('/mavros/state',
State,
callback=self.state_cb)
rate = rospy.Rate(10) # Hz
rate.sleep()
self.des_pose = self.copy_pose(self.curr_pose)
current_x = self.curr_pose.pose.position.x
current_y = self.curr_pose.pose.position.y
del_val = 0.2
pre_z = 10
while not rospy.is_shutdown():
if self.isReadyToFly:
des_z = pre_z
if self.tagDetected:
if abs(self.x_cam) > 0.2:
des_y = current_y - del_val * self.x_cam
print "Correcting..", self.x_cam, self.y_cam, des_z
if abs(self.y_cam) > 0.2:
des_x = current_x - del_val * self.y_cam
print "Correcting..", self.x_cam, self.y_cam, des_z
if abs(self.x_cam) < 0.2 and abs(self.y_cam) < 0.2:
self.tagDetected = False
pre_z = des_z - 0.1 * des_z
if pre_z < 2:
mavros.set_namespace()
command.arming(False)
print "Arming", des_z
des_z = pre_z
print "Corrected", des_z
else:
des_y = current_y
des_x = current_x
print "Else", des_x, des_y, des_z
self.des_pose.pose.position.x = des_x
self.des_pose.pose.position.y = des_y
self.des_pose.pose.position.z = des_z
curr_x = self.curr_pose.pose.position.x
curr_y = self.curr_pose.pose.position.y
curr_z = self.curr_pose.pose.position.z
dist = math.sqrt((curr_x - des_x) * (curr_x - des_x) +
(curr_y - des_y) * (curr_y - des_y) +
(curr_z - des_z) * (curr_z - des_z))
if dist < self.distThreshold:
current_x = curr_x
current_y = curr_y
mavros.set_namespace()
command.arming(False)
print "HI"
# print dist, curr_x, curr_y, curr_z, self.waypointIndex
pose_pub.publish(self.des_pose)
rate.sleep()
#!/usr/bin/env python import mavros from mavros import command mavros.set_namespace() # arming command.arming(True) # takeoff command.takeoff(altitude=5, latitude=0, longitude=0, min_pitch=0, yaw=0) # disarming command.arming(False)
Xval_ep = []
Uval_ep = []
tval_ep = []
track_error = []
ctrl_effort = []
print('Starting episodic learning...')
for ep in range(Nep + 1):
# Run single landing with no perturbation for validation plots:
print("Executing trajectory with no perturbation noise...")
print("Resetting to initial point...")
handler.ctrl_history = []
handler.time_history = []
handler.pert_noise = 0.
command.arming(True)
go_waypoint.gopoint(np.array(p_init))
X, p_final, U, Upert, t = bintel.gotopoint(p_init,
p_final,
duration_low,
controller=handler)
#land()
X_val, Xd_val, U_val, _, t_val, ctrl_hist = handler.clean_data(
X, p_final, U, Upert, t, ctrl_hist=handler.ctrl_history)
handler.pert_noise = pert_noise
ctrl_history_ep.append(ctrl_hist)
handler.plot_control_ep()
# Run training loop:
X_w = []
def arm(self):
rospy.loginfo("Arming pixhawk...")
command.arming(True)
self.armed = True
def disarm(self):
rospy.loginfo("Disarming pixhawk...")
command.arming(False)
self.armed = False