def set_ang_vel_thrust(self, ang_vel, thrust, freq):
"""
Offboard method that sends angular velocity and thrust references to the PX4 autopilot of the the drone.
Makes convertions from the local NED frame adopted for the ISR Flying Arena to the local ENU frame used by ROS.
Converts the thrust from newtons to a normalized value between 0 and 1 through the mathematical expression of the thrust curve of the vehicle.
Parameters
----------
ang_vel : np.array of floats with shape (3,1)
Desired angular velocity for the drone.
thrust : float
Desired thrust value in newtons.
freq : float
Topic publishing frequency, in Hz.
"""
pub = rospy.Publisher(self.info.drone_ns +
'/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=1)
msg = AttitudeTarget()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.type_mask = 128
msg.body_rate.x, msg.body_rate.y, msg.body_rate.z = ned_to_enu(ang_vel)
msg.thrust = normalize_thrust(thrust, self.info.thrust_curve,
norm(self.ekf.vel))
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
def set_att_thrust(self, att, att_type, thrust, freq):
"""
Offboard method that sends attitude and thrust references to the PX4 autopilot of the the vehicle.
Makes convertions from the local NED frame adopted for the ISR Flying Arena to the local ENU frame used by ROS.
Converts the thrust from newtons to a normalized value between 0 and 1 through mathematical expression of the thrust curve of the vehicle.
Parameters
----------
att : np.array of floats with shape (3,1) or with shape (4,1)
Desired attitude for the vehicle, expressed in euler angles or in a quaternion.
att_type : str
Must be equal to either 'euler' or 'quaternion'. Specifies the format of the desired attitude.
thrust : float
Desired thrust value in newtons.
freq : float
Topic publishing frequency, in Hz.
"""
pub = rospy.Publisher(self.info.drone_ns +
'/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=1)
msg = AttitudeTarget()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
if att_type == 'euler':
msg.orientation = Quaternion(*quaternion_from_euler(
*ned_to_enu(att)))
elif att_type == 'quaternion':
msg.orientation = Quaternion(*SI_quaternion_to_ROS_quaternion(att))
msg.thrust = normalize_thrust(thrust, self.info.thrust_curve,
norm(self.ekf.vel))
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
def att_controller(self, r=0, p=0, y=0, z=2):
if hasattr(self, "roll_zero"):
r += self.roll_zero
if hasattr(self, "pitch_zero"):
p + self.pitch_zero
att = AttitudeTarget()
q = tf.transformations.quaternion_from_euler(r, p, y)
att.orientation.x = q[0]
att.orientation.y = q[1]
att.orientation.z = q[2]
att.orientation.w = q[3]
rx, ry, rz, r, p, y = self.parse_local_position(mode="e")
vx, vy, vz, wx, wy, wz = self.parse_velocity()
try:
param = 1.0 / (np.cos(r) * np.cos(p))
except Exception as e:
param = 1
finally:
vzt = self.pidz.step(z - rz)
att.thrust = (self.pidzv.step(vzt - vz) + 0.57) * param
# for plot only..
self.thrust = att.thrust
self.pidzv_out = self.pidzv.out
self.pidz_out = self.pidz.out
att.header = Header()
att.header.frame_id = "map"
att.header.stamp = rospy.Time.now()
self.att_setpoint_pub.publish(att)
def main1():
global setpoint
# thrust_pub = rospy.Publisher('/mavros/setpoint_attitude/thrust',Thrust,queue_size=10)
# thrust = Thrust()
attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude',AttitudeTarget,queue_size=10)
point_pub = rospy.Publisher('/waypoint',PoseStamped,queue_size=10)
attitude = AttitudeTarget()
attitude.type_mask = 3
attitude.header = msg.header
# attitude.header.frame_id = 'FRAME_LOCAL_NED'
# quaternion = tf.transformations.quaternion_from_euler(msg.roll,msg.pitch, 0)
# attitude.orientation.x = quaternion[0]
# attitude.orientation.y = quaternion[1]
# attitude.orientation.z = quaternion[2]
# attitude.orientation.w = quaternion[3]
attitude.orientation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(msg.roll, msg.pitch, 0))
attitude.body_rate.z = msg.yaw_rate
t = msg.thrust.z/100
if t>1:
t=1
elif t<-1:
t=-1
attitude.thrust = (t+1)/2
attitude_pub.publish(attitude)
point_pub.publish(setpoint)
def track_and_trajectory_dock_control(self,des_trajectory_point, curr_odom, yaw_des, time_now, docker):
if docker == self.num:
pass
else:
return
Rot_des = np.matrix(np.zeros((4, 4)))
Rot_des[3, 3] = 1
attitude_des = AttitudeTarget()
thrust = Thrust()
des_acc = Vector3()
if self.A_init_flag:
thrust.header = curr_odom.header
attitude_des.header = curr_odom.header
dt = time_now - self.time_prev
ex = des_trajectory_point.position.x - curr_odom.position.x
ey = des_trajectory_point.position.y - curr_odom.position.y
ez = des_trajectory_point.position.z - curr_odom.position.z
evx = des_trajectory_point.velocity.x - curr_odom.velocity.x
evy = des_trajectory_point.velocity.y - curr_odom.velocity.y
evz = des_trajectory_point.velocity.z - curr_odom.velocity.z
self.intx = self.intx + ex * dt
self.inty = self.inty + ey * dt
self.intz = self.intz + ez * dt
des_acc.x = des_trajectory_point.acceleration.x + self.Kp_track.x * ex + self.Kd_track.x * evx + self.Ki_track.x * self.intx
des_acc.y = des_trajectory_point.acceleration.y + self.Kp_track.y * ey + self.Kd_track.y * evy + self.Ki_track.y * self.inty
des_acc.z = des_trajectory_point.acceleration.z + self.Kp_track.z * ez + self.Kd_track.z * evz + self.Ki_track.z * self.intz
R_waitmod_w = np.matrix([[np.cos(self.tag_angle),-np.sin(self.tag_angle),0],[np.sin(self.tag_angle),np.cos(self.tag_angle),0],[0,0,1]]).transpose()
curr_quaternion = [curr_odom.orientation.x, curr_odom.orientation.y,
curr_odom.orientation.z, curr_odom.orientation.w]
H_curr = tf.transformations.quaternion_matrix(curr_quaternion)
Rot_curr = np.matrix(H_curr[:3, :3])
des_acc_relative = R_waitmod_w*np.matrix([[des_acc.x],
[des_acc.y],
[des_acc.z]])
Force_des = np.matrix([[0], [0], [self.m * self.g]])+self.m * des_acc_relative
Force_des_body = Rot_curr * Force_des
thrust.thrust = Force_des_body[2]
Rot_des[:3, 2] = np.matrix(Force_des / LA.norm(Force_des))
x_body_in_world = np.matrix([[np.cos(des_trajectory_point.yaw)], [np.sin(des_trajectory_point.yaw)], [0]])
y_body_in_world = np.cross(Rot_des[:3, 2], x_body_in_world, axis=0)
Rot_des[:3, 1] = np.matrix(y_body_in_world / LA.norm(y_body_in_world))
Rot_des[:3, 0] = np.matrix(np.cross(Rot_des[:3, 1], Rot_des[:3, 2], axis=0))
quad_des = tf.transformations.quaternion_from_matrix(Rot_des)
attitude_des.orientation.x = quad_des[0]
attitude_des.orientation.y = quad_des[1]
attitude_des.orientation.z = quad_des[2]
attitude_des.orientation.w = quad_des[3]
attitude_des.thrust = thrust.thrust
self.mavros_attitude_pub.publish(attitude_des)
self.mavros_thrust_pub.publish(thrust)
self.time_prev = time_now
def pos_control(self,des_trajectory_point,curr_pose, curr_vel, yaw_des, time_now):
Rot_des = np.matrix(np.zeros((4, 4)))
Rot_des[3, 3] = 1
attitude_des = AttitudeTarget()
thrust = Thrust()
des_acc = Vector3()
if self.A_init_flag:
thrust.header = curr_pose.header
attitude_des.header = curr_pose.header
dt = time_now - self.time_prev
ex = des_trajectory_point.position.x - curr_pose.pose.position.x
ey = des_trajectory_point.position.y - curr_pose.pose.position.y
ez = des_trajectory_point.position.z - curr_pose.pose.position.z
evx = des_trajectory_point.velocity.x - curr_vel.twist.linear.x
evy = des_trajectory_point.velocity.y - curr_vel.twist.linear.y
evz = des_trajectory_point.velocity.z - curr_vel.twist.linear.z
self.intx = self.intx + ex * dt
self.inty = self.inty + ey * dt
self.intz = self.intz + ez * dt
des_acc.x = des_trajectory_point.acceleration.x + self.Kp.x * ex + self.Kd.x * evx + self.ki.x * self.intx
des_acc.y = des_trajectory_point.acceleration.y + self.Kp.y * ey + self.Kd.y * evy + self.ki.y * self.inty
des_acc.z = des_trajectory_point.acceleration.z + self.Kp.z * ez + self.Kd.z * evz + self.ki.z * self.intz
curr_quaternion = [curr_pose.pose.orientation.x, curr_pose.pose.orientation.y,
curr_pose.pose.orientation.z, curr_pose.pose.orientation.w]
H_curr = tf.transformations.quaternion_matrix(curr_quaternion)
Rot_curr = np.matrix(H_curr[:3, :3])
Force_des = np.matrix([[0], [0], [self.m * self.g]])+self.m * np.matrix([[des_acc.x], [des_acc.y], [des_acc.z]])
Force_des_body = Rot_curr * Force_des
thrust.thrust = Force_des_body[2]
Rot_des[:3, 2] = np.matrix(Force_des / LA.norm(Force_des))
x_body_in_world = np.matrix([[np.cos(des_trajectory_point.yaw)], [np.sin(des_trajectory_point.yaw)], [0]])
y_body_in_world = np.cross(Rot_des[:3, 2], x_body_in_world, axis=0)
Rot_des[:3, 1] = np.matrix(y_body_in_world / LA.norm(y_body_in_world))
Rot_des[:3, 0] = np.matrix(np.cross(Rot_des[:3, 1], Rot_des[:3, 2], axis=0))
quad_des = tf.transformations.quaternion_from_matrix(Rot_des)
attitude_des.orientation.x = quad_des[0]
attitude_des.orientation.y = quad_des[1]
attitude_des.orientation.z = quad_des[2]
attitude_des.orientation.w = quad_des[3]
attitude_des.thrust = thrust.thrust
self.mavros_attitude_pub.publish(attitude_des)
self.mavros_thrust_pub.publish(thrust)
self.time_prev = time_now
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()
