def goto(self,
waypoint,
duration=rospy.Duration(60, 0),
low_fuel_trip=False):
"""
Go to specific waypoint action
"""
init_fuel = self.fuel
start = rospy.Time.now()
wp = self.waypoints[waypoint - 1]
original = GlobalPositionTarget()
original.header.seq = 1
original.header.stamp = rospy.Time.now()
original.header.frame_id = 'map'
original.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_REL_ALT
original.type_mask = 0b001111111000
original.latitude = wp['lat']
original.longitude = wp['long']
original.altitude = 0.0
original.yaw = yaw_ned_to_enu(wp['yaw'])
original.yaw_rate = 2.0
rospy.loginfo("%s is going to waypoint %d, lat: %.5f, long: %.5f" %
(self.namespace, waypoint, wp['lat'], wp['long']))
reached_original = self.goto_coordinate(original,
low_fuel_trip=low_fuel_trip,
radius=self._radius,
duration=duration)
rospy.loginfo("%s is reaching waypoint %d, lat: %.5f, long: %.5f" %
(self.namespace, waypoint, wp['lat'], wp['long']))
self.calculate_fuel_rate(init_fuel, start, self.fuel_rate_std)
return reached_original
def inspect_wt(self, duration=rospy.Duration(600, 0)):
"""
ASV inspection for a WT that involves pointing a camera
to the WT while ASV is moving back and forth
"""
fuel = self.fuel
start = rospy.Time.now()
# position where drone is originated in one of the wps
wp = self.waypoints[self._current_wp - 1]
original = GlobalPositionTarget()
original.header.seq = 1
original.header.stamp = rospy.Time.now()
original.header.frame_id = 'map'
original.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_REL_ALT
original.type_mask = 0b001111111000
original.latitude = wp['lat']
original.longitude = wp['long']
original.altitude = 0.0
original.yaw = yaw_ned_to_enu(wp['yaw'])
original.yaw_rate = 2.0
rospy.loginfo("%s is scanning a wind turbine..." % self.namespace)
first_blade = self.blade_inspect(original, [0.0, 90.0, 0.0], duration)
self.calculate_fuel_rate(fuel, start, self.fuel_rate_std)
rospy.loginfo("%s has done the inspection..." % self.namespace)
return first_blade
def blade_inspect(self,
original,
target_position,
duration=rospy.Duration(300, 0)):
"""
Inspecting the blade given the [original] pose to return to
and end [target] position to scan
"""
start = rospy.Time.now()
reached_original = self.ACTION_FAIL
# rotate the camera
for _ in range(3):
self._rotate_cam.publish(Int32(1))
rospy.sleep(0.1)
# position where ASV is supposed to be
heading = self.heading
offset_x = (target_position[0] * np.cos(heading) +
target_position[1] * np.sin(heading))
offset_y = (-1 * target_position[0] * np.sin(heading) +
target_position[1] * np.cos(heading))
latitude_offset, longitude_offset = xy_to_longlat(
offset_x, offset_y, self.global_pose.latitude)
target = GlobalPositionTarget()
target.header.seq = 1
target.header.stamp = rospy.Time.now()
target.header.frame_id = 'map'
target.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_REL_ALT
target.type_mask = 0b001111111000
target.latitude = self.global_pose.latitude + latitude_offset
target.longitude = self.global_pose.longitude + longitude_offset
target.altitude = target_position[2]
target.yaw = yaw_ned_to_enu(heading)
target.yaw_rate = 2.0
duration = duration - (rospy.Time.now() - start)
start = rospy.Time.now()
reached_target = self.goto_coordinate(target,
low_fuel_trip=False,
duration=duration)
rospy.loginfo("%s is returning to %3.5f, %3.5f position..." %
(self.namespace, original.latitude, original.longitude))
if reached_target == self.ACTION_SUCCESS:
# rotate the camera
for _ in range(3):
self._rotate_cam.publish(Int32(-1))
rospy.sleep(0.1)
original.header.seq = 1
original.header.stamp = rospy.Time.now()
original.header.frame_id = 'map'
duration = duration - (rospy.Time.now() - start)
start = rospy.Time.now()
reached_original = self.goto_coordinate(original,
low_fuel_trip=False,
duration=duration)
# rotate the camera
for _ in range(3):
self._rotate_cam.publish(Int32(0))
rospy.sleep(0.1)
return np.min([reached_target, reached_original])
def publish_target_pose(self, pose_s):
coords = self.pose2wgs84(pose_s)
if not coords:
rospy.logerr('Cannot compute pose in UTM frame')
return
lat, lon, heading = coords
msg = GlobalPositionTarget()
msg.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_INT
msg.type_mask = (GlobalPositionTarget.IGNORE_VX +
GlobalPositionTarget.IGNORE_VY +
GlobalPositionTarget.IGNORE_VZ +
GlobalPositionTarget.IGNORE_AFX +
GlobalPositionTarget.IGNORE_AFY +
GlobalPositionTarget.IGNORE_AFZ +
GlobalPositionTarget.IGNORE_YAW +
GlobalPositionTarget.IGNORE_YAW_RATE)
msg.latitude = lat
msg.longitude = lon
msg.altitude = 0.0
msg.yaw = heading
self.pub.publish(msg)
self.pub_pose.publish(pose_s)
def publish_target_pose(self, pose):
point = _a(pose.pose.position)
# print 'point' , point
yaw = np.arctan2(pose.pose.orientation.z,
pose.pose.orientation.w) * 2.0
latlon, heading = self.pose2wgs84(point, yaw)
lon, lat = latlon.tolist()[0]
# print lon, lat
msg = GlobalPositionTarget()
msg.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_INT
msg.type_mask = (GlobalPositionTarget.IGNORE_VX +
GlobalPositionTarget.IGNORE_VY +
GlobalPositionTarget.IGNORE_VZ +
GlobalPositionTarget.IGNORE_AFX +
GlobalPositionTarget.IGNORE_AFY +
GlobalPositionTarget.IGNORE_AFZ +
GlobalPositionTarget.IGNORE_YAW +
GlobalPositionTarget.IGNORE_YAW_RATE)
msg.latitude = lat
msg.longitude = lon
msg.altitude = 0.0
msg.yaw = heading
self.pub.publish(msg)
self.pub_pose.publish(pose)
def do_helix_trajectory(self):
while (not self.home_pose_set) or (not self.global_home_pose_set):
# do nothing, wait for node initialization to finish
a = 1
ladder_position = self.get_ladder_location()
init_x = self.home_pose.pose.position.x #self.state.pose.pose.position.x
init_y = self.home_pose.pose.position.y #self.state.pose.pose.position.y
init_z = self.home_pose.pose.position.z #self.state.pose.pose.position.z
print("Initial helix position: \n x: {}, y: {}, z: {}\nTime: {}\n".
format(init_x, init_y, init_z,
rospy.Time.now().to_sec()))
self.helix_controller = htc(vrate=0.1,
radius=2.0,
center=(1, 0, 0),
init=(init_x, init_y, init_z),
t_init=rospy.Time.now().to_sec(),
w=0.2) # init with any parameters
self.helix_controller.set_helix_center(ladder_position)
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)
while (self.state.pose.pose.position.z <
(init_z + self.ladder_height + self.ladder_safety_margin)):
# 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 = [-29.5,12.5]
#self.helix_controller.set_helix_center(ladder_position)
ux, uy, uz, ref = self.helix_controller.compute_command(
states,
rospy.Time.now().to_sec())
ux, uy = (ref[0][0][0], ref[1][0][0])
q, cyaw = self.compute_yaw2(ladder_position)
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.yaw = cyaw
global_refmsg.altitude = uz #(uz - init_z) + self.global_home.altitude
self.ref_pub_global.publish(global_refmsg)
rate.sleep()
wp = GlobalPositionTarget() wp_target = GlobalPositionTarget() wp_avoidance = GlobalPositionTarget() wp_helical = GlobalPositionTarget() wp0.coordinate_frame = GlobalPositionTarget().FRAME_GLOBAL_INT wp0.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 wp0.latitude = 37.565011 wp0.longitude = 126.628919 wp0.altitude = 5.0 wp0.yaw = np.pi / 180.0 * 117.0 wp1.coordinate_frame = wp0.coordinate_frame wp1.type_mask = wp0.type_mask wp1.latitude = 37.566025 wp1.longitude = 126.628206 wp1.altitude = 100.0 wp1.yaw = np.pi / 180.0 * 117.0 wp2.coordinate_frame = wp0.coordinate_frame wp2.type_mask = wp0.type_mask wp2.latitude = 37.565350 wp2.longitude = 126.626778 wp2.altitude = 100.0 wp2.yaw = np.pi / 180.0 * 210.0
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)
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()
global_pos_pub.publish(point)
rate.sleep()
print("Creating Objects for services")
offb_set_mode = SetMode()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBool()
arm_cmd.value = True
last_request = rospy.Time.now()
start_time = rospy.Time.now()