def __init__(self):
self.step = 0
self.lds = rospy.Subscriber("riseq/sacc/ladder_info", RiseSaccHelix,
self.lds_cb)
self.ref_pub_local = rospy.Publisher("riseq/sacc/setpoint_helix_local",
PoseStamped,
queue_size=10)
self.ref_pub_global = rospy.Publisher(
"riseq/sacc/setpoint_helix_global",
GlobalPositionTarget,
queue_size=10)
self.ladder_depth = 0.0
self.ladder_height = 30
# PX4 related
self.state = Odometry()
self.setup_mavros()
self.command_pose = PoseStamped()
self.command_pose.pose.orientation.w = 1.0
self.home_pose = PoseStamped()
self.home_pose_set = False
self.global_home = GlobalPositionTarget()
self.global_state = GlobalPositionTarget()
self.global_home_pose_set = False
def reaching_cb(user_data):
rospy.loginfo('mode GUIDED')
setGuidedMode()
rospy.loginfo('Balloon found')
rospy.loginfo('Mission Reaching')
(locked, err_x_pix, err_y_pix, err_x_m, err_y_m, dist, res_w,
res_h) = balloonchecker()
#position_topic = rospy.Subscriber('/mavros/global_position/global', NavSatFix, home_callback)
#Alt_topic = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_callback)
#compass_topic = rospy.Subscriber('mavros/global_position/compass_hdg', Float64, compass_callback)
#rospy.sleep(0.5)
if locked == 1:
vel_x, vel_y, vel_z, yaw_rate = dir_pid_return()
set_velocity_body(vel_x, vel_y, vel_z, yaw_rate)
while locked == 1 and mode == 'GUIDED':
vel_x, vel_y, vel_z, yaw_rate = dir_pid_return()
set_velocity_body(vel_x, vel_y, vel_z, yaw_rate)
(locked, err_x_pix, err_y_pix, err_x_m, err_y_m, dist, res_w,
res_h) = balloonchecker()
if locked == 1 and abs(err_x_m) <= 0.1 and abs(err_y_m) < 0.1 and abs(
dist) < 1.2:
break
if mode == 'ALT_HOLD':
Clear_Mission()
rospy.signal_shutdown('Quit for Alt_hold')
os.system('rosnode kill ' + mavros_name)
sis.stop()
elif locked == 1 and abs(err_x_m) <= 0.1 and abs(err_y_m) < 0.1 and abs(
dist) < 1.2:
#if i got it in my hands
my_latit = lat
my_longit = lon
#my_new_altit = alt + user_data.delta_alt
my_new_altit = alt
set_position = GlobalPositionTarget()
set_position.coordinate_frame = 6
set_position.latitude = my_latit
set_position.longitude = my_longit
set_position.altitude = my_new_altit
set_position.type_mask = 4088
setpoint_position_pub.publish(set_position)
time.sleep(0.5)
print 'STOP'
print 'Balloon reached!'
return 'searching'
elif locked == 0:
# user_data.target_loc = target_loc
# print target_loc
return 'searching'
else:
return 'failed'
def __init__(self):
# Publisher to ardrone cmd_vel topic, can be run in namespace 48.213
self.global_position_target_pub = rospy.Publisher(
"/mavros/setpoint_raw/global", GlobalPositionTarget, queue_size=1)
# Initialize joy and cmd_vel variables
self.joy_data = Joy()
self.command_velocity = Twist() # Publishing to real cmd_vel
self.override_control = 0
self.current_reference = GlobalPositionTarget()
self.current_reference.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_INT
self.current_reference.type_mask = 2552
self.max_vel_x = rospy.get_param("~max_vel_x", 0.5)
self.max_vel_y = rospy.get_param("~max_vel_y", 0.5)
self.max_vel_z = rospy.get_param("~max_vel_z", 0.5)
self.max_vel_yaw = rospy.get_param("~max_vel_yaw", 0.1)
self.rate = rospy.get_param("~rate", 20)
self.gps_factor = 0.00001
self.global_yaw = 0.0
self.get_current_global_reference_flag = 0
self.first_global_reference_flag = 0
# Subscriber to joystick topic
rospy.Subscriber("/manual_control/joy",
Joy,
self.joyCallback,
queue_size=1)
# subscriber na trenutnu globalnu poziciju
rospy.Subscriber("/mavros/global_position/global",
NavSatFix,
self.currentGlobalPositionCallback,
queue_size=1)
# subscriber to imu for yaw
rospy.Subscriber("/mavros/imu/data",
Imu,
self.imuCallback,
queue_size=1)
def cbPubCmd(self, event):
if not self.started: return
if self.home_pos[2] < 1: return # Wait for home pos to be received
# Check if distance to waypoint is small enough to count it as reached TODO may be wrong approximation and TODO change for RTK
if self.getDistanceBetweenGPS(self.current_pos,
self.waypoints[self.currentWPidx]) < 2:
self.currentWPidx += 1
if self.currentWPidx == self.maxWPidx:
rospy.loginfo("MISSION DONE")
self.started = False
return
# Log info about current waypoint
rospy.loginfo("Current waypoint: " + str(self.currentWPidx) + " / " +
str(self.maxWPidx))
# Check if mode needs to be changed for OFFBOARD and ARM vehicle (this is a startup procedure)
if str(self.current_state.mode) != "OFFBOARD" and rospy.Time.now(
) - self.last_request > rospy.Duration(5.0):
resp = self.set_mode_client(0, "OFFBOARD")
if resp.mode_sent:
rospy.loginfo("Offboard enabled")
self.last_request = rospy.Time.now()
else:
if not self.current_state.armed and rospy.Time.now(
) - self.last_request > rospy.Duration(5.0):
resp = self.arming_client(True)
if resp.success:
rospy.loginfo("Vehicle armed")
self.last_request = rospy.Time.now()
# Publish information - where should the drone fly next?
pose = PoseStamped()
latWP = self.waypoints[self.currentWPidx][0]
lonWP = self.waypoints[self.currentWPidx][1]
altWP = self.waypoints[self.currentWPidx][2]
velWP = self.waypoints[self.currentWPidx][3]
# latHome = self.home_pos[0]
# lonHome = self.home_pos[1]
# altHome = self.home_pos[2]
# pose.pose.position.x = 6371000 * radians(lonWP - lonHome) * cos(radians(latHome))
# pose.pose.position.y = 6371000 * radians(latWP - latHome)
# pose.pose.position.z = altWP - altHome
#self.local_pos_pub.publish(pose)
if self.ready:
msg = GlobalPositionTarget()
msg.latitude = latWP
msg.longitude = lonWP
msg.altitude = altWP
msg.header.stamp = rospy.Time.now()
msg.coordinate_frame = 5
msg.type_mask = 0b101111111000
msg.yaw = self.getNextYaw()
self.global_pos_pub.publish(msg)
par = ParamValue()
par.integer = 0
par.real = velWP
try:
self.set_vel("MPC_XY_VEL_MAX", par)
except Exception:
print("e")
else:
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 2.0
self.local_pos_pub.publish(pose)
try:
par = ParamValue()
par.integer = 0
par.real = velWP
resp = self.set_vel("MPC_XY_VEL_MAX", par)
if resp.success: self.ready = True
except Exception as e:
print(e)
a/d : increase/decrease longitude jingdu east/west
space key, s : force stop
CTRL-C to quit
"""
e = """
Communications Failed
"""
"""
global variable
"""
gps_curr = NavSatFix()
gps_pub_msg = GlobalPositionTarget()
gps_pub_geo_msg = GeoPoseStamped()
uav_state = State()
ismove = Bool()
ismove = False
def getKey():
if os.name == 'nt':
return msvcrt.getch()
tty.setraw(sys.stdin.fileno())
rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
if rlist:
key = sys.stdin.read(1)
else:
key = ''
queue_size=10)
print("Publisher and Subscriber Created")
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
landing_client = rospy.ServiceProxy('mavros/cmd/land', CommandTOL)
print("Clients Created")
rate = rospy.Rate(20)
while (not current_state.connected):
print(current_state.connected)
rate.sleep()
print("Creating global position")
point = GlobalPositionTarget()
point.header.stamp = rospy.Time.now()
point.coordinate_frame = GlobalPositionTarget().FRAME_GLOBAL_TERRAIN_ALT
point.type_mask = GlobalPositionTarget().IGNORE_VX + GlobalPositionTarget(
).IGNORE_VY + GlobalPositionTarget().IGNORE_VZ + GlobalPositionTarget(
).IGNORE_AFX + GlobalPositionTarget().IGNORE_AFY + GlobalPositionTarget(
).IGNORE_AFZ + GlobalPositionTarget().FORCE + GlobalPositionTarget(
).IGNORE_YAW_RATE
point.latitude = 37.5647106
point.longitude = 126.6276294
point.altitude = 25.0657
point.yaw = 0.0
for i in range(100):
#local_pos_pub.publish(pose)
point.header.stamp = rospy.Time.now()
topic_global = '/mavros/global_position/global'
topic_local = '/local_position'
topic_origin = '/set_origin'
topic_alt = '/mavros/altitude'
topic_global_set = '/mavros/setpoint_position/global'
topic_marker = "/goal_pose"
frame_id = 'map'
current_global_pose = None
origin_pose = None
local_pose_msg = PoseStamped()
heiht = 0.0
goal_global_msg = GlobalPositionTarget()
mask=int('111111111000', 2)
def global_pose_clb(data):
"""
Robot Geodetic coordinates from GPS callback.
:param data: GPS data
:type data: sensor_msgs.msg.NavSatFix
:type origin_pose: sensor_msgs.msg.NavSatFix
:type current_global_pose: NavSatFix
:type local_pose_msg: PoseStamped
"""
global origin_pose, current_global_pose, local_pose_msg, heiht
def do_helix_trajectory(self):
init_x = self.state.pose.pose.position.x
init_y = self.state.pose.pose.position.y
init_z = self.state.pose.pose.position.z
self.helix_controller = htc(vrate=0.05,
radius=1.0,
center=(1, 0, 0),
init=(init_x, init_y, init_z),
t_init=rospy.get_time(),
w=0.5)
self.yaw_controller = sc2(Kp=6., Kv=0.0)
q = self.state.pose.pose.orientation
q = [q.x, q.y, q.z, q.w]
yaw, pitch, roll = tt.euler_from_quaternion(q, axes='rzyx')
self.helix_controller.phase = yaw + np.pi # yaw angle + 180 degrees because its center perspective
print("Phase: ", self.helix_controller.phase)
rate = rospy.Rate(20)
# do helix trajectory for 30 seconds
print("Doing helix trajectory")
#while( (rospy.get_time() - self.helix_controller.t_init) < 120.):
while (self.state.pose.pose.position.z < (self.ladder_height + 2)):
# state for x and y position
xs = np.array([[self.state.pose.pose.position.x],
[self.state.twist.twist.linear.x], [0.0]])
ys = np.array([[self.state.pose.pose.position.y],
[self.state.twist.twist.linear.y], [0.0]])
states = [xs, ys]
ladder_position = self.get_ladder_location()
ladder_position = [0.0, 1.0]
self.helix_controller.set_helix_center(ladder_position)
ux, uy, uz, ref = self.helix_controller.compute_command(
states, rospy.get_time())
q = self.compute_yaw()
self.command_pose.header.stamp = rospy.Time.now()
self.command_pose.pose.position.x = ux
self.command_pose.pose.position.y = uy
self.command_pose.pose.position.z = uz
self.command_pose.pose.orientation.x = q[0]
self.command_pose.pose.orientation.y = q[1]
self.command_pose.pose.orientation.z = q[2]
self.command_pose.pose.orientation.w = q[3]
self.local_pos_pub.publish(self.command_pose)
# publish reference trajectory in local frame
refmsg = PoseStamped()
refmsg.header.stamp = rospy.Time.now()
refmsg.pose.position.x = ref[0][0][0]
refmsg.pose.position.y = ref[1][0][0]
refmsg.pose.position.z = uz
self.ref_pub_local.publish(refmsg)
# publish reference trajectory in global frame
lat, lon = self.get_global_coordinates(ux, uy)
global_refmsg = GlobalPositionTarget()
global_refmsg.header.stamp = rospy.Time.now()
global_refmsg.header.frame_id = self.global_state.header.frame_id
global_refmsg.latitude = lat
global_refmsg.longitude = lon
global_refmsg.altitude = uz
self.ref_pub_global.publish(global_refmsg)
rate.sleep()
class bt_missions:
rospy.init_node('mavros_takeoff_python')
mavros.set_namespace()
rate = rospy.Rate(20)
dirct_ = Float64()
home_ = HomePosition()
current_state = State()
current_ = GlobalPositionTarget()
home_set_ = False
isContinue = True
dist_first = True
dis_count = True
#triangle
target = [[24.985059, 121.572934], [24.985161, 121.572858]]
forward_t = rospy.get_time()
index = 0
time_index = 0
p0 = 0
p1 = 0
brng = 0
dLon = 0
y = 0
x = 0
current_yaw = 0
dist = 0
heading = 0
lat = 0
lon = 0
cnt = 0
set_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
set_v_pub = rospy.Publisher('/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position',
'local'),
PoseStamped,
queue_size=10)
def __init__(self):
self.hdg_subscriber = rospy.Subscriber(
"/mavros/global_position/compass_hdg", Float64, self.compass)
self.home_sub = rospy.Subscriber("/mavros/home_position/home",
HomePosition, self.setHomeGeoPointCB)
self.gps_sub = rospy.Subscriber("/mavros/global_position/global",
NavSatFix, self.current_position_cb)
state_sub = rospy.Subscriber(mavros.get_topic('state'), State,
self.state_cb)
self.tree = self.acheive | (
(self.isGuided | (self.notGuided >> self.set_guided)) >>
(self.yaw_correction | self.notGuided | self.notGuided
| self.correction) >> self.forward)
#(self.yaw_discorrection | self.notGuided | (self.yaw_correction >>
def compass(self, dirct):
bt_missions.dirct_ = dirct
bt_missions.heading = bt_missions.dirct_.data
def state_cb(self, state):
bt_missions.current_state = State()
bt_missions.current_state = state
def angleFromCoordinate(self, lat0, long0, lat1, long1):
bt_missions.dLon = (long1 - long0)
bt_missions.y = math.sin(bt_missions.dLon) * math.cos(bt_missions.lat1)
bt_missions.x = math.cos(lat0) * math.sin(lat1) - math.sin(lat0) * cos(
lat1) * math.cos(bt_missions.dLon)
bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.brng = (bt_missions.brng + 360) % 360
bt_missions.brng = 360 - bt_missions.brng
bt_missions.brng = math.radians(bt_missions.brng)
def setHomeGeoPointCB(self, home):
bt_missions.home_ = home
bt_missions.home_set_ = True
rospy.loginfo(
"Received Home (WGS84 datum): %lf, %lf, %lf" %
(bt_missions.home_.geo.latitude, bt_missions.home_.geo.longitude,
bt_missions.home_.geo.altitude))
def current_position_cb(self, current):
bt_missions.current_ = current
bt_missions.lat = bt_missions.current_.latitude
bt_missions.lon = bt_missions.current_.longitude
@condition
def acheive(self):
if bt_missions.index == 0 and bt_missions.dist_first == True:
bt_missions.dist_first = False
bt_missions.p0 = (bt_missions.home_.geo.latitude,
bt_missions.home_.geo.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
print(bt_missions.dist)
if bt_missions.dist >= 1.5:
return False
else:
bt_missions.index = bt_missions.index + 1
elif bt_missions.index == 0 and bt_missions.dist_first == False:
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
print(bt_missions.dist)
if bt_missions.dist >= 1.5:
return False
else:
bt_missions.index = bt_missions.index + 1
elif bt_missions.index == 1:
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
print(bt_missions.dist)
if bt_missions.dist >= 1.5:
return False
else:
return True
@condition
def isGuided(self):
if bt_missions.current_state == "GUIDED":
return True
else:
return False
@condition
def notGuided(self):
if bt_missions.current_state == "GUIDED":
return False
else:
return True
@condition
def yaw_correction(self):
if bt_missions.index == 0:
bt_missions.dLon = (bt_missions.target[bt_missions.index][1] -
bt_missions.home_.geo.longitude)
bt_missions.y = math.sin(bt_missions.dLon) * math.cos(
bt_missions.target[bt_missions.index][0])
bt_missions.x = math.cos(bt_missions.home_.geo.latitude) * math.sin(
bt_missions.target[bt_missions.index][0]) - math.sin(
bt_missions.home_.geo.latitude) * cos(bt_missions.target[
bt_missions.index][0]) * math.cos(bt_missions.dLon)
bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.brng = (bt_missions.brng + 360) % 360
bt_missions.brng = 360 - bt_missions.brng
bt_missions.brng = math.radians(bt_missions.brng)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.current_yaw = bt_missions.heading
if bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
return False
else:
return True
else:
bt_missions.dis_count = True
bt_missions.dLon = (bt_missions.target[bt_missions.index][1] -
bt_missions.current_.longitude)
bt_missions.y = math.sin(bt_missions.dLon) * math.cos(
bt_missions.target[bt_missions.index][0])
bt_missions.x = math.cos(bt_missions.home_.geo.latitude) * math.sin(
bt_missions.target[bt_missions.index][0]) - math.sin(
bt_missions.current_.latitude) * cos(bt_missions.target[
bt_missions.index][0]) * math.cos(bt_missions.dLon)
bt_missions.brng = math.atan2(bt_missions.y, bt_missions.x)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.brng = (bt_missions.brng + 360) % 360
bt_missions.brng = 360 - bt_missions.brng
bt_missions.brng = math.radians(bt_missions.brng)
bt_missions.brng = math.degrees(bt_missions.brng)
bt_missions.current_yaw = bt_missions.heading
if bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
return False
else:
return True
@condition
def yaw_discorrection(self):
if bt_missions.index == 0:
bt_missions.current_yaw = bt_missions.heading
print("discorrection heading: %s" % bt_missions.current_yaw)
print("discorrection brng: %s" % bt_missions.brng)
if bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
bt_missions.current_yaw = bt_missions.heading
return True
else:
return False
else:
bt_missions.current_yaw = bt_missions.heading
print("discorrection heading: %s" % bt_missions.current_yaw)
print("discorrection brng: %s" % bt_missions.brng)
if bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
bt_missions.current_yaw = bt_missions.heading
return True
else:
return False
@action
def set_guided(self):
rospy.wait_for_service('/mavros/set_mode')
try:
change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
response = change_mode(custom_mode="GUIDED")
except rospy.ServiceException as e:
print("Set mode failed: %s" % e)
@action
def correction(self):
if bt_missions.dis_count == True:
bt_missions.current_yaw = bt_missions.heading
while bt_missions.current_yaw > (
360 - bt_missions.brng) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng) - 2:
msg = TwistStamped()
if 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw < 180:
msg.twist.angular.z = -0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw > 0 and 360 - bt_missions.brng - bt_missions.current_yaw >= 180:
msg.twist.angular.z = 0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs(
360 - bt_missions.brng -
bt_missions.current_yaw) >= 180:
msg.twist.angular.z = -0.1
elif 360 - bt_missions.brng - bt_missions.current_yaw < 0 and math.fabs(
360 - bt_missions.brng -
bt_missions.current_yaw) < 180:
msg.twist.angular.z = 0.1
bt_missions.set_vel_pub.publish(msg)
bt_missions.current_yaw = bt_missions.heading
#print("first correction : %s" %bt_missions.current_yaw)
else:
#pass
bt_missions.current_yaw = bt_missions.heading
while bt_missions.current_yaw > (
360 - bt_missions.brng
) + 2 or bt_missions.current_yaw < (
360 - bt_missions.brng
) - 2: #and int(rospy.get_time() -bt_missions.forward_t) % 10 == 0:
msg = TwistStamped()
#print("forward correction: %s" %bt_missions.current_yaw)
msg.twist.angular.z = math.radians(bt_missions.current_yaw -
360 + bt_missions.brng)
bt_missions.set_vel_pub.publish(msg)
bt_missions.current_yaw = bt_missions.heading
@action
def forward(self):
print(123)
forward_t = rospy.get_time()
#while rospy.get_time() - forward_t <= 5:
bt_missions.dis_count = False
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 = 0.8
vel.velocity.z = 0
bt_missions.set_v_pub.publish(vel)
bt_missions.rate.sleep()
def run(self):
while True:
if bt_missions.isContinue == False:
break
bt_count = self.tree.blackboard(1)
bt_state = bt_count.tick()
print("bt state = %s\n" % bt_state)
while bt_count == RUNNING:
bt_state = bt_count.tick()
print("state = %s\n" % bt_state)
assert bt_state == SUCCESS or bt_state == FAILURE
