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 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 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])