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 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 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 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 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 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 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 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 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 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 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,
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 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 publish():
pub = rospy.Publisher('/mavros/setpoint_raw/local',
PositionTarget,
queue_size=1)
data = PositionTarget()
data.header.stamp = rospy.Time.now()
data.header.frame_id = "test"
data.coordinate_frame = 8
data.type_mask = 1991
#type mask kisminda hangi veriler kullanilmayacaksa alttaki sayilar toplanip yazilmali kod yoksa calismaz
data.velocity.x = int(10)
data.velocity.y = 0
data.velocity.z = 0
data.yaw_rate = 0.0
pub.publish(data)
rospy.sleep(2)
print(data)
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 _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_ibvs_command(self, flag):
if self.mode_flag == 'mavros':
if flag == 'inner':
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) + self.target_VE * np.cos(
self.psi) - self.target_VN * np.sin(self.psi)
ibvs_command_msg.velocity.y = self.saturate(
self.ibvs_y_inner, self.v_max_inner,
-self.v_max_inner) + self.target_VN * np.cos(
self.psi) + self.target_VE * np.sin(self.psi)
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_mavros.publish(ibvs_command_msg)
elif flag == 'outer':
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) + self.target_VE * np.cos(
self.psi) - self.target_VN * np.sin(self.psi)
ibvs_command_msg.velocity.y = self.saturate(
self.ibvs_y, self.v_max,
-self.v_max) + self.target_VN * np.cos(
self.psi) + self.target_VE * np.sin(self.psi)
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_mavros.publish(ibvs_command_msg)
else:
pass
else:
if flag == 'inner':
ibvs_command_msg = Command()
ibvs_command_msg.x = self.saturate(
self.ibvs_x_inner, self.u_max_inner,
-self.u_max_inner) + self.target_VN * np.cos(
self.psi) + self.target_VE * np.sin(self.psi)
ibvs_command_msg.y = self.saturate(
self.ibvs_y_inner, self.v_max_inner,
-self.v_max_inner) + self.target_VE * np.cos(
self.psi) - self.target_VN * np.sin(self.psi)
ibvs_command_msg.F = self.saturate(self.ibvs_F_inner,
self.w_max_inner,
-self.w_max_inner)
ibvs_command_msg.z = self.ibvs_z_inner
ibvs_command_msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE
self.command_pub_roscopter.publish(ibvs_command_msg)
elif flag == 'outer':
ibvs_command_msg = Command()
ibvs_command_msg.x = self.saturate(
self.ibvs_x, self.u_max,
-self.u_max) + self.target_VN * np.cos(
self.psi) + self.target_VE * np.sin(self.psi)
ibvs_command_msg.y = self.saturate(
self.ibvs_y, self.v_max,
-self.v_max) + self.target_VE * np.cos(
self.psi) - self.target_VN * np.sin(self.psi)
ibvs_command_msg.F = self.saturate(self.ibvs_F, self.w_max,
-self.w_max)
ibvs_command_msg.z = self.ibvs_z
ibvs_command_msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE
self.command_pub_roscopter.publish(ibvs_command_msg)
else:
pass
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)
lastErrorX = errorX
if __name__ == '__main__':
try:
rospy.init_node('eksen', anonymous=True)
pub = rospy.Publisher('/uav1/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
rospy.Subscriber('konum', String, call_back)
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "world"
msg.coordinate_frame = 8
msg.type_mask = 1479
msg.velocity.x = 0.0
msg.velocity.y = 0.0
msg.velocity.z = 0.0
msg.yaw_rate = 0.0
while not rospy.is_shutdown():
pub.publish(msg)
except rospy.ROSInterruptException:
pass
x = desired_x
y = desired_y
else:
x = -1 * desired_y
y = desired_x
z = desired_z
th = desired_yaw
speed = 1
turn = 1
body = PositionTarget()
body.velocity.x = x * speed
body.velocity.y = y * speed
body.velocity.z = z * speed
body.yaw_rate = th * turn
body.header.stamp = rospy.get_rostime()
body.header.frame_id = "world"
if desired_pitch == 1:
body.coordinate_frame = 1
else:
body.coordinate_frame = 8
body.type_mask = int('011111000111', 2)
# pub1.publish(body)
except Exception as e:
print(e)
finally:
body = PositionTarget()
body.velocity.x = 0
z = last_z
yaw_rate = 0
if (key == '\x03'):
break
post = PositionTarget() # Use PositionTarget publisher
post.header.frame_id = "home"
post.header.stamp = rospy.Time.now()
post.coordinate_frame = 8 # pos.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
post.type_mask = mask
post.position.z = z * alt
# Altittude Position
post.velocity.x = vy * speed
post.velocity.y = vx * speed
# X and Y velocity
post.yaw_rate = yaw_rate * turn # Yaw rate
pub.publish(post)
rate.sleep()
except Exception as e:
print(e)
finally:
post = PositionTarget() # Use PositionTarget publisher
post.header.frame_id = "home"
post.header.stamp = rospy.Time.now()
post.coordinate_frame = 8 # pos.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
post.type_mask = mask
post.position.z = 1
# Altittude Position
post.velocity.x = 0
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
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