def update(self):
mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ)
if mission_plan is None or mission_plan.is_complete():
return pt.Status.FAILURE
if self.no_service() or not self.service_ok:
# there is no path planner, just copy the coarse points to the refined side
mission_plan.set_refined_waypoints(mission_plan.waypoints)
return pt.Status.SUCCESS
if len(mission_plan.waypoints) <= 1:
# there is literally just one point, cant plan for that apparently
mission_plan.set_refined_waypoints(mission_plan.waypoints)
return pt.Status.SUCCESS
# give the location of the auv as the first point in the plan
# to prevent overshooting the first real waypoint
ps = self.bb.get(bb_enums.LOCATION_POINT_STAMPED)
trajectory_response = self.path_planner(
mission_plan.get_pose_array(ps))
refined_path = trajectory_response.fine
refined_path.header.frame_id = 'map'
self.path_pub.publish(refined_path)
mission_plan.set_refined_waypoints(
mission_plan.path_to_list(refined_path))
rospy.loginfo_throttle_identical(
10, "Refined waypoints length:" +
str(len(mission_plan.refined_waypoints)))
return pt.Status.SUCCESS
def handle_request_get_state(self, plandb_msg):
rospy.loginfo_throttle_identical(
30, "Got REQUEST GET_STATE planDB msg from Neptus")
current_mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ)
if current_mission_plan is None:
return
# https://github.com/LSTS/imcjava/blob/d95fddeab4c439e603cf5e30a32979ad7ace5fbc/src/java/pt/lsts/imc/adapter/PlanDbManager.java#L160
# See above for an example
# TODO it seems like we need to keep a planDB ourselves on this side, collect all the plans we
# received and answer this get_state with data from them all.
# lets try telling neptus that we just got one plan, maybe that'll be okay?
# seems alright, but after this message is sent, the plan goes red :/
response = PlanDB()
response.plan_id = current_mission_plan.plan_id
response.type = imc_enums.PLANDB_TYPE_SUCCESS
response.op = imc_enums.PLANDB_OP_GET_STATE
response.plandb_state = PlanDBState()
response.plandb_state.plan_count = 1
response.plandb_state.plans_info.append(self.make_plandb_info())
self.plandb_pub.publish(response)
rospy.loginfo_throttle_identical(
30, "Answered GET_STATE for plan:\n" + str(response.plan_id))
def _timer_callback(self, event):
if not self._enable_path_tracer:
rospy.loginfo_throttle_identical(1, "Path execution is disabled")
return
if self._utm_path is None:
rospy.logerr("Wayfinding is not possible, the path is empty!")
return
if self._currently_executing_plan and not self._move_base_observer.move_base_is_active(
):
rospy.logwarn("Waypoint reached!")
self._currently_executing_plan = False
# Notice that this crapy-ass algorithm depend on the timer callback being slow enough
# so that we can see an update status on the move base action server before the next
# time the callback gets called, or we might trigger the goal more than once before
# it actually gets executed
if not self._currently_executing_plan:
# Keep executing the plan in a loop
self._utm_path_index += 1
if self._utm_path_index >= len(self._utm_path):
self._utm_path_index = 0
self._plan_path()
self._currently_executing_plan = True
rospy.logwarn("Setting sails to new goal!: {}".format(
self._utm_path[self._utm_path_index]))
self._move_base_simple_goal_pub.publish(
self._utm_path[self._utm_path_index])
def angle_wrap(self, angle):
if (abs(angle) > 3.141516):
angle = angle - (
abs(angle) /
angle) * 2 * 3.141516 #Angle wrapping between -pi and pi
rospy.loginfo_throttle_identical(20, "Angle Error Wrapped")
return angle
def update(self):
plan_is_go = self.bb.get(bb_enums.PLAN_IS_GO)
self.feedback_message = "Plan is go:{}".format(plan_is_go)
if plan_is_go is None or plan_is_go == False:
rospy.loginfo_throttle_identical(5, "Waiting for start plan")
return pt.Status.FAILURE
return pt.Status.SUCCESS
def update(self):
poi = self.bb.get(bb_enums.POI_POINT_STAMPED)
if poi is None:
rospy.loginfo_throttle_identical(10, "No POI :(")
return pt.Status.SUCCESS
if self._last_known_poi is None:
# a poi exists but we didnt know any beforehand, new poi!
self._last_known_poi = poi
rospy.logwarn_throttle_identical(10, "Our first POI!")
return pt.Status.FAILURE
# we knew a poi, there is a poi we see, far enough?
xdiff = poi.point.x - self._last_known_poi.point.x
ydiff = poi.point.y - self._last_known_poi.point.y
zdiff = poi.point.z - self._last_known_poi.point.z
dist = math.sqrt(xdiff**2 + ydiff**2 + zdiff**2)
# its far enough!
if dist > self.new_poi_distance:
self._last_known_poi = poi
rospy.logwarn_throttle_identical(
10, "A new POI that is far enough!" + str(dist))
return pt.Status.FAILURE
# aww, not far enough
self._last_known_poi = poi
rospy.loginfo_throttle_identical(10,
"Probably the same POI as before...")
return pt.Status.SUCCESS
def update(self):
mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ)
if mission_plan is None or not mission_plan.is_complete():
rospy.loginfo_throttle_identical(5, "Plan is not done")
return pt.Status.FAILURE
self.feedback_message = "Current plan:{}".format(mission_plan.plan_id)
return pt.Status.SUCCESS
def _twist_callback(self, twist_msg):
self._expected_linear_speed = twist_msg.linear.x
self._expected_angular_speed = twist_msg.angular.z
self._latest_command_timestamp = rospy.Time.now()
rospy.loginfo_throttle_identical(
1, "Linear speed setpoint: %f" % (self._expected_linear_speed))
rospy.loginfo_throttle_identical(
1, "Angular speed setpoint: %f" % (self._expected_angular_speed))
def handle_set_plan(self, plandb_msg):
# there is a plan we can at least look at
mission_plan = MissionPlan(plan_frame=self.utm_link,
local_frame=self.local_link,
plandb_msg=plandb_msg)
self.bb.set(bb_enums.MISSION_PLAN_OBJ, mission_plan)
self.bb.set(bb_enums.ENABLE_AUTONOMY, False)
rospy.loginfo_throttle_identical(
5, "Set the mission plan to:" + str(mission_plan.waypoints))
def respond_set_success(self):
current_mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ)
if current_mission_plan is None:
rospy.logwarn_throttle_identical(30, "No mission plan obj!")
return
plan_id = current_mission_plan.plan_id
self.plandb_msg.plan_id = plan_id
self.plandb_pub.publish(self.plandb_msg)
rospy.loginfo_throttle_identical(30, "Answered set success for plan_id:"+str(plan_id))
def _black_box_pose_callback(self, point_msg):
rospy.loginfo_throttle_identical(
3, "Receiving infomation from the acoustic sensor...")
pose = PoseStamped()
pose.header = point_msg.header
pose.pose.position = point_msg.point
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
self._black_box_pose = pose
def handle_set_plan(self, plandb_msg):
# there is a plan we can at least look at
mission_plan = MissionPlan(plan_frame = self.utm_link,
plandb_msg = plandb_msg,
latlontoutm_service_name = self.latlontoutm_service_name)
self.bb.set(bb_enums.MISSION_PLAN_OBJ, mission_plan)
self.bb.set(bb_enums.ENABLE_AUTONOMY, False)
self.bb.set(bb_enums.MISSION_FINALIZED, False)
rospy.loginfo_throttle_identical(5, "Set the mission plan to:{} and un-finalized the mission.".format(mission_plan))
def handle_plandb_msg(self):
plandb_msg = self.latest_plandb_msg
if plandb_msg is None:
return
typee = plandb_msg.type
op = plandb_msg.op
# request get_info
if typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_GET_INFO:
self.handle_request_get_info(plandb_msg)
elif typee == imc_enums.PLANDB_TYPE_REQUEST and op == imc_enums.PLANDB_OP_GET_STATE:
self.handle_request_get_state(plandb_msg)
elif typee == imc_enums.PLANDB_TYPE_SUCCESS and op == imc_enums.PLANDB_OP_SET:
rospy.loginfo_throttle_identical(
20, "Received SUCCESS for plandb set")
elif typee == imc_enums.PLANDB_TYPE_SUCCESS and op == imc_enums.PLANDB_OP_GET_INFO:
rospy.loginfo_throttle_identical(
20, "Received SUCCESS for plandb get info")
elif typee == imc_enums.PLANDB_TYPE_SUCCESS and op == imc_enums.PLANDB_OP_GET_STATE:
rospy.loginfo_throttle_identical(
20, "Received SUCCESS for plandb get state")
elif op == imc_enums.PLANDB_OP_SET:
self.handle_set_plan(plandb_msg)
else:
rospy.loginfo_throttle_identical(
5,
"Received some unhandled planDB message:\n" + str(plandb_msg))
def update(self):
current_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ)
if current_plan is None:
# there is no plan, it can not change
self.last_known_id = None
self.last_known_time = 0
rospy.loginfo_throttle_identical(
10, "There was no plan, plan is not changed")
return pt.Status.SUCCESS
if self.last_known_id is None:
# this is the first plan we saw
# record it, and let the tree tick again
self.last_known_id = current_plan.plan_id
self.last_known_time = current_plan.creation_time
rospy.loginfo_throttle_identical(
10, "First time seeing any plan, plan is changed")
return pt.Status.FAILURE
if self.last_known_id == current_plan.plan_id and self.last_known_time < current_plan.creation_time:
# this is the same plan, but it was sent again, this means a restart
# so the plan IS changed
rospy.loginfo_throttle_identical(
10,
"Same plan_id, but the current plan is newer, plan is changed")
self.last_known_id = current_plan.plan_id
self.last_known_time = current_plan.creation_time
return pt.Status.FAILURE
rospy.loginfo_throttle_identical(60, "Plan is not changed")
return pt.Status.SUCCESS
def update(self):
# UNTESTED STUFF HERE, RETURN FAILURE TO KEEP PPL
# FROM USING THIS ACTION
return pt.Status.FAILURE
if not self.poi_link_available:
return pt.Status.FAILURE
poi = self.bb.get(bb_enums.POI_POINT_STAMPED)
if poi is None:
return pt.Status.SUCCESS
poi_local = self.tf_listener.transformPoint(self.utm_link, poi)
x = poi_local.point.x
y = poi_local.point.y
depth = poi.point.z
# construct the waypoints that we want to go to
inspection_depth = max(1, depth - 5)
radius = 10
# go east,west,north,south,center
# so we do bunch of fly-overs
waypoints = [
(x+radius, y, inspection_depth),
(x-radius, y, inspection_depth),
(x, y+radius, inspection_depth),
(x, y-radius, inspection_depth),
(x, y, 0)
]
waypoint_man_ids = ['east', 'west', 'north', 'south', 'surface_center']
# construct a planDB message to be given to the mission_plan
# we will not fill the plan_spec of this plandb message,
# and instead call a different constructor of MissionPlan
# to bypass the lat/lon stuff
pdb = PlanDB()
pdb.request_id = 42
pdb.plan_id = "POI"
# set it in the tree
mission_plan = MissionPlan(plan_frame = self.utm_link,
plandb_msg = pdb,
waypoints = waypoints,
waypoint_man_ids=waypoint_man_ids,
latlontoutm_service_name = self.latlontoutm_service_name)
self.bb.set(bb_enums.MISSION_PLAN_OBJ, mission_plan)
rospy.loginfo_throttle_identical(5, "Due to POI, set the mission plan to:"+str(mission_plan))
return pt.Status.SUCCESS
def handle_request_get_info(self, plandb_msg):
# we need to respond to this with some info... but what?
rospy.loginfo_throttle_identical(30, "Got REQUEST GET_INFO planDB msg from Neptus")
current_mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ)
if current_mission_plan is None:
return
response = PlanDB()
response.plan_id = current_mission_plan.plan_id
response.type = imc_enums.PLANDB_TYPE_SUCCESS
response.op = imc_enums.PLANDB_OP_GET_INFO
response.plandb_information = self.make_plandb_info()
self.plandb_pub.publish(response)
rospy.loginfo_throttle_identical(30, "Answered GET_INFO for plan:"+str(response.plan_id))
def timer_callback(self, event):
if self.nav_goal is None:
#rospy.loginfo_throttle(30, "Nav goal is None!")
return
try:
(trans,
rot) = self.listener.lookupTransform(self.nav_goal_frame,
self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
# TODO: we could use this code for the other check also
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
#print("Checking if nav goal is reached!")
start_pos = np.array(trans)
end_pos = np.array([
self.nav_goal.position.x, self.nav_goal.position.y,
self.nav_goal.position.z
])
# We check for success out of the main control loop in case the main control loop is
# running at 300Hz or sth. like that. We dont need to check succes that frequently.
xydiff = start_pos[:2] - end_pos[:2]
zdiff = np.abs(np.abs(start_pos[2]) - np.abs(end_pos[2]))
xydiff_norm = np.linalg.norm(xydiff)
# rospy.logdebug("diff xy:"+ str(xydiff_norm)+' z:' + str(zdiff))
rospy.loginfo_throttle_identical(5, "Using Crosstrack Error")
rospy.loginfo_throttle_identical(
5,
"diff xy:" + str(xydiff_norm) + ' z:' + str(zdiff) + " WP tol:" +
str(self.wp_tolerance) + "Depth tol:" + str(self.depth_tolerance))
if xydiff_norm < self.wp_tolerance and zdiff < self.depth_tolerance:
rospy.loginfo("Reached WP!")
self.x_prev = self.nav_goal.position.x
self.y_prev = self.nav_goal.position.y
self.nav_goal = None
self._result.reached_waypoint = True
def update(self):
mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ)
if mission_plan is None:
msg = "No plan received yet"
self.feedback_message = msg
rospy.loginfo_throttle(5, msg)
return pt.Status.FAILURE
elif not mission_plan.is_complete():
msg = "Progress:{}/{} on plan {}".format(
mission_plan.current_wp_index, len(mission_plan.waypoints),
mission_plan.plan_id)
self.feedback_message = msg
rospy.loginfo_throttle_identical(5, msg)
return pt.Status.FAILURE
rospy.loginfo_throttle_identical(2, "Plan is complete!")
self.feedback_message = "Current plan:{}".format(mission_plan.plan_id)
return pt.Status.SUCCESS
def _task_callback(self, task_msg):
task_name = task_msg.name
task_state = task_msg.state
task_ready_time = task_msg.ready_time
task_running_time = task_msg.running_time
task_elapsed_time = task_msg.elapsed_time
task_current_score = task_msg.score
if task_state != self._current_task_state:
self._current_task_state = task_state
self._transition_to_state(task_name, task_state)
rospy.loginfo("Task message received:")
rospy.loginfo(" - Task name : {}".format(task_name))
rospy.loginfo(" - Task state : {}".format(task_state))
rospy.loginfo(" - Ready time : {}".format(task_ready_time))
rospy.loginfo(" - Running time : {}".format(task_running_time))
rospy.loginfo(" - Elapsed time : {}".format(task_elapsed_time))
rospy.loginfo_throttle_identical(5.0, "Current task score : {:.2f}".format(task_current_score))
def _move_base_status_callback(self, status_array):
# Check if there's any goal active in the action server
def is_active(status):
return (status == GoalStatus.ACTIVE) or (
status == GoalStatus.PREEMPTING) or (status
== GoalStatus.RECALLING)
a_goal_is_active = False
for entry in status_array.status_list:
a_goal_is_active = a_goal_is_active or is_active(entry.status)
if a_goal_is_active:
rospy.loginfo_throttle_identical(
3.0, "A goal is active, station keeping is disabled")
else:
rospy.loginfo_throttle_identical(
3, "Station keeping enabled%s" %
(", but no goal set" if not self._framing_box_corners else ""))
self._station_keeping_is_enabled = not a_goal_is_active
def update(self):
mission_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ)
if mission_plan is None:
rospy.logwarn_throttle(5, "Mission plan was None!")
return pt.Status.FAILURE
if not self.do_not_visit:
mission_plan.visit_wp()
next_action = mission_plan.get_current_wp()
if next_action is None:
self.feedback_message = "Next action was None"
rospy.logwarn_throttle(5, "Mission is complete:{}".format(mission_plan.is_complete()))
return pt.Status.FAILURE
rospy.loginfo_throttle_identical(5, "Set CURRENT_PLAN_ACTION {} to: {}".format(self.do_not_visit, str(next_action)))
self.bb.set(bb_enums.CURRENT_PLAN_ACTION, next_action)
return pt.Status.SUCCESS
def update(self):
current_plan = self.bb.get(bb_enums.MISSION_PLAN_OBJ)
if current_plan is None:
# there is no plan, it can not change
self.last_known_id = None
self.last_known_time = 0
self.feedback_message = "There was no plan, plan is not changed"
rospy.loginfo_throttle_identical(10, self.feedback_message)
return pt.Status.SUCCESS
if self.last_known_id is None:
# this is the first plan we saw
# record it, and let the tree tick again
self.last_known_id = current_plan.plan_id
self.last_known_time = current_plan.creation_time
self.feedback_message = "First time seeing any plan, plan is changed"
rospy.loginfo_throttle_identical(10, self.feedback_message)
return pt.Status.FAILURE
if self.last_known_id == current_plan.plan_id and self.last_known_time < current_plan.creation_time:
# this is the same plan, but it was sent again, this means a restart
# so the plan IS changed
self.feedback_message = "Same plan_id, but the current plan is newer, plan is changed"
rospy.loginfo_throttle_identical(10, self.feedback_message)
self.last_known_id = current_plan.plan_id
self.last_known_time = current_plan.creation_time
self.bb.set(bb_enums.PLAN_IS_GO, False)
return pt.Status.FAILURE
if self.last_known_id != current_plan.plan_id:
# the plan has been changed completely
self.feedback_message = "A new plan is received!"
rospy.loginfo_throttle_identical(10, self.feedback_message)
self.last_known_id = current_plan.plan_id
self.last_known_time = current_plan.creation_time
self.bb.set(bb_enums.PLAN_IS_GO, False)
return pt.Status.FAILURE
self.feedback_message = "last_id:{}, current_id:{}".format(
self.last_known_id, current_plan.plan_id)
rospy.loginfo_throttle_identical(10, self.feedback_message)
return pt.Status.SUCCESS
def update(self):
data = self.bb.get(bb_enums.GPS_FIX)
if (data is None or data.latitude is None or data.latitude == 0.0
or data.longitude is None or data.latitude == 0.0
or data.status.status == -1):
rospy.loginfo_throttle_identical(
self._spam_period,
"GPS lat/lon are 0s or Nones, cant set utm zone/band from these >:( "
)
# shitty gps
self._spam_period = min(self._spam_period * 2,
self._max_spam_period)
return pt.Status.SUCCESS
self.gps_zone, self.gps_band = fromLatLong(data.latitude,
data.longitude).gridZone()
if self.gps_zone is None or self.gps_band is None:
rospy.logwarn_throttle_identical(
10, "gps zone and band from fromLatLong was None")
return pt.Status.SUCCESS
# first read the UTMs given by ros params
prev_band = self.bb.get(bb_enums.UTM_BAND)
prev_zone = self.bb.get(bb_enums.UTM_ZONE)
if prev_zone != self.gps_zone or prev_band != self.gps_band:
rospy.logwarn_once(
"PREVIOUS UTM AND GPS_FIX UTM ARE DIFFERENT!\n Prev:" +
str((prev_zone, prev_band)) + " gps:" +
str((self.gps_zone, self.gps_band)))
if common_globals.TRUST_GPS:
rospy.logwarn_once("USING GPS UTM!")
self.bb.set(bb_enums.UTM_ZONE, self.gps_zone)
self.bb.set(bb_enums.UTM_BAND, self.gps_band)
else:
rospy.logwarn_once("USING PREVIOUS UTM!")
self.bb.set(bb_enums.UTM_ZONE, prev_zone)
self.bb.set(bb_enums.UTM_BAND, prev_band)
return pt.Status.SUCCESS
def update(self):
# try not to call the service every tick...
dvl_is_running = self.bb.get(bb_enums.DVL_IS_RUNNING)
if dvl_is_running is not None:
if dvl_is_running == self.sb.data:
rospy.loginfo_throttle_identical(20, "DVL is already running:"+str(self.sb.data))
return pt.Status.SUCCESS
# check if enough time has passed since last call
t = time.time()
if t - self.last_toggle < self.cooldown:
# nope, return running while we wait
rospy.loginfo_throttle_identical(5, "Waiting on DVL toggle cooldown")
return pt.Status.RUNNING
try:
ret = self.switcher_service(self.running)
except rospy.service.ServiceException:
rospy.logwarn_throttle_identical(60, "DVL Start/stop service not found! Succeeding by default namespace:{}".format(self.service_name))
return pt.Status.SUCCESS
if ret.success:
rospy.loginfo_throttle_identical(5, "DVL TOGGLED:"+str(self.sb.data))
self.last_toggle = time.time()
self.bb.set(bb_enums.DVL_IS_RUNNING, self.sb.data)
return pt.Status.SUCCESS
rospy.logwarn_throttle_identical(5, "DVL COULD NOT BE TOGGLED:{}, ret:{}".format(self.sb.data, ret))
return pt.Status.FAILURE
def toggle(self, enable_):
#function that toggles the service, that can be called from the code
ret = self.toggle_ctrl_service(enable_)
if ret.success:
rospy.loginfo_throttle_identical(5, "Controller toggled")
def _pose_callback(self, pose_msg):
if self._state_timestamp is None:
self._state_timestamp = pose_msg.header.stamp
return
if not self._controller_enabled:
rospy.logwarn_throttle_identical(
1, "Engine controller disabled, control loop disengaged")
return
# Validate input based on speed estimation. This is just a hack to
# solve a transient in the EKF filter during startup.
# TODO Figure out why there's a transient in the ekf filter during startup
absolute_velocity = math.sqrt(pose_msg.twist.twist.linear.x**2 +
pose_msg.twist.twist.linear.y**2)
if (absolute_velocity > self._max_valid_odometry_velocity):
# Ignore this sample
rospy.logwarn_throttle_identical(
3, "Ignoring pose message, speed out of range")
return
# If we haven't received a command in a while, reset expected values
now = rospy.Time.now()
age_of_command = (now - self._latest_command_timestamp).to_sec()
if (age_of_command > self._max_command_duration):
self._latest_command_timestamp = now
rospy.logwarn(
"Ran too long with no commands, resetting loop setpoints to zero."
)
self._reset_control_setpoints()
delta_t = (pose_msg.header.stamp - self._state_timestamp).to_sec()
self._state_timestamp = pose_msg.header.stamp
rospy.loginfo_throttle_identical(1, "Loop time delta: %f" % (delta_t))
current_linear_speed = pose_msg.twist.twist.linear.x
current_angular_speed = pose_msg.twist.twist.angular.z
rospy.loginfo(
"Loop inputs:\n linear setpoint=%f\n linear feedback=%f\n angular setpoint=%f\n angular feedback=%f"
% (self._expected_linear_speed, current_linear_speed,
self._expected_angular_speed, current_angular_speed))
linear_speed_error = self._expected_linear_speed - current_linear_speed
angular_speed_error = self._expected_angular_speed - current_angular_speed
linear_speed_control_action = self._linear_control.update_filter_state(
linear_speed_error, delta_t)
angular_speed_control_action = self._angular_control.update_filter_state(
angular_speed_error, delta_t)
within_actuator_range = self.send_engine_messages(
linear_speed_control_action - angular_speed_control_action,
linear_speed_control_action + angular_speed_control_action)
if (not within_actuator_range):
rospy.logwarn(
"Actuators outside of control range, anti-windup activated!")
self._linear_control.reset_state()
self._angular_control.reset_state()
def _station_keeping(self):
# do nothing, it'll take care of itself
rospy.loginfo_throttle_identical(
2.0, "Station keeping, no more execution stages")
def test_log(self):
rosout_logger = logging.getLogger('rosout')
import rospy
self.assertTrue(len(rosout_logger.handlers) == 2)
self.assertTrue(rosout_logger.handlers[0],
rosgraph.roslogging.RosStreamHandler)
self.assertTrue(rosout_logger.handlers[1],
rospy.impl.rosout.RosOutHandler)
default_ros_handler = rosout_logger.handlers[0]
# Remap stdout for testing
lout = StringIO()
lerr = StringIO()
test_ros_handler = rosgraph.roslogging.RosStreamHandler(colorize=False,
stdout=lout,
stderr=lerr)
try:
# hack to replace the stream handler with a debug version
rosout_logger.removeHandler(default_ros_handler)
rosout_logger.addHandler(test_ros_handler)
import rospy
rospy.loginfo("test 1")
lout_last = lout.getvalue().splitlines()[-1]
self.assert_("test 1" in lout_last)
rospy.logwarn("test 2")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[WARN]" in lerr_last)
self.assert_("test 2" in lerr_last)
rospy.logerr("test 3")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[ERROR]" in lerr_last)
self.assert_("test 3" in lerr_last)
rospy.logfatal("test 4")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[FATAL]" in lerr_last)
self.assert_("test 4" in lerr_last)
# logXXX_once
lout_len = -1
for i in range(3):
rospy.loginfo_once("test 1")
lout_last = lout.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 1" in lout_last)
lout_len = len(lout.getvalue())
else: # making sure the length of lout doesnt change
self.assert_(lout_len == len(lout.getvalue()))
lerr_len = -1
for i in range(3):
rospy.logwarn_once("test 2")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 2" in lerr_last)
lerr_len = len(lerr.getvalue())
else: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
lerr_len = -1
for i in range(3):
rospy.logerr_once("test 3")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 3" in lerr_last)
lerr_len = len(lerr.getvalue())
else: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
lerr_len = -1
for i in range(3):
rospy.logfatal_once("test 4")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 4" in lerr_last)
lerr_len = len(lerr.getvalue())
else: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
# logXXX_throttle
lout_len = -1
for i in range(3):
rospy.loginfo_throttle(3, "test 1")
lout_last = lout.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 1" in lout_last)
lout_len = len(lout.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lout doesnt change
self.assert_(lout_len == len(lout.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 1" in lout_last)
lerr_len = -1
for i in range(3):
rospy.logwarn_throttle(3, "test 2")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 2" in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 2" in lerr_last)
lerr_len = -1
for i in range(3):
rospy.logerr_throttle(3, "test 3")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 3" in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 3" in lerr_last)
lerr_len = -1
for i in range(3):
rospy.logfatal_throttle(3, "test 4")
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0:
self.assert_("test 4" in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(1))
elif i == 1: # making sure the length of lerr doesnt change
self.assert_(lerr_len == len(lerr.getvalue()))
rospy.sleep(rospy.Duration(2))
else:
self.assert_("test 4" in lerr_last)
# logXXX_throttle_identical
lout_len = -1
for i in range(5):
if i < 2:
test_text = "test 1"
else:
test_text = "test 1a"
rospy.loginfo_throttle_identical(2, test_text)
lout_last = lout.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lout_last)
lout_len = len(lout.getvalue())
elif i == 1: # Confirm the length of lout hasn't changed
self.assert_(lout_len == len(lout.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lout_last)
lout_len = len(lout.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lout has changed
self.assert_(lout_len != len(lout.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lout_last)
lerr_len = -1
for i in range(5):
if i < 2:
test_text = "test 2"
else:
test_text = "test 2a"
rospy.logwarn_throttle_identical(2, test_text)
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
elif i == 1: # Confirm the length of lerr hasn't changed
self.assert_(lerr_len == len(lerr.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lerr has incremented
self.assert_(lerr_len != len(lerr.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lerr_last)
lerr_len = -1
for i in range(5):
if i < 2:
test_text = "test 3"
else:
test_text = "test 3a"
rospy.logerr_throttle_identical(2, test_text)
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
elif i == 1: # Confirm the length of lerr hasn't changed
self.assert_(lerr_len == len(lerr.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lerr has incremented
self.assert_(lerr_len != len(lerr.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lerr_last)
lerr_len = -1
for i in range(5):
if i < 2:
test_text = "test 4"
else:
test_text = "test 4a"
rospy.logfatal_throttle_identical(2, test_text)
lerr_last = lerr.getvalue().splitlines()[-1]
if i == 0: # Confirm test text was logged
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
elif i == 1: # Confirm the length of lerr hasn't changed
self.assert_(lerr_len == len(lerr.getvalue()))
elif i == 2: # Confirm the new message was logged correctly
self.assert_(test_text in lerr_last)
lerr_len = len(lerr.getvalue())
rospy.sleep(rospy.Duration(2))
elif i == 3: # Confirm the length of lerr has incremented
self.assert_(lerr_len != len(lerr.getvalue()))
else: # Confirm new test text is in last log
self.assert_(test_text in lerr_last)
rospy.loginfo("test child logger 1", logger_name="log1")
lout_last = lout.getvalue().splitlines()[-1]
self.assert_("test child logger 1" in lout_last)
rospy.logwarn("test child logger 2", logger_name="log2")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[WARN]" in lerr_last)
self.assert_("test child logger 2" in lerr_last)
rospy.logerr("test child logger 3", logger_name="log3")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[ERROR]" in lerr_last)
self.assert_("test child logger 3" in lerr_last)
rospy.logfatal("test child logger 4", logger_name="log4")
lerr_last = lerr.getvalue().splitlines()[-1]
self.assert_("[FATAL]" in lerr_last)
self.assert_("test child logger 4" in lerr_last)
finally:
# restoring default ros handler
rosout_logger.removeHandler(test_ros_handler)
lout.close()
lerr.close()
rosout_logger.addHandler(default_ros_handler)
def execute_cb(self, goal):
rospy.loginfo("Goal received")
#success = True
self.nav_goal = goal.waypoint_pose.pose
self.nav_goal_frame = goal.waypoint_pose.header.frame_id
if self.nav_goal_frame is None or self.nav_goal_frame == '':
rospy.logwarn("Goal has no frame id! Using utm by default")
self.nav_goal_frame = 'utm' #'utm'
self.nav_goal.position.z = goal.travel_depth # assign waypoint depth from neptus, goal.z is 0.
if goal.speed_control_mode == 2:
self.vel_ctrl_flag = 1 # check if NEPTUS sets a velocity
elif goal.speed_control_mode == 1:
self.vel_ctrl_flag = 0 # use RPM ctrl
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_local = self.listener.transformPoint(
self.nav_goal_frame, goal_point)
self.nav_goal.position.x = goal_point_local.point.x
self.nav_goal.position.y = goal_point_local.point.y
self.nav_goal.position.z = goal_point_local.point.z
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("Not transforming point to world local")
pass
rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x)
r = rospy.Rate(11.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
#self.toggle_yaw_ctrl.toggle(True)
#self.toggle_depth_ctrl.toggle(True)
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
#success = False
self.nav_goal = None
# Stop thrusters
rpm1 = ThrusterRPM()
rpm2 = ThrusterRPM()
rpm1.rpm = 0
rpm2.rpm = 0
self.rpm1_pub.publish(rpm1)
self.rpm2_pub.publish(rpm2)
self.toggle_yaw_ctrl.toggle(False)
self.toggle_depth_ctrl.toggle(False)
self.toggle_vbs_ctrl.toggle(False)
self.toggle_speed_ctrl.toggle(False)
self.toggle_roll_ctrl.toggle(False)
print('wp depth action planner: stopped thrusters')
self._as.set_preempted(self._result, "Preempted WP action")
return
# Publish feedback
if counter % 5 == 0:
try:
(trans, rot) = self.listener.lookupTransform(
self.nav_goal_frame, self.base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo("Error with tf:" + str(self.nav_goal_frame) +
" to " + str(self.base_frame))
continue
pose_fb = PoseStamped()
pose_fb.header.frame_id = self.nav_goal_frame
pose_fb.pose.position.x = trans[0]
pose_fb.pose.position.y = trans[1]
pose_fb.pose.position.z = trans[2]
#self._feedback.feedback.pose = pose_fb
#self._feedback.feedback.pose.header.stamp = rospy.get_rostime()
#self._as.publish_feedback(self._feedback)
#rospy.loginfo("Sending feedback")
crosstrack_flag = 1 # set to 1 if we want to include crosstrack error, otherwise it computes a heading based on the next waypoint position
if crosstrack_flag:
#considering cross-track error according to Fossen, Page 261 eq 10.73,10.74
x_goal = self.nav_goal.position.x
y_goal = self.nav_goal.position.y
#checking if there is a previous WP, if there is no previous WP, it considers the current position
if self.y_prev == 0 and self.x_prev == 0:
self.y_prev = pose_fb.pose.position.y
self.x_prev = pose_fb.pose.position.x
y_prev = self.y_prev #read previous WP
x_prev = self.x_prev
#considering cross-track error according to Fossen, Page 261 eq 10.73,10.74
err_tang = math.atan2(y_goal - y_prev, x_goal -
x_prev) # path tangential vector
err_crosstrack = -(pose_fb.pose.position.x -
x_prev) * math.sin(err_tang) + (
pose_fb.pose.position.y -
y_prev) * math.cos(
err_tang) # crosstrack error
lookahead = 3 #lookahead distance(m)
err_velpath = math.atan2(-err_crosstrack, lookahead)
yaw_setpoint = (err_tang) + (err_velpath)
rospy.loginfo_throttle_identical(
5, "Using Crosstrack Error, err_tang =" +
str(err_tang) + "err_velpath" + str(err_velpath))
else:
#Compute yaw setpoint based on waypoint position and current position
xdiff = self.nav_goal.position.x - pose_fb.pose.position.x
ydiff = self.nav_goal.position.y - pose_fb.pose.position.y
#yaw_setpoint = 1.57-math.atan2(ydiff,xdiff)
#The original yaw setpoint!
yaw_setpoint = math.atan2(ydiff, xdiff)
#print('xdiff:',xdiff,'ydiff:',ydiff,'yaw_setpoint:',yaw_setpoint)
#compute yaw_error (e.g. for turbo_turn)
#yaw_error= -(self.yaw_feedback - yaw_setpoint)
#yaw_error= self.angle_wrap(yaw_error) #wrap angle error between -pi and pi
#TODO Add logic for depth control with services here!
depth_setpoint = self.nav_goal.position.z
#depth_setpoint = goal.travel_depth
#rospy.loginfo("Depth setpoint: %f", depth_setpoint)
#Diving logic to use VBS at low speeds below 0.5 m/s
self.toggle_depth_ctrl.toggle(True)
self.toggle_vbs_ctrl.toggle(False)
self.depth_pub.publish(depth_setpoint)
self.vel_ctrl_flag = 0 #use constant rpm
if self.vel_ctrl_flag:
# if speed control is activated from neptus
#if goal.speed_control_mode == 2:
rospy.loginfo_throttle_identical(
5, "Neptus vel ctrl, no turbo turn")
#with Velocity control
self.toggle_yaw_ctrl.toggle(True)
self.toggle_speed_ctrl.toggle(False)
self.toggle_roll_ctrl.toggle(False)
self.yaw_pub.publish(yaw_setpoint)
# Publish to velocity controller
travel_speed = goal.travel_speed
self.toggle_speed_ctrl.toggle(True)
#self.vel_pub.publish(self.vel_setpoint)
self.vel_pub.publish(travel_speed)
self.toggle_roll_ctrl.toggle(True)
self.roll_pub.publish(self.roll_setpoint)
#rospy.loginfo("Velocity published")
else:
# use rpm control instead of velocity control
self.toggle_yaw_ctrl.toggle(True)
self.yaw_pub.publish(yaw_setpoint)
# Thruster forward
rpm1 = ThrusterRPM()
rpm2 = ThrusterRPM()
rpm1.rpm = self.forward_rpm
rpm2.rpm = self.forward_rpm
self.rpm1_pub.publish(rpm1)
self.rpm2_pub.publish(rpm2)
#rospy.loginfo("Thrusters forward")
counter += 1
r.sleep()
# Stop thruster
self.toggle_speed_ctrl.toggle(False)
self.toggle_roll_ctrl.toggle(False)
#self.vel_pub.publish(0.0)
#self.roll_pub.publish(0.0)
rpm1 = ThrusterRPM()
rpm2 = ThrusterRPM()
rpm1.rpm = 0
rpm2.rpm = 0
self.rpm1_pub.publish(rpm1)
self.rpm2_pub.publish(rpm2)
if self._result.reached_waypoint:
#turbo turn at POI
self.toggle_yaw_ctrl.toggle(False)
self.toggle_speed_ctrl.toggle(False)
self.toggle_roll_ctrl.toggle(False)
self.toggle_depth_ctrl.toggle(False)
self.toggle_vbs_ctrl.toggle(True)
self.vbs_pub.publish(depth_setpoint)
pitch_setpoint = -0.4
self.toggle_pitch_ctrl.toggle(True)
self.lcg_pub.publish(pitch_setpoint)
angle_err = 0
count = 0
initial_yaw = self.yaw_feedback
while angle_err < 6.0 and count < 10:
rospy.loginfo_throttle_identical(
5, 'Turbo-turning at POI!' + str(count))
self.turbo_turn(angle_err)
angle_err = np.abs(initial_yaw - self.yaw_feedback)
count = count + 1
# Stop thruster
self.toggle_speed_ctrl.toggle(False)
self.vel_pub.publish(0.0)
rpm1 = ThrusterRPM()
rpm2 = ThrusterRPM()
rpm1.rpm = 0
rpm2.rpm = 0
self.rpm1_pub.publish(rpm1)
self.rpm2_pub.publish(rpm2)
#Stop controllers
self.toggle_yaw_ctrl.toggle(False)
self.toggle_depth_ctrl.toggle(False)
self.toggle_vbs_ctrl.toggle(False)
self.toggle_speed_ctrl.toggle(False)
self.toggle_roll_ctrl.toggle(False)
rospy.loginfo('%s: Succeeded' % self._action_name)
#self.x_prev = self.nav_goal.position.x
#self.y_prev = self.nav_goal.position.y
#self._result.reached_waypoint= True
self._as.set_succeeded(self._result, "WP Reached")
def execute_cb(self, goal):
rospy.loginfo("Goal received")
success = True
self.nav_goal = goal.target_pose.pose
self.nav_goal_frame = goal.target_pose.header.frame_id
if self.nav_goal_frame is None or self.nav_goal_frame == '':
rospy.logwarn("Goal has no frame id! Using utm by default")
self.nav_goal_frame = 'utm'
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_local = self.listener.transformPoint(self.nav_goal_frame, goal_point)
self.nav_goal.position.x = goal_point_local.point.x
self.nav_goal.position.y = goal_point_local.point.y
self.nav_goal.position.z = goal_point_local.point.z
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print ("Not transforming point to world local")
pass
rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x)
r = rospy.Rate(11.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
self.yaw_pid_enable.publish(True)
self.depth_pid_enable.publish(True)
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
success = False
self.nav_goal = None
# Stop thrusters
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = 0.
rpm.thruster_back.rpm = 0.
self.rpm_pub.publish(rpm)
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
print('wp depth action planner: stopped thrusters')
self._as.set_preempted(self._result, "Preempted WP action")
return
# Publish feedback
if counter % 5 == 0:
try:
(trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("Error with tf:"+str(self.nav_goal_frame) + " to "+str(self.base_frame))
continue
pose_fb = PoseStamped()
pose_fb.header.frame_id = self.nav_goal_frame
pose_fb.pose.position.x = trans[0]
pose_fb.pose.position.y = trans[1]
pose_fb.pose.position.z = trans[2]
self._feedback.base_position = pose_fb
self._feedback.base_position.header.stamp = rospy.get_rostime()
self._as.publish_feedback(self._feedback)
#rospy.loginfo("Sending feedback")
#Compute yaw setpoint.
xdiff = self.nav_goal.position.x - pose_fb.pose.position.x
ydiff = self.nav_goal.position.y - pose_fb.pose.position.y
yaw_setpoint = math.atan2(ydiff,xdiff)
print('xdiff:',xdiff,'ydiff:',ydiff,'yaw_setpoint:',yaw_setpoint)
#compute yaw_error (e.g. for turbo_turn)
yaw_error= -(self.yaw_feedback - yaw_setpoint)
yaw_error= self.angle_wrap(yaw_error) #wrap angle error between -pi and pi
depth_setpoint = self.nav_goal.position.z
self.depth_pub.publish(depth_setpoint)
#self.vbs_pid_enable.publish(False)
#self.vbs_pub.publish(depth_setpoint)
if self.vel_ctrl_flag:
rospy.loginfo_throttle_identical(5, "vel ctrl, no turbo turn")
#with Velocity control
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
# Publish to velocity controller
self.vel_pid_enable.publish(True)
self.vel_pub.publish(self.vel_setpoint)
self.roll_pub.publish(self.roll_setpoint)
#rospy.loginfo("Velocity published")
else:
if self.turbo_turn_flag:
#if turbo turn is included
rospy.loginfo("Yaw error: %f", yaw_error)
if abs(yaw_error) > self.turbo_angle_min and abs(yaw_error) < self.turbo_angle_max:
#turbo turn with large deviations, maximum deviation is 3.0 radians to prevent problems with discontinuities at +/-pi
self.yaw_pid_enable.publish(False)
self.turbo_turn(yaw_error)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(True)
self.vbs_pub.publish(depth_setpoint)
else:
rospy.loginfo_throttle_identical(5,"Normal WP following")
#normal turning if the deviation is small
self.vbs_pid_enable.publish(False)
self.depth_pid_enable.publish(True)
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
self.create_marker(yaw_setpoint,depth_setpoint)
# Thruster forward
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = self.forward_rpm
rpm.thruster_back.rpm = self.forward_rpm
self.rpm_pub.publish(rpm)
#rospy.loginfo("Thrusters forward")
else:
#turbo turn not included, no velocity control
rospy.loginfo_throttle_identical(5, "Normal WP following, no turbo turn")
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
self.create_marker(yaw_setpoint,depth_setpoint)
# Thruster forward
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = self.forward_rpm
rpm.thruster_back.rpm = self.forward_rpm
self.rpm_pub.publish(rpm)
#rospy.loginfo("Thrusters forward")
counter += 1
r.sleep()
# Stop thruster
self.vel_pid_enable.publish(False)
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = 0.0
rpm.thruster_back.rpm = 0.0
self.rpm_pub.publish(rpm)
#Stop controllers
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
