def main():
# INITIALIZE ADAPTIVE NETWORK PARAMETERS:
N_INPUT = 3 # Number of input network
N_OUTPUT = 1 # Number of output network
INIT_NODE = 1 # Number of node(s) for initial structure
INIT_LAYER = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output)
N_WINDOW = 500 # Sliding Window Size
EVAL_WINDOW = 3 # Evaluation Window for layer Adaptation
DELTA = 0.05 # Confidence Level for layer Adaptation (0.05 = 95%)
ETA = 0.0001 # Minimum allowable value if divided by zero
FORGET_FACTOR = 0.98 # Forgetting Factor of Recursive Calculation
#EXP_DECAY = 1 # Learning Rate decay factor
#LEARNING_RATE = 0.00005 # Network optimization learning rate
LEARNING_RATE = 0.00002 / 2 # Network optimization learning rate
WEIGHT_DECAY = 0.000125 / 5 # Regularization weight decay factor
#WEIGHT_DECAY = 0.0 # Regularization weight decay factor
# INITIALIZE SYSTEM AND SIMULATION PARAMETERS:
IRIS_THRUST = 0.5629 # Nominal thrust for IRIS Quadrotor
RATE = 30.0 # Sampling Frequency (Hz)
# PID_GAIN_X = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-X
# PID_GAIN_Y = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-Y
PID_GAIN_X = [0.15, 0.0, 0.004] # PID Gain Parameter [KP,KI,KD] axis-X
PID_GAIN_Y = [0.15, 0.0, 0.004] # PID Gain Parameter [KP,KI,KD] axis-Y
#PID_GAIN_Z = [0.013,0.0004,0.2] # PID Gain Parameter [KP,KI,KD] axis-Z
PID_GAIN_Z = [0.013, 0.0, 0.2] # PID Gain Parameter [KP,KI,KD] axis-Z
SIM_TIME = 200 # Simulation time duration (s)
# Initial conditions of UAV system
xref = 0.0
yref = 0.0
zref = 2.0
interrx = 0.0
interry = 0.0
interrz = 0.0
errlastx = 0.0
errlasty = 0.0
errlastz = 0.0
runtime = 0.0
# Ignite the Evolving NN Controller
Xcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE)
Ycon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE)
Zcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE)
if INIT_LAYER > 2:
pdb.set_trace()
for i in range(INIT_LAYER - 2):
Xcon.addhiddenlayer()
Ycon.addhiddenlayer()
Zcon.addhiddenlayer()
dt = 1 / RATE
Xcon.dt = dt
Ycon.dt = dt
Zcon.dt = dt
Xcon.smcpar = PID_GAIN_X
Ycon.smcpar = PID_GAIN_Y
Zcon.smcpar = PID_GAIN_Z
# PX4 API object
modes = fcuModes() # Flight mode object
uav = uavCommand() # UAV command object
# Data Logger object
logData = dataLogger()
xconLog = dataLogger()
yconLog = dataLogger()
zconLog = dataLogger()
# Initiate node and subscriber
rospy.init_node('setpoint_node', anonymous=True)
rate = rospy.Rate(RATE) # ROS loop rate, [Hz]
# Subscribe to drone state
rospy.Subscriber('mavros/state', State, uav.stateCb)
# Subscribe to drone's local position
#rospy.Subscriber('mavros/local_position/pose', PoseStamped, uav.posCb)
rospy.Subscriber('mavros/mocap/pose', PoseStamped, uav.posCb)
# Setpoint publisher
att_sp_pub = rospy.Publisher('mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=10)
# Initiate Attitude messages
att = AttitudeTarget()
att.body_rate = Vector3()
att.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
att.thrust = IRIS_THRUST
att.type_mask = 7
# Arming the UAV --> no auto arming
text = colored("Ready for Arming..Press Enter to continue...",
'green',
attrs=['reverse', 'blink'])
raw_input(text)
while not uav.state.armed:
modes.setArm()
rate.sleep()
text = colored('Vehicle is arming..', 'yellow')
print(text)
# Switch to OFFBOARD after send few setpoint messages
text = colored("Vehicle Armed!! Press Enter to switch OFFBOARD...",
'green',
attrs=['reverse', 'blink'])
raw_input(text)
while not uav.state.armed:
modes.setArm()
rate.sleep()
text = colored('Vehicle is arming..', 'yellow')
print(text)
rate.sleep()
k = 0
while k < 10:
att_sp_pub.publish(att)
rate.sleep()
k = k + 1
modes.setOffboardMode()
text = colored('Now in OFFBOARD Mode..',
'blue',
attrs=['reverse', 'blink'])
print(text)
# ROS Main loop::
k = 0
while not rospy.is_shutdown():
start = time.time()
rate.sleep()
# Setpoint generator
if runtime <= 20:
xref = 0.0
yref = 0.0
zref = runtime * 1.5 / 20.0
if runtime > 20 and runtime <= 30:
xref = (runtime - 20) * 1.0 / 10.0
#xref = (runtime-20)* 5.0/10.0
yref = 0.0
zref = 1.5
if runtime > 30:
xref = 0.0 + 1.0 * math.cos(math.pi * (runtime - 30) / 20.0)
yref = 0.0 + 1.0 * math.sin(math.pi * (runtime - 30) / 20.0)
#xref = 0.0+5.0*math.cos(math.pi*(runtime-30)/60.0)
#yref = 0.0+5.0*math.sin(math.pi*(runtime-30)/60.0)
zref = 1.5
# Adjust the number of nodes in the latest layer (Network Width)
Xcon.adjustWidth(FORGET_FACTOR, N_WINDOW)
Ycon.adjustWidth(FORGET_FACTOR, N_WINDOW)
#Zcon.adjustWidth(FORGET_FACTOR,N_WINDOW)
# Adjust the number of layer (Network Depth)
Xcon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW)
Ycon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW)
#Zcon.adjustDepth(ETA,DELTA,N_WINDOW,EVAL_WINDOW)
# update current position
xpos = uav.local_pos.x
ypos = uav.local_pos.y
zpos = uav.local_pos.z
# calculate errors,delta errors, and integral errors
errx, derrx, interrx = Xcon.calculateError(xref, xpos)
erry, derry, interry = Ycon.calculateError(yref, ypos)
errz, derrz, interrz = Zcon.calculateError(zref, zpos)
# PID Controller equations
#theta_ref = 0.04*errx+0.0005*interrx+0.01*derrx
#phi_ref = 0.04*erry+0.0005*interry+0.01*derry
# vx = PID_GAIN_X[0]*errx+PID_GAIN_X[1]*interrx+PID_GAIN_X[2]*derrx
# vy = PID_GAIN_Y[0]*erry+PID_GAIN_Y[1]*interry+PID_GAIN_Y[2]*derry
#thrust_ref = IRIS_THRUST+PID_GAIN_Z[0]*errz+PID_GAIN_Z[1]*interrz+PID_GAIN_Z[2]*derrz
# SMC + NN controller
vx = Xcon.controlUpdate(xref) # Velocity X
vy = Ycon.controlUpdate(yref) # Velocity Y
thz = Zcon.controlUpdate(zref) # Additional Thrust Z
euler = euler_from_quaternion(uav.q)
psi = euler[2]
phi_ref, theta_ref = transform_control_signal(vx, vy, psi)
thrust_ref = IRIS_THRUST + thz
# control signal limiter
#phi_ref = limiter(-1*phi_ref,0.2)
phi_ref = limiter(-1 * phi_ref, 0.25)
theta_ref = limiter(-1 * theta_ref, 0.25)
att.thrust = limiter(thrust_ref, 1)
# Phi, Theta and Psi quaternion transformation
att.orientation = Quaternion(
*quaternion_from_euler(phi_ref, theta_ref, 0.0))
# Publish the control signal
att_sp_pub.publish(att)
# NN Learning stage
Xcon.optimize(LEARNING_RATE, WEIGHT_DECAY)
Ycon.optimize(LEARNING_RATE, WEIGHT_DECAY)
Zcon.optimize(LEARNING_RATE, WEIGHT_DECAY)
# Print all states
print('Xr,Yr,Zr = ', xref, yref, zref)
print('X, Y, Z = ', uav.local_pos.x, uav.local_pos.y,
uav.local_pos.z)
print('phi, theta = ', phi_ref, theta_ref)
print('att angles = ', euler)
print('Thrust = ', att.thrust)
print('Nodes X Y Z = ', Xcon.nNodes, Ycon.nNodes, Zcon.nNodes)
print('Layer X Y Z = ', Xcon.nLayer, Ycon.nLayer, Zcon.nLayer)
k += 1
runtime = k * dt
# Append Data Logger
logData.appendStateData(runtime, xref, yref, zref, xpos, ypos, zpos,
phi_ref, theta_ref, thrust_ref)
xconLog.appendControlData(runtime, Xcon.us, Xcon.un, Xcon.u,
Xcon.nNodes)
yconLog.appendControlData(runtime, Ycon.us, Ycon.un, Ycon.u,
Ycon.nNodes)
zconLog.appendControlData(runtime, Zcon.us, Zcon.un, Zcon.u,
Zcon.nNodes)
print('Runtime = ', runtime)
print('Exec time = ', time.time() - start)
print('')
# Break condition
if runtime > SIM_TIME:
modes.setRTLMode()
text = colored('Auto landing mode now..',
'blue',
attrs=['reverse', 'blink'])
print(text)
text = colored('Now saving all logged data..',
'yellow',
attrs=['reverse', 'blink'])
print(text)
logData.plotFigure()
xconLog.plotControlData()
yconLog.plotControlData()
zconLog.plotControlData()
text = colored('Mission Complete!!',
'green',
attrs=['reverse', 'blink'])
print(text)
break
def main(args):
global list_current_position, list_current_velocity, current_position, usingvelocitycontrol, usingpositioncontrol, xvel, yvel, state, cur_pose, list_error, target
global global_obs_detected, list_velocity_angle, vController
par.CurrentIteration = 1
if (len(args) > 1):
uav_ID = str(args[1])
else:
uav_ID = "1"
idx = int(uav_ID) - 1
rospy.init_node("control_uav_" + uav_ID)
print "UAV" + uav_ID
vController = VelocityController()
try:
rospy.wait_for_service("/uav" + uav_ID + "/mavros/cmd/arming")
rospy.wait_for_service("/uav" + uav_ID + "/mavros/set_mode")
except rospy.ROSException:
fail("failed to connect to service")
state_sub = rospy.Subscriber("/uav" + uav_ID + "/mavros/state",
State,
queue_size=10,
callback=state_cb)
local_vel_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_velocity/cmd_vel",
TwistStamped,
queue_size=10)
local_pos_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
local_pos_target = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_raw/local",
PositionTarget,
queue_size=10)
atittude_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_raw/attitude",
AttitudeTarget,
queue_size=10)
thr_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_attitude/att_throttle",
Float64,
queue_size=10)
Astar_grid_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_grid",
GridCells,
queue_size=10)
Astar_path_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_path",
GridCells,
queue_size=10)
arming_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/cmd/arming",
CommandBool)
set_mode_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/set_mode",
SetMode)
for i in range(4):
#velocity_sub = rospy.Subscriber("/uav"+repr(i+1)+"/mavros/local_position/velocity",TwistStamped,queue_size = 10,callback=velocity_cb, callback_args=i)
position_sub = rospy.Subscriber("/uav" + repr(i + 1) +
"/mavros/local_position/pose",
PoseStamped,
queue_size=10,
callback=position_cb,
callback_args=(i, int(uav_ID) - 1))
scan_lidar_sub = rospy.Subscriber("/scan",
LaserScan,
queue_size=10,
callback=scan_lidar_cb,
callback_args=uav_ID)
br = tf.TransformBroadcaster()
r = rospy.Rate(10)
print "TRY TO CONNECT"
while ((not rospy.is_shutdown()) and (not current_state.connected)):
#rospy.spinOnce()
r.sleep()
#print(current_state.connected.__class__)
rospy.loginfo("CURRENT STATE CONNECTED")
poses = PoseStamped()
#poses.pose = Pose()
#poses.pose.position = Point()
target = Pose()
if (idx == 0):
target.position.x = 0
target.position.y = 0
if (idx == 1):
target.position.x = 0
target.position.y = 0
if (idx == 2):
target.position.x = 0
target.position.y = 0
if (idx == 3):
target.position.x = 4
target.position.y = 4
target.position.z = 10
poses.pose.position.x = target.position.x
poses.pose.position.y = target.position.y
poses.pose.position.z = target.position.z
q = quaternion_from_euler(0, 0, 45 * np.pi / 180.0)
poses.pose.orientation.x = q[0]
poses.pose.orientation.y = q[1]
poses.pose.orientation.z = q[2]
poses.pose.orientation.w = q[3]
i = 100
#while((not rospy.is_shutdown()) and (i>0)):
# local_pos_pub.publish(poses)
# i = i-1
rviz_visualization_start = False
last_request = rospy.Time.now()
count = 0
if (idx == 1):
target.position.x = 28
target.position.y = 28
if (idx == 2):
target.position.x = 0
target.position.y = 17
#thread1 = Thread(target = key_function)
#thread1.start()
style.use('fivethirtyeight')
thread2 = Thread(target=plot_error_live)
thread2.start()
thread3 = Thread(target=calculate_and_publish_wall,
args=(Astar_grid_pub, Astar_path_pub, idx))
thread3.start()
#file_error_pos_x = open(dir_path+'/txt/error_pos_x.txt','w')
#file_error_pos_y = open(dir_path+'/txt/error_pos_y.txt','w')
#file_error_pos_z = open(dir_path+'/txt/error_pos_z.txt','w')
error_time = 0
print poses
uav_color = [255, 0, 0, 255]
topic = 'uav_marker_' + uav_ID
uav_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10)
uav_marker = Marker()
uav_marker.header.frame_id = "world"
uav_marker.type = uav_marker.SPHERE
uav_marker.action = uav_marker.ADD
uav_marker.scale.x = par.ws_model['robot_radius'] * 3
uav_marker.scale.y = par.ws_model['robot_radius'] * 3
uav_marker.scale.z = 0.005 * par.RealScale
uav_marker.color.r = float(uav_color[0]) / 255
uav_marker.color.g = float(uav_color[1]) / 255
uav_marker.color.b = float(uav_color[2]) / 255
uav_marker.color.a = float(uav_color[3]) / 255
uav_marker.pose.orientation.w = 1.0
uav_marker.pose.position.x = 0 #init_pos[0]
uav_marker.pose.position.y = 0 #init_pos[1]
uav_marker.pose.position.z = 0 #init_pos[2]
uav_marker_publisher.publish(uav_marker)
uav_color = [255, 255, 255, 255]
topic = 'uav_goal_marker_' + uav_ID
uav_goal_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10)
uav_goal_marker = Marker()
uav_goal_marker.header.frame_id = "world"
uav_goal_marker.type = uav_goal_marker.SPHERE
uav_goal_marker.action = uav_goal_marker.ADD
uav_goal_marker.scale.x = par.ws_model['robot_radius'] * 2
uav_goal_marker.scale.y = par.ws_model['robot_radius'] * 2
uav_goal_marker.scale.z = 0.005 * par.RealScale
uav_goal_marker.color.r = float(uav_color[0]) / 255
uav_goal_marker.color.g = float(uav_color[1]) / 255
uav_goal_marker.color.b = float(uav_color[2]) / 255
uav_goal_marker.color.a = float(uav_color[3]) / 255
uav_goal_marker.pose.orientation.w = 1.0
uav_goal_marker.pose.position.x = 0 #init_pos[0]
uav_goal_marker.pose.position.y = 0 #init_pos[1]
uav_goal_marker.pose.position.z = 0 #init_pos[2]
uav_goal_marker_publisher.publish(uav_goal_marker)
uav_goal_marker = update_uav_marker(
uav_goal_marker,
(target.position.x, target.position.y, target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
last_request_rviz = rospy.Time.now()
last_HRVO_request = rospy.Time.now()
while (not rospy.is_shutdown()):
if (rviz_visualization_start and
(rospy.Time.now() - last_request_rviz > rospy.Duration(0.2))):
publish_uav_position_rviz(
br, list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z, uav_ID)
uav_marker = update_uav_marker(
uav_marker, (list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z, 1.0))
uav_marker_publisher.publish(uav_marker)
last_request_rviz = rospy.Time.now()
if ((not rviz_visualization_start)
and (current_state.mode != 'OFFBOARD')
and (rospy.Time.now() - last_request > rospy.Duration(1.0))):
offb_set_mode = set_mode_client(0, 'OFFBOARD')
if (offb_set_mode.mode_sent):
rospy.loginfo("OFFBOARD ENABLED")
last_request = rospy.Time.now()
else:
if ((not current_state.armed) and
(rospy.Time.now() - last_request > rospy.Duration(1.0))):
arm_cmd = arming_client(True)
if (arm_cmd.success):
rospy.loginfo("VEHICLE ARMED")
rviz_visualization_start = True
last_request = rospy.Time.now()
#print distance3D(cur_pose.pose,target)
if (distance3D(cur_pose.pose, target) <
0.5) and (not usingvelocitycontrol):
print "sampai"
usingvelocitycontrol = True
usingpositioncontrol = False
target = Pose()
if (idx == 0):
target.position.x = 28
target.position.y = 28
if (idx == 1):
target.position.x = 0
target.position.y = 0
if (idx == 2):
target.position.x = 21
target.position.y = 14
if (idx == 3):
target.position.x = 0
target.position.y = 0
target.position.z = 10
print target
psi_target = 45 * np.pi / 180.0 #np.pi/180.0 #np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x)
diagram4 = GridWithWeights(par.NumberOfPoints, par.NumberOfPoints)
diagram4.walls = []
for i in range(par.NumberOfPoints
): # berikan wall pada algoritma pathfinding A*
for j in range(par.NumberOfPoints):
if (par.ObstacleRegion[j, i] == 0):
diagram4.walls.append((i, j))
for i in range(par.NumberOfPoints):
for j in range(par.NumberOfPoints):
if (par.ObstacleRegionWithClearence[j][i] == 0):
diagram4.weights.update({(i, j): par.UniversalCost})
goal_pos = (target.position.x / par.RealScale,
target.position.y / par.RealScale)
start_pos = (cur_pose.pose.position.x / par.RealScale,
cur_pose.pose.position.y / par.RealScale)
pathx, pathy = Astar_version1(par, start_pos, goal_pos, diagram4)
print pathx
print pathy
vController.setHeadingTarget(deg2rad(90.0)) #45.0))
target.position.x = pathx[0] * par.RealScale
target.position.y = pathy[0] * par.RealScale
uav_goal_marker = update_uav_marker(
uav_goal_marker,
(target.position.x, target.position.y, target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
vController.setTarget(target)
vController.setPIDGainX(1, 0.0, 0.0)
vController.setPIDGainY(1, 0.0, 0.0)
vController.setPIDGainZ(1, 0, 0)
vController.setPIDGainPHI(1, 0.0, 0.0)
vController.setPIDGainTHETA(1, 0.0, 0.0)
vController.setPIDGainPSI(1, 0.0, 0.0)
if (usingpositioncontrol):
print "kontrol posisi"
poses.pose.position.x = 29
poses.pose.position.y = 29
poses.pose.position.z = 10
local_pos_pub.publish(poses)
elif (usingvelocitycontrol):
if (distance2D(cur_pose.pose, target) < 1.5):
if (len(pathx) > 1):
del pathx[0]
del pathy[0]
target.position.x = pathx[0] * par.RealScale
target.position.y = pathy[0] * par.RealScale
uav_goal_marker = update_uav_marker(
uav_goal_marker, (target.position.x, target.position.y,
target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
print target
vController.setTarget(target)
if (rospy.Time.now() - last_HRVO_request > rospy.Duration(0.05)):
last_HRVO_request = rospy.Time.now()
header.stamp = rospy.Time.now()
des_vel = vController.update(cur_pose)
current_agent_pose = (
list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z)
current_agent_vel = (list_current_velocity[idx].twist.linear.x,
list_current_velocity[idx].twist.linear.y)
current_neighbor_pose = []
for i in range(par.NumberOfAgents):
if (i != idx):
current_neighbor_pose.append(
(list_current_position[i].pose.position.x,
list_current_position[i].pose.position.y))
current_neighbor_vel = []
for i in range(par.NumberOfAgents):
if (i != idx):
current_neighbor_vel.append(
(list_current_velocity[i].twist.linear.x,
list_current_velocity[i].twist.linear.y))
V_des = (des_vel.twist.linear.x, des_vel.twist.linear.y)
quat_arr = np.array([
list_current_position[idx].pose.orientation.x,
list_current_position[idx].pose.orientation.y,
list_current_position[idx].pose.orientation.z,
list_current_position[idx].pose.orientation.w
])
att = euler_from_quaternion(quat_arr, 'sxyz')
#V = locplan.RVO_update_single(current_agent_pose, current_neighbor_pose, current_agent_vel, current_neighbor_vel, V_des, par,list_velocity_angle,list_distance_obs,att[2])
V, RVO_BA_all = locplan.RVO_update_single_static(
current_agent_pose, current_neighbor_pose,
current_agent_vel, current_neighbor_vel, V_des, par)
V_msg = des_vel
V_msg.twist.linear.x = V[0]
V_msg.twist.linear.y = V[1]
local_vel_pub.publish(V_msg)
goal = (target.position.x, target.position.y,
target.position.z)
x = current_position
list_error[0].append(error_time)
list_error[1].append(goal[0] - x[0])
list_error[2].append(goal[1] - x[1])
list_error[3].append(goal[2] - x[2])
error_time = error_time + 1
k = 0.1
vx = k * (goal[0] - x[0])
vy = k * (goal[1] - x[1])
vz = k * (goal[2] - x[2])
postar = PositionTarget()
postar.header = header
postar.coordinate_frame = PositionTarget().FRAME_LOCAL_NED
p = PositionTarget().IGNORE_PX | PositionTarget(
).IGNORE_PY | PositionTarget().IGNORE_PZ
a = PositionTarget().IGNORE_AFX | PositionTarget(
).IGNORE_AFY | PositionTarget().IGNORE_AFZ
v = PositionTarget().IGNORE_VX | PositionTarget(
).IGNORE_VY | PositionTarget().IGNORE_VZ
y = PositionTarget().IGNORE_YAW | PositionTarget(
).IGNORE_YAW_RATE
f = PositionTarget().FORCE
postar.type_mask = a | y | p | f #| PositionTarget().IGNORE_YAW | PositionTarget().IGNORE_YAW_RATE | PositionTarget().FORCE
postar.velocity.x = vx
postar.velocity.y = vy
postar.velocity.z = vz
postar.position.x = goal[0]
postar.position.y = goal[1]
postar.position.z = goal[2]
vel_msg = TwistStamped()
vel_msg.header = header
vel_msg.twist.linear.x = xvel
vel_msg.twist.linear.y = yvel
vel_msg.twist.linear.z = 0.0
vel_msg.twist.angular.x = 0.0
vel_msg.twist.angular.y = 0.0
vel_msg.twist.angular.z = 0.0
q = quaternion_from_euler(0, 0, 0.2)
att = PoseStamped()
att.pose.orientation.x = q[0]
att.pose.orientation.y = q[1]
att.pose.orientation.z = q[2]
att.pose.orientation.w = q[3]
att.pose.position.x = 0.0
att.pose.position.y = 0.0
att.pose.position.z = 2.0
cmd_thr = Float64()
cmd_thr.data = 0.3
att_raw = AttitudeTarget()
att_raw.header = Header()
att_raw.body_rate = Vector3()
att_raw.thrust = 0.7 #Float64()
att_raw.header.stamp = rospy.Time.now()
att_raw.header.frame_id = "fcu"
att_raw.type_mask = 71
att_raw.orientation = Quaternion(
*quaternion_from_euler(0, 0, 0.2))
else:
local_pos_pub.publish(poses)
count = count + 1
r.sleep()
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
from time import sleep
import rospy
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Vector3
from mavros_msgs.msg import Thrust
from mavros_msgs.msg import AttitudeTarget
node = rospy.init_node("thrust_test")
rate = rospy.Rate(20) # Hz
pub = rospy.Publisher('/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=1)
print("looping!")
while True:
msg = AttitudeTarget()
msg.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
msg.body_rate = Vector3(x=0.0, y=0.0, z=0.0)
msg.thrust = -0.001
pub.publish(msg)
rate.sleep()
print("Sending thrust [%f]!" % msg.thrust)
