def construct_target(self,
x=0,
y=0,
z=0,
vx=0,
vy=0,
vz=0,
afx=0,
afy=0,
afz=0,
yaw=0,
yaw_rate=0):
target_raw_pose = PositionTarget()
target_raw_pose.coordinate_frame = self.coordinate_frame
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.velocity.x = vx
target_raw_pose.velocity.y = vy
target_raw_pose.velocity.z = vz
target_raw_pose.acceleration_or_force.x = afx
target_raw_pose.acceleration_or_force.y = afy
target_raw_pose.acceleration_or_force.z = afz
if self.is_mavros_yaw_different: # rotate 90 degrees
target_raw_pose.yaw = (yaw + 3 * math.pi / 2) if (
yaw - math.pi / 2 < -math.pi) else (yaw - math.pi / 2)
target_raw_pose.position.x = y
target_raw_pose.position.y = -x
else:
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
if (self.motion_type == 0):
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW_RATE
if (self.motion_type == 1):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW
if (self.motion_type == 2):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_YAW
return target_raw_pose
def construct_target(self, x, y, z, yaw, yaw_rate=1):
"""
This is a function that will constuct a valid target message for the drone
:param x: Defining the target function in terms of x coordinates(Target)
:param y: Defining the target function in terms of y coordinates(Target)
:param z: Defining the target function in terms of z coordinates(Target)
:param yaw: yaw movement to move towards target
:param yaw_rate: Yaw rate is 1 ms
:rtype: target_raw_pose
:return: Target Position
"""
target_raw_pose = PositionTarget()
target_raw_pose.header.stamp = rospy.Time.now()
target_raw_pose.coordinate_frame = 9
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.FORCE
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
return target_raw_pose
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, yaw=0):
target_raw_pose = PositionTarget()
target_raw_pose.coordinate_frame = self.coordinate_frame
if self.coordinate_frame == 1:
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.velocity.x = vx
target_raw_pose.velocity.y = vy
target_raw_pose.velocity.z = vz
else:
target_raw_pose.position.x = -y
target_raw_pose.position.y = x
target_raw_pose.position.z = z
target_raw_pose.velocity.x = -vy
target_raw_pose.velocity.y = vx
target_raw_pose.velocity.z = vz
target_raw_pose.yaw = yaw
if (self.motion_type == 0):
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_YAW_RATE
if (self.motion_type == 1):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW_RATE
return target_raw_pose
def send_waypoint_command(self):
if self.mode_flag == 'mavros':
waypoint_command_msg = PositionTarget()
waypoint_command_msg.header.stamp = rospy.get_rostime()
waypoint_command_msg.coordinate_frame = waypoint_command_msg.FRAME_LOCAL_NED
waypoint_command_msg.type_mask = self.ned_pos_mask
waypoint_command_msg.position.x = self.wp_E # E
waypoint_command_msg.position.y = self.wp_N # N
waypoint_command_msg.position.z = self.rendezvous_height # U
# This converts an NED heading command into an ENU heading command
yaw = -self.heading_command + np.radians(90.0)
while yaw > np.radians(180.0):
yaw = yaw - np.radians(360.0)
while yaw < np.radians(-180.0):
yaw = yaw + np.radians(360.0)
waypoint_command_msg.yaw = yaw
# Publish.
self.command_pub_mavros.publish(waypoint_command_msg)
else:
wp_command_msg = Command()
wp_command_msg.x = self.wp_N
wp_command_msg.y = self.wp_E
wp_command_msg.F = -self.rendezvous_height
wp_command_msg.z = self.heading_command
wp_command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE
# Publish.
self.command_pub_roscopter.publish(wp_command_msg)
def construct_target(self,
x,
y,
z,
yaw,
x_vel=0.2,
y_vel=0.2,
z_vel=0.2,
yaw_rate=1):
target_raw_pose = PositionTarget(
) # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html
target_raw_pose.header.stamp = rospy.Time.now()
target_raw_pose.coordinate_frame = 9
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.velocity.x = x_vel
target_raw_pose.velocity.y = y_vel
target_raw_pose.velocity.z = z_vel
target_raw_pose.type_mask = PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.FORCE
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
return target_raw_pose
def set_vel_yaw(self, vel, yaw, freq):
"""
Offboard method that sends velocity and yaw 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.
Parameters
----------
vel : np.array of floats with shape (3,1)
Desired linear velocity for drone, in local NED coordinates.
yaw : float
Desired yaw for the vehicle, in radians.
freq : float
Topic publishing frequency, in Hz.
"""
pub = rospy.Publisher(self.info.drone_ns +
'/mavros/setpoint_raw/local',
PositionTarget,
queue_size=1)
msg = PositionTarget()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.coordinate_frame = 1
msg.type_mask = 3015
msg.velocity.x, msg.velocity.y, msg.velocity.z = ned_to_enu(vel)
msg.yaw = -yaw
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
def Aruco_marker_detector(self):
""" Use the build in python library to detect the aruco marker and its coordinates """
img = self.frame.copy()
# Detect the markers in the image
markerCorners, markerIds, rejectedCandidates = cv.aruco.detectMarkers(img, self.dictionary, parameters=self.parameters)
# Get the transformation matrix to the marker
if markerCorners:
markerSize = 2.0
axisLength = 3.0
rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(markerCorners, markerSize, self.K, self.distCoeffs)
# Draw the axis on the marker
frame_out = cv.aruco.drawAxis( img, self.K, self.distCoeffs, rvecs, tvecs, axisLength)
rotMat = tr.euler_matrix(np.pi / 2.0, 0, 0)
rotMat = rotMat[0:3, 0:3]
tvecs = np.matmul(rotMat, tvecs[0][0].T)
rotMat, _ = cv.Rodrigues(rvecs)
eul = tr.euler_from_matrix(rotMat)
yaw_angle = self.pos[3] - eul[2]
# Publish the position of the marker
marker_pos = PT()
marker_pos.position.x = tvecs[0]
marker_pos.position.y = - tvecs[2]
marker_pos.position.z = tvecs[1]
marker_pos.yaw = eul[2] * np.pi / 180.0
self.aruco_marker_pos_pub.publish(marker_pos)
# Publish the image with the detected marker
self.aruco_marker_img_pub.publish(self.bridge.cv2_to_imgmsg(frame_out, "bgr8"))
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0):
target_raw_pose = PositionTarget()
target_raw_pose.coordinate_frame = self.coordinate_frame
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.velocity.x = vx
target_raw_pose.velocity.y = vy
target_raw_pose.velocity.z = vz
target_raw_pose.acceleration_or_force.x = afx
target_raw_pose.acceleration_or_force.y = afy
target_raw_pose.acceleration_or_force.z = afz
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
if(self.motion_type == 0):
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW_RATE
if(self.motion_type == 1):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW
if(self.motion_type == 2):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_YAW
return target_raw_pose
def cmdvel2PosTarget(vel):
'''
Translates from JderobotTypes CMDVel to ROS Twist.
@param vel: JderobotTypes CMDVel to translate
@type img: JdeRobotTypes.CMDVel
@return a Twist translated from vel
'''
msg = PositionTarget(header=Header(stamp=rospy.Time.now(), frame_id=''), )
msg.coordinate_frame = 8
msg.type_mask = 1475
msg.position.x = vel.px
msg.position.y = vel.py
msg.position.z = vel.pz
msg.velocity.x = vel.vx
msg.velocity.y = vel.vy
msg.velocity.z = vel.vz
msg.acceleration_or_force.x = vel.ax
msg.acceleration_or_force.y = vel.ay
msg.acceleration_or_force.z = vel.az
msg.yaw = vel.yaw
msg.yaw_rate = vel.yaw_rate
return msg
def set_pose(self, x=0, y=0, z=2, yaw=0, BODY_OFF_SET_ENU=True):
pose = PositionTarget()
pose.header.stamp = rospy.Time.now()
if BODY_OFF_SET_ENU:
pose.header.frame_id = 'frame.body'
pose.position.x = x
pose.position.y = y
pose.position.z = z
pose.yaw = yaw
else:
pose.header.frame_id = 'frame.local_enu'
pose.position.x = x
pose.position.y = y
pose.position.z = z
pose.yaw = yaw
return pose
def Intarget_local():
set_target_local = PositionTarget()
set_target_local.type_mask = 0b100111111000
set_target_local.coordinate_frame = 1
set_target_local.position.x = 10
set_target_local.position.y = 10
set_target_local.position.z = 10
set_target_local.yaw = 0
return set_target_local
def altitude_target(altitude, yaw=0.0, frame_id='world'):
pt = PositionTarget()
pt.header.stamp = rospy.Time.now()
pt.header.frame_id = frame_id
pt.coordinate_frame = 1
pt.type_mask = POSITION_MASK
pt.position.z = altitude
pt.yaw = yaw # in radians!
return pt
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0):
target_raw_pose = PositionTarget()
target_raw_pose.coordinate_frame = self.coordinate_frame
if self.coordinate_frame == 1:
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
else:
target_raw_pose.position.x = -y
target_raw_pose.position.y = x
target_raw_pose.position.z = z
if self.transition_state == 'plane':
if self.plane_mission == 'takeoff':
target_raw_pose.type_mask = 4096
elif self.plane_mission == 'land':
target_raw_pose.type_mask = 8192
elif self.plane_mission == 'loiter':
target_raw_pose.type_mask = 12288
else:
target_raw_pose.type_mask = 16384
else:
if self.coordinate_frame == 1:
target_raw_pose.velocity.x = vx
target_raw_pose.velocity.y = vy
target_raw_pose.velocity.z = vz
target_raw_pose.acceleration_or_force.x = afx
target_raw_pose.acceleration_or_force.y = afy
target_raw_pose.acceleration_or_force.z = afz
else:
target_raw_pose.velocity.x = -vy
target_raw_pose.velocity.y = vx
target_raw_pose.velocity.z = vz
target_raw_pose.acceleration_or_force.x = -afy
target_raw_pose.acceleration_or_force.y = afx
target_raw_pose.acceleration_or_force.z = afz
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
if(self.motion_type == 0):
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW_RATE
if(self.motion_type == 1):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW
if(self.motion_type == 2):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_YAW
return target_raw_pose
def listener():
rospy.init_node('listener', anonymous=True)
rate = rospy.Rate(20)
rospy.Subscriber('mavros/state', State, callback)
local_pos_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=2)
local_posi_raw_pub = rospy.Publisher('mavros/setpoint_raw/local',
PositionTarget,
queue_size=2)
#local_vel_pub=rospy.Publisher('mavros/setpoint_position/local',PoseStamped,queue_size=2)
set_mode_cmd = rospy.ServiceProxy('mavros/set_mode', SetMode)
arm_cmd = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
newpos = PoseStamped()
newpos.pose.position.z = 10.0 #=Point(0,0,2)
#set_mode_cmd('POSITION CONTROL','')
#for _ in range(10):
# rate.sleep()
newvel = PositionTarget()
newvel.velocity.x = -0.5
newvel.yaw = -90.0 / 180.0 * 3.142
newvel.type_mask = newvel.FRAME_LOCAL_NED | newvel.IGNORE_AFX | newvel.IGNORE_AFY | newvel.IGNORE_AFZ
newvel.type_mask = newvel.type_mask | newvel.IGNORE_PX | newvel.IGNORE_PY | newvel.IGNORE_PZ
for _ in range(100):
rate.sleep()
local_pos_pub.publish(newpos)
mymode = 'OFFBOARD'
last_req = rospy.get_time()
start_time = last_req
#print '---',rospy.get_time(),start_time
while rospy.get_time() - start_time < 70:
if rospy.get_time() - last_req > 5:
if state.mode != mymode:
set_mode_cmd(0, mymode)
rospy.loginfo('setting mode...')
elif not state.armed:
arm_cmd(True)
rospy.loginfo('arming...')
last_req = rospy.get_time()
dt = rospy.get_time() - start_time
if dt < 20:
local_pos_pub.publish(newpos)
elif dt < 40:
local_posi_raw_pub.publish(newvel)
elif dt < 60:
newvel.velocity.x = 0.5
local_posi_raw_pub.publish(newvel)
else:
newpos.pose.position.z = 0
local_pos_pub.publish(newpos)
rate.sleep()
def construct_target(self, x, y, z, yaw, yaw_rate = 1):
target_raw_pose = PositionTarget()
target_raw_pose.header.stamp = rospy.Time.now()
target_raw_pose.coordinate_frame = 9
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
return target_raw_pose
def velocity_target(velocity, frame_id='world'):
pt = PositionTarget()
pt.header.stamp = rospy.Time.now()
pt.header.frame_id = frame_id
pt.coordinate_frame = 1
pt.type_mask = VELOCITY_MASK
pt.velocity.x = velocity[0]
pt.velocity.y = velocity[1]
pt.velocity.z = velocity[2]
pt.yaw = position_to_angle(velocity[0], velocity[1])
return pt
def position_target(position, yaw=0.0, frame_id='world'):
pt = PositionTarget()
pt.header.stamp = rospy.Time.now()
pt.header.frame_id = frame_id
pt.coordinate_frame = 1
pt.type_mask = POSITION_MASK
pt.position.x = position[0]
pt.position.y = position[1]
pt.position.z = position[2]
pt.yaw = yaw # in radians!
return pt
def set_target_pose(self,point,yaw, yaw_rate=1):
target_pose = PositionTarget()
target_pose.header.stamp = rospy.get_rostime()
target_pose.coordinate_frame = 9
target_pose.position = point
target_pose.yaw = yaw
target_pose.yaw_rate = yaw_rate
target_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE
self.current_pose = point
return target_pose
def pose_publisher_line():
pub = rospy.Publisher('/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
pose_msg = PositionTarget()
#pose_msg.model_name = 'car_landing_pad'
rate = rospy.Rate(100)
linear_vel = 1.0
time = 0.0
while not rospy.is_shutdown():
#pose_msg.header.stamp = rospy.Time.now()
#pose_msg.header.frame_id = "home"
pose_msg.coordinate_frame = 1
pose_msg.position.x = 2.0
pose_msg.position.y = 2.0
pose_msg.position.z = 0.0
pose_msg.velocity.x = 0.2
pose_msg.velocity.y = 0.2
pose_msg.velocity.z = 0.0
pose_msg.yaw = 0.0
pose_msg.yaw_rate = 0.0
if (motion_type == 0):
pose_msg.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_YAW_RATE
pose_msg.type_mask = 504
print('Pos_x :', pose_msg.position.x)
print('Pos_y :', pose_msg.position.y)
print('Pos_z :', pose_msg.position.z)
print('Yaw :', pose_msg.yaw)
if (motion_type == 1):
pose_msg.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_YAW_RATE
print('Vel_x :', pose_msg.velocity.x)
print('Vel_y :', pose_msg.velocity.y)
print('Vel_z :', pose_msg.velocity.z)
print('Yaw :', pose_msg.yaw)
pub.publish(pose_msg)
rate.sleep()
def send_commands(self, event):
waypoint_command_msg = PositionTarget()
waypoint_command_msg.header.stamp = rospy.get_rostime()
waypoint_command_msg.coordinate_frame = waypoint_command_msg.FRAME_LOCAL_NED
waypoint_command_msg.type_mask = self.ignore_mask
waypoint_command_msg.position.x = 0.0 # E
waypoint_command_msg.position.y = 0.0 # N
waypoint_command_msg.position.z = 15.0 # U
yaw = -self.heading + np.radians(90.0)
while yaw > np.radians(180.0): yaw = yaw - np.radians(360.0)
while yaw < np.radians(-180.0): yaw = yaw + np.radians(360.0)
waypoint_command_msg.yaw = yaw
# Publish.
self.command_pub.publish(waypoint_command_msg)
def set_Local_Waypoint(x_Pos, y_Pos, travel_Height, x_Vel, y_Vel, z_Vel,
yaw_Angle):
"""
This function sets the local waypoint using ENU notation.
x_Pos: East - positive position in meters from home position
y_Pos: North - positive position in meters from home position
z_Pos: Up - positive position in meters from the home position
x_Vel, y_Vel, z_Vel: Corresponding velocities. Not currently working properly.
yaw_Angle: Desired yaw, in radians, as measured counterclockwise from North
"""
takeoff_Waypoint = PositionTarget()
#Timestamp message
takeoff_Waypoint.header.stamp = rospy.get_rostime()
#Set reference frame as global
takeoff_Waypoint.header.frame_id = "1"
#Local ENU coordinate system
takeoff_Waypoint.coordinate_frame = 1
#Taken from ardupilot documentation.
#Sets acceleration and force control bits to 1, or inactive
#Sets position, velocity, and yaw bits to 0, or active, allowing for
# control of these elements. Decimal equivalent is 3008.
takeoff_Waypoint.type_mask = 0b0000101111000000
#set desired positions and travel velocities
takeoff_Waypoint.position.x = x_Pos
takeoff_Waypoint.position.y = y_Pos
takeoff_Waypoint.position.z = travel_Height
takeoff_Waypoint.velocity.x = x_Vel
takeoff_Waypoint.velocity.y = y_Vel
takeoff_Waypoint.velocity.z = z_Vel
takeoff_Waypoint.yaw = yaw_Angle
return takeoff_Waypoint
def _ConstructTarget(self, x, y, z, yaw, yaw_rate=1):
target_raw_pose = PositionTarget()
target_raw_pose.header.stamp = rospy.Time.now()
# uint8 coordinate_frame
# FRAME_LOCAL_NED = 1
# FRAME_LOCAL_OFFSET_NED = 7
# FRAME_BODY_NED = 8
# FRAME_BODY_OFFSET_NED = 9
target_raw_pose.coordinate_frame = 9
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.FORCE
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
return target_raw_pose
def send_commands(self, msg):
time_cur = rospy.get_time()
self.ibvs_active_pub_.publish(self.ibvs_active_msg)
# reset the ibvs counter if we haven't seen the ArUco for more than 1 second
if time_cur - self.ibvs_time >= 1.0 and time_cur - self.ibvs_time_inner >= 1.0 and self.counters_frozen == False:
self.ibvs_count = 0
self.ibvs_count_inner = 0
self.ibvs_active_msg.data = False
# print "reset"
# if the ArUco has been in sight for a while
if self.ibvs_count > self.count_outer_req or self.ibvs_count_inner > self.count_inner_req:
self.ibvs_active_msg.data = True
if self.ibvs_count_inner > self.count_inner_req:
print(self.distance)
# in this case we enter an open-loop drop onto the marker
if self.distance < 1.0 and self.distance > 0.1 and self.land_mode_sent == False:
# set the PX4 to land mode
self.execute_landing()
# freeze the counters
# self.counters_frozen = True
# ibvs_command_msg = Command()
# ibvs_command_msg.x = 0.0
# ibvs_command_msg.y = 0.0
# ibvs_command_msg.F = 0.0
# ibvs_command_msg.z = 0.0
# ibvs_command_msg.mode = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE
# self.waypoint_pub_.publish(ibvs_command_msg)
# # stuff
else:
ibvs_command_msg = PositionTarget()
ibvs_command_msg.header.stamp = rospy.get_rostime()
ibvs_command_msg.coordinate_frame = ibvs_command_msg.FRAME_BODY_NED
ibvs_command_msg.type_mask = self.velocity_mask
ibvs_command_msg.velocity.x = self.saturate(
self.ibvs_x_inner, self.u_max_inner, -self.u_max_inner)
ibvs_command_msg.velocity.y = self.saturate(
self.ibvs_y_inner, self.v_max_inner, -self.v_max_inner)
ibvs_command_msg.velocity.z = self.saturate(
self.ibvs_F_inner, self.w_max_inner, -self.w_max_inner)
ibvs_command_msg.yaw_rate = self.ibvs_z_inner
# Publish.
self.command_pub.publish(ibvs_command_msg)
if self.ibvs_count_inner > 1000:
self.ibvs_count_inner = self.count_inner_req + 1 # reset so it doesn't get too big
else:
ibvs_command_msg = PositionTarget()
ibvs_command_msg.header.stamp = rospy.get_rostime()
ibvs_command_msg.coordinate_frame = ibvs_command_msg.FRAME_BODY_NED
ibvs_command_msg.type_mask = self.velocity_mask
ibvs_command_msg.velocity.x = self.saturate(
self.ibvs_x, self.u_max, -self.u_max)
ibvs_command_msg.velocity.y = self.saturate(
self.ibvs_y, self.v_max, -self.v_max)
ibvs_command_msg.velocity.z = self.saturate(
self.ibvs_F, self.w_max, -self.w_max)
ibvs_command_msg.yaw_rate = self.ibvs_z
# Publish.
self.command_pub.publish(ibvs_command_msg)
if self.ibvs_count > 1000:
self.ibvs_count = self.count_outer_req + 1 # reset so it doesn't get too big
# go back to following regular waypoints
else:
# print "following waypoints"
waypoint_command_msg = PositionTarget()
waypoint_command_msg.header.stamp = rospy.get_rostime()
waypoint_command_msg.coordinate_frame = waypoint_command_msg.FRAME_LOCAL_NED
waypoint_command_msg.type_mask = self.ned_pos_mask
waypoint_command_msg.position.x = self.wp_E # E
waypoint_command_msg.position.y = self.wp_N # N
waypoint_command_msg.position.z = -self.wp_D # U
waypoint_command_msg.yaw = np.radians(0.0) # Point North?
# Publish.
self.command_pub.publish(waypoint_command_msg)
try:
rospy.init_node('eksen',anonymous=True)
global msg
pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget,queue_size=10)
rospy.Subscriber('/mavros/local_position/odom', Odometry, call_back2)
rospy.Subscriber('konum', String, call_back)
msg.header.stamp = rospy.Time.now()
msg.header.frame_id= "world"
msg.coordinate_frame = 8
msg.type_mask=2503
msg.velocity.x = 0.0
msg.velocity.y = 0.0
msg.velocity.z = 0.0
msg.yaw = 0.0
while not rospy.is_shutdown():
pub.publish(msg)
except rospy.ROSInterruptException:
pass
#近地点不控制
if feedback_z < 0.05:
output_x = 0.0
output_y = 0.0
# 自由落体测试，整定速度PID专用MPC_Z_VEL_*
# if feedback_z > 0.3:
# set_z = 1
# if set_z:
# output_z = -0.08
# output_y = 0
# output_x = 0
raw_msg.velocity.x = output_x
raw_msg.velocity.y = output_y
raw_msg.velocity.z = output_z
raw_msg.yaw = output_yaw
if enable_setpoint:
position_pub.publish(raw_msg)
tag_fuse = PoseStamped()
tag_fuse.header.stamp = stamp
tag_fuse.pose.position.x = tag_fuse_x
tag_fuse.pose.position.y = tag_fuse_y
position_tag.publish(tag_fuse)
#outter loop
print "x_set:", pid_x.SetPoint, "x_out:", output_x, "px_Feb:", feedback_x, "vx_Feb", local_linear_x
print "y_set:", pid_y.SetPoint, "y_out:", output_y, "py_Feb:", feedback_y, "vy_Feb", local_linear_y
print "z_set:", pid_z.SetPoint, "z_out:", output_z, "pz_Feb:", feedback_z, "vz_Feb", local_linear_z, 'yaw_set', output_yaw, 'yaw_feb', odom_yaw, 'pitch_feb', odom_pitch, 'roll_feb', odom_roll
rate.sleep()
current_state = state
current_pose = PoseStamped()
def pos_cb(poseStamped):
global current_pose
current_pose = poseStamped
# Commaded Setpoint
setpoint = PositionTarget()
setpoint.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
setpoint.type_mask = PositionTarget.IGNORE_AFX | PositionTarget.IGNORE_AFY | PositionTarget.IGNORE_AFZ | PositionTarget.IGNORE_YAW_RATE
setpoint.yaw = 0
# distance function
def DistToGoal():
return math.dist([
current_pose.pose.position.x, current_pose.pose.position.y,
current_pose.pose.position.z
], [setpoint.position.x, setpoint.position.y, setpoint.position.z])
# Hoops Lookup Table
hoops = [1, 0, 2, -1, -1.5, 0.5, -1.5, 1.5, -1.5, 1, -0.5, 2, 0, -1, 1]
counter = 0
y_off = -0.10
x_off = 0.05
def construct_target(self,
x=0,
y=0,
z=0,
vx=0,
vy=0,
vz=0,
afx=0,
afy=0,
afz=0,
yaw=0,
yaw_rate=0):
target_raw_pose = PositionTarget()
target_raw_pose.coordinate_frame = self.coordinate_frame
if self.platform.split('-')[-2] == '16.04':
if self.coordinate_frame == 1:
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.velocity.x = vx
target_raw_pose.velocity.y = vy
target_raw_pose.velocity.z = vz
target_raw_pose.acceleration_or_force.x = afx
target_raw_pose.acceleration_or_force.y = afy
target_raw_pose.acceleration_or_force.z = afz
else:
target_raw_pose.position.x = -y
target_raw_pose.position.y = x
target_raw_pose.position.z = z
target_raw_pose.velocity.x = -vy
target_raw_pose.velocity.y = vx
target_raw_pose.velocity.z = vz
target_raw_pose.acceleration_or_force.x = afx
target_raw_pose.acceleration_or_force.y = afy
target_raw_pose.acceleration_or_force.z = afz
elif self.platform.split('-')[-2] == '18.04':
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.velocity.x = vx
target_raw_pose.velocity.y = vy
target_raw_pose.velocity.z = vz
target_raw_pose.acceleration_or_force.x = afx
target_raw_pose.acceleration_or_force.y = afy
target_raw_pose.acceleration_or_force.z = afz
else:
print('XTDrone only supports Ubuntu16.04 and Ubuntu18.04 now.')
sys.exit(0)
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
if (self.motion_type == 0):
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW_RATE
if (self.motion_type == 1):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.IGNORE_YAW
if (self.motion_type == 2):
target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \
+ PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_YAW
return target_raw_pose
def build_position_target(px=None,
py=None,
pz=None,
v=None,
vx=None,
vy=None,
vz=None,
a=None,
ax=None,
ay=None,
az=None,
yaw=None,
yaw_rate=None,
is_force=False):
msg = PositionTarget()
msg.header.stamp = rospy.Time.now()
msg.type_mask = ALL_FLAGS
msg.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
if v is not None:
vx, vy, vz = v, v, v
if a is not None:
ax, ay, az = a, a, a
if px is not None:
msg.position.x = px
msg.type_mask -= PositionTarget.IGNORE_PX
if py is not None:
msg.position.y = py
msg.type_mask -= PositionTarget.IGNORE_PY
if pz is not None:
msg.position.z = pz
msg.type_mask -= PositionTarget.IGNORE_PZ
if vx is not None:
msg.velocity.x = vx
msg.type_mask -= PositionTarget.IGNORE_VX
if vy is not None:
msg.velocity.y = vy
msg.type_mask -= PositionTarget.IGNORE_VY
if vz is not None:
msg.velocity.z = vz
msg.type_mask -= PositionTarget.IGNORE_VZ
if ax is not None:
msg.velocity.x = ax
msg.type_mask -= PositionTarget.IGNORE_AFX
if ay is not None:
msg.velocity.y = ay
msg.type_mask -= PositionTarget.IGNORE_AFY
if az is not None:
msg.velocity.z = az
msg.type_mask -= PositionTarget.IGNORE_AFZ
if yaw is not None:
msg.yaw = yaw
msg.type_mask -= PositionTarget.IGNORE_YAW
if yaw_rate is not None:
msg.yaw_rate = yaw_rate
msg.type_mask -= PositionTarget.IGNORE_YAW_RATE
if is_force:
msg.type_mask -= PositionTarget.FORCE
return msg
att_cmd.velocity = Vector3()
att_cmd.velocity.x = 0
att_cmd.velocity.y = 0
att_cmd.velocity.z = 0.5
for i in range(0, 100):
set_raw_local.publish(att_cmd)
rate.sleep()
set_mode("OFFBOARD", 5)
set_arm(True, 5)
att_cmd.velocity.x = 0
att_cmd.velocity.y = 0
att_cmd.velocity.z = 0.1
att_cmd.yaw = 0
while not rospy.is_shutdown():
if not q.empty():
key = q.get()
# Left and right.
if key == "d":
att_cmd.velocity.x += 0.1
if key == "q":
att_cmd.velocity.x -= 0.1
# Forward and backwards.
if key == "z":
att_cmd.velocity.y += 0.1
if key == "s":
set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), mavros_msgs.srv.SetMode)
set_mode(0,'OFFBOARD')
counter = 0
try:
while not rospy.is_shutdown():
pub.publish(msg)
counter= counter + 1
if( counter>=50):
msg.type_mask ^= PositionTarget.IGNORE_YAW
msg.yaw = math.pi
print counter
if( counter>=100):
set_mode(0,'AUTO.LAND')
rate.sleep()
except rospy.ROSInterruptException:
pass
