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 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 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=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 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 send_commands(self, event):
# Create the NED velocity vector.
ned_vec = np.array([[self.vel_x], [self.vel_y], [self.vel_z]],
dtype=np.float32)
# Rotate the NED vector to ENU (for MAVROS).
enu_vec = self.ned_to_enu(ned_vec)
# Create the PositionTarget command message.
command_msg = PositionTarget()
# Fill out the message.
command_msg.header.stamp = rospy.get_rostime()
# FRAME_BODY_NED is the vehicle-1 frame
command_msg.coordinate_frame = command_msg.FRAME_BODY_NED
command_msg.type_mask = self.ignore_mask
command_msg.velocity.x = enu_vec[0][0]
command_msg.velocity.y = enu_vec[1][0]
command_msg.velocity.z = enu_vec[2][0]
command_msg.yaw_rate = -self.yaw_rate
# Publish the message.
self.command_pub.publish(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_body_yaw_rate(self, vel_body, yaw_rate, freq):
"""
Offboard method that sends velocity_body and yaw_rate references to the PX4 autopilot of the the drone.
Makes convertions from the body NED frame adopted for the ISR Flying Arena to the body ENU frame used by ROS.
Parameters
----------
vel_body : np.array of floats with shape (3,1)
Desired linear velocity for the drone, in body NED coordinates.
yaw_rate : float
Desired yaw rate for the vehicle, in radians per second.
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 = 8
msg.type_mask = 1991
msg.velocity.x, msg.velocity.y, msg.velocity.z = ned_to_enu(vel_body)
msg.yaw_rate = -yaw_rate
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
def forward(self):
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.angular.z = 0
print("forward")
vel = PositionTarget()
vel.header.stamp = rospy.Time.now()
vel.coordinate_frame = PositionTarget.FRAME_BODY_NED
vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE
vel.velocity.x = 0
vel.velocity.y = 1.0
vel.velocity.z = 0
bt_missions.p0 = (bt_missions.current_.latitude,
bt_missions.current_.longitude)
bt_missions.p1 = (bt_missions.target[bt_missions.index][0],
bt_missions.target[bt_missions.index][1])
bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters
forward_t = rospy.get_time()
while rospy.get_time(
) - forward_t <= bt_missions.dist * 0.1: #bt_missions.dist *1.25*0.25:
#rospy.wait_for_service('/mavros/set_mode')
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
bt_missions.set_vel_pub.publish(msg)
bt_missions.set_v_pub.publish(vel)
bt_missions.rate.sleep()
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, 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 twist_obj(x, y, z, a, b, c):
move_cmd = PositionTarget(header=Header(stamp=rospy.get_rostime()))
move_cmd.velocity.x = x
move_cmd.velocity.y = y
move_cmd.velocity.z = z
move_cmd.yaw_rate = c
move_cmd.coordinate_frame = 8
move_cmd.type_mask = 1479 # 1479/ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0)
return move_cmd
def set_velocity_body(v_x, v_y, v_z, rate):
set_velocity = PositionTarget()
set_velocity.coordinate_frame = 8 # body NED
set_velocity.velocity.x = v_y # verificare che le componenti siano invertite
set_velocity.velocity.y = v_x
set_velocity.velocity.z = v_z
set_velocity.yaw_rate = rate
set_velocity.type_mask = 1479
setpoint_velocity_pub.publish(set_velocity)
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 send_ned_velocity(v_x, v_y, v_z): set_velocity = PositionTarget() set_velocity.coordinate_frame = 1 # local NED set_velocity.velocity.x = v_y # verificare che le componenti siano invertite set_velocity.velocity.y = v_x set_velocity.velocity.z = v_z set_velocity.type_mask = 4039 setpoint_velocity_pub.publish(set_velocity) time.sleep(0.5)
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 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 custom(twisted):
#print("custom1")
move_cmd = PositionTarget(header=Header(stamp=rospy.get_rostime()))
move_cmd.velocity.x = twisted.linear.y
move_cmd.velocity.y = twisted.linear.x
move_cmd.velocity.z = twisted.linear.z
move_cmd.yaw_rate = twisted.angular.z
move_cmd.coordinate_frame = 8
move_cmd.type_mask = 1479 # 1479/ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0)
pub.publish(move_cmd)
def get_descent(self,x,y,z):
des_vel = PositionTarget()
des_vel.header.frame_id = "world"
des_vel.header.stamp=rospy.Time.from_sec(time.time())
des_vel.coordinate_frame= 8
des_vel.type_mask = 3527
des_vel.velocity.x = x
des_vel.velocity.y = y
des_vel.velocity.z = z
return des_vel
def takeoff_obj(z):
print("--------------------Takeoff-----------------")
move_cmd = PositionTarget(header=Header(stamp=rospy.get_rostime()))
move_cmd.velocity.x = 0
move_cmd.velocity.y = 0
move_cmd.velocity.z = 0.5
move_cmd.position.z = z
move_cmd.type_mask = (0x1000 | 0b110111000011)
move_cmd.coordinate_frame = 9
return move_cmd
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 displacement(x_disp, y_disp, z_disp):
print("I am in displacement")
des_disp = PositionTarget()
des_disp.header.frame_id = "world"
des_disp.header.stamp = rospy.Time.from_sec(time.time())
des_disp.coordinate_frame = 8
des_disp.type_mask = 3527
des_disp.velocity.x = x_disp
des_disp.velocity.y = y_disp
des_disp.velocity.z = z_disp
return des_disp
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 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 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 twist_obj(x, y, z, a, b, c):
# move_cmd = Twist()
move_cmd = PositionTarget()
move_cmd.velocity.x = x
move_cmd.velocity.y = y
move_cmd.velocity.z = z
# move_cmd.yaw_rate = c
move_cmd.header.stamp = rospy.get_rostime()
move_cmd.header.frame_id = "world"
move_cmd.coordinate_frame = 8
move_cmd.type_mask = 3527
return move_cmd
def global_position():
global home_
global home_set_
global current_
global brng
global dirct, scale
global current_heading
global current_yaw
guide()
hdg_subscriber = rospy.Subscriber('/mavros/global_position/compass_hdg',
Float64, compass)
home_sub = rospy.Subscriber("/mavros/home_position/home", HomePosition,
setHomeGeoPointCB)
set_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
set_v_pub = rospy.Publisher('/mavros/setpoint_yaw/local',
PositionTarget,
queue_size=10)
gps_sub = rospy.Subscriber("/mavros/global_position/global", NavSatFix,
current_position)
rospy.Subscriber("/mavros/global_position/raw/fix", NavSatFix, gpsstate_cb)
gps_pub = rospy.Publisher('/mavros/global_position/raw/gps_vel',
TwistStamped,
queue_size=10)
#altitude_sub = rospy.Subscriber('/mavros/altitude', Altitude, alt_cb)
local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position',
'local'),
PoseStamped,
queue_size=10)
time_now = rospy.get_time()
while rospy.get_time() - time_now <= 3:
pass
print("gps now: %s" % gps_state_.status)
if gps_state_.status == -1:
RTL()
else:
msg = TwistStamped()
msg.header.stamp = rospy.Time.now()
msg.twist.angular.z = 0
print("forward")
vel = PositionTarget()
vel.header.stamp = rospy.Time.now()
vel.coordinate_frame = PositionTarget.FRAME_BODY_NED
vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE
vel.velocity.x = 0
vel.velocity.y = 1
vel.velocity.z = 0
forward_t = rospy.get_time()
while rospy.get_time() - forward_t <= 5:
set_vel_pub.publish(msg)
set_v_pub.publish(vel)
rate.sleep()
def getvelBody(self, u, v, w):
msg = PositionTarget()
msg.header.stamp = rospy.Time.now()
msg.coordinate_frame = 8
msg.type_mask = 4039
msg.velocity.x = u
msg.velocity.y = v
msg.velocity.z = w
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
self.pub2.publish(msg)
r.sleep()
def circle(self, x, y, z):
msg = PositionTarget()
msg.header.stamp = rospy.Time.now()
msg.coordinate_frame = 8
msg.type_mask = 1543
msg.velocity.x = 0
msg.velocity.y = -1
msg.velocity.z = 0
msg.acceleration_or_force.x = -0.2
msg.acceleration_or_force.y = 0
msg.acceleration_or_force.z = 0
msg.yaw_rate = -0.2
self.pub2.publish(msg)
def construct_target(self, x=0, y=0, z=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.mission == 'takeoff':
target_raw_pose.type_mask = 4096
elif self.mission == 'land':
target_raw_pose.type_mask = 8192
elif self.mission == 'loiter':
target_raw_pose.type_mask = 12288
else:
target_raw_pose.type_mask = 16384
return target_raw_pose
