def __init__(self, name):
self._action_name = name
self._server = actionlib.SimpleActionServer(
self._action_name,
ow_lander.msg.GuardedMoveAction,
execute_cb=self.on_guarded_move_action,
auto_start=False)
self._server.start()
# Action Feedback/Result
self._fdbk = ow_lander.msg.GuardedMoveFeedback()
self._result = ow_lander.msg.GuardedMoveResult()
self._current_link_state = LinkStateSubscriber()
self._interface = MoveItInterface()
self._timeout = 0.0
self._estimated_plan_fraction_completed = 0.0
# ratio between guarded pre-guarded move trajectory and the whole trajectory
self._guarded_move_plan_ratio = 0.0
self.trajectory_async_executer = TrajectoryAsyncExecuter()
self.trajectory_async_executer.connect("arm_controller")
self.guarded_move_traj = RobotTrajectory()
self.ground_detector = GroundDetector()
self.pos = Point()
self.guarded_move_pub = rospy.Publisher('/guarded_move_result',
GuardedMoveFinalResult,
queue_size=10)
def __init__(self):
super(PathPlanningCommander, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
self.arm_move_group = moveit_commander.MoveGroupCommander(
"arm", wait_for_servers=20.0)
self.limbs_move_group = moveit_commander.MoveGroupCommander(
"limbs", wait_for_servers=20.0)
self.grinder_move_group = moveit_commander.MoveGroupCommander(
"grinder", wait_for_servers=20.0)
self.trajectory_async_executer = TrajectoryAsyncExecuter()
def __init__(self, name):
self._action_name = name
self._server = actionlib.SimpleActionServer(
self._action_name,
ow_lander.msg.UnstowAction,
execute_cb=self.on_unstow_action,
auto_start=False)
self._server.start()
# Action Feedback/Result
self._fdbk = ow_lander.msg.UnstowFeedback()
self._result = ow_lander.msg.UnstowResult()
self._current_link_state = LinkStateSubscriber()
self._interface = MoveItInterface()
self._timeout = 0.0
self.trajectory_async_executer = TrajectoryAsyncExecuter()
self.trajectory_async_executer.connect("arm_controller")
class DeliverActionServer(object):
def __init__(self, name):
self._action_name = name
self._server = actionlib.SimpleActionServer(
self._action_name,
ow_lander.msg.DeliverAction,
execute_cb=self.on_deliver_action,
auto_start=False)
self._server.start()
# Action Feedback/Result
self._fdbk = ow_lander.msg.UnstowFeedback()
self._result = ow_lander.msg.UnstowResult()
self._current_link_state = LinkStateSubscriber()
self._interface = MoveItInterface()
self._timeout = 0.0
self.trajectory_async_executer = TrajectoryAsyncExecuter()
self.trajectory_async_executer.connect("arm_controller")
self.deliver_sample_traj = RobotTrajectory()
def _update_feedback(self):
self._ls = self._current_link_state._link_value
self._fdbk.current.x = self._ls.x
self._fdbk.current.y = self._ls.y
self._fdbk.current.z = self._ls.z
self._server.publish_feedback(self._fdbk)
def _update_motion(self, goal):
rospy.loginfo("Deliver sample activity started")
self.deliver_sample_traj = all_action_trajectories.deliver_sample(
self._interface.move_arm, self._interface.robot,
self._interface.moveit_fk, goal)
if self.deliver_sample_traj == False:
return
else:
n_points = len(self.deliver_sample_traj.joint_trajectory.points)
start_time = self.deliver_sample_traj.joint_trajectory.points[
0].time_from_start
end_time = self.deliver_sample_traj.joint_trajectory.points[
n_points - 1].time_from_start
self._timeout = end_time - start_time
def on_deliver_action(self, goal):
self._update_motion(goal)
if self.deliver_sample_traj == False:
self._server.set_aborted(self._result)
return
success = False
self.trajectory_async_executer.execute(
self.deliver_sample_traj.joint_trajectory,
done_cb=None,
active_cb=None,
feedback_cb=self.trajectory_async_executer.stop_arm_if_fault)
# Record start time
start_time = rospy.get_time()
def now_from_start(start):
# return rospy.get_time() - start
return rospy.Duration(secs=rospy.get_time() - start)
while ((now_from_start(start_time) < self._timeout)):
self._update_feedback()
success = self.trajectory_async_executer.success(
) and self.trajectory_async_executer.wait()
if success:
self._result.final.x = self._fdbk.current.x
self._result.final.y = self._fdbk.current.y
self._result.final.z = self._fdbk.current.z
rospy.loginfo('%s: Succeeded' % self._action_name)
self._server.set_succeeded(self._result)
else:
rospy.loginfo('%s: Failed' % self._action_name)
self._server.set_aborted(self._result)
class DigCircularActionServer(object):
def __init__(self, name):
self._action_name = name
self._server = actionlib.SimpleActionServer(
self._action_name,
ow_lander.msg.DigCircularAction,
execute_cb=self.on_DigCircular_action,
auto_start=False)
self._server.start()
# Action Feedback/Result
self._fdbk = ow_lander.msg.DigCircularFeedback()
self._result = ow_lander.msg.DigCircularResult()
self._current_link_state = LinkStateSubscriber()
self._interface = MoveItInterface()
self._timeout = 0.0
self.trajectory_async_executer = TrajectoryAsyncExecuter()
self.trajectory_async_executer.connect("arm_controller")
self.current_traj = RobotTrajectory()
def switch_controllers(self, start_controller, stop_controller):
rospy.wait_for_service('/controller_manager/switch_controller')
success = False
try:
switch_controller = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
success = switch_controller([start_controller], [stop_controller],
2, False, 1.0)
except rospy.ServiceException as e:
rospy.loginfo("switch_controllers error: %s" % e)
finally:
# This sleep is a workaround for "start point deviates from current robot
# state" error on dig_circular trajectory execution.
rospy.sleep(0.2)
return success
def _update_feedback(self):
self._ls = self._current_link_state._link_value
self._fdbk.current.x = self._ls.x
self._fdbk.current.y = self._ls.y
self._fdbk.current.z = self._ls.z
self._server.publish_feedback(self._fdbk)
def _update_motion(self, goal):
rospy.loginfo("DigCircular activity started")
self.current_traj = None
self.current_traj = all_action_trajectories.dig_circular(
self._interface.move_arm, self._interface.move_limbs,
self._interface.robot, self._interface.moveit_fk, goal)
if self.current_traj == False:
return
else:
n_points = len(self.current_traj.joint_trajectory.points)
start_time = self.current_traj.joint_trajectory.points[
0].time_from_start
end_time = self.current_traj.joint_trajectory.points[
n_points - 1].time_from_start
self._timeout = (end_time - start_time)
def on_DigCircular_action(self, goal):
self._update_motion(goal)
if self.current_traj == False:
self._server.set_aborted(self._result)
return
success = False
self.trajectory_async_executer.execute(
self.current_traj.joint_trajectory,
done_cb=None,
active_cb=None,
feedback_cb=self.trajectory_async_executer.stop_arm_if_fault)
# Record start time
start_time = rospy.get_time()
def now_from_start(start):
return rospy.Duration(secs=rospy.get_time() - start)
while ((now_from_start(start_time) < self._timeout)):
self._update_feedback()
success = self.trajectory_async_executer.success(
) and self.trajectory_async_executer.wait()
if success:
self._result.final.x = self._fdbk.current.x
self._result.final.y = self._fdbk.current.y
self._result.final.z = self._fdbk.current.z
rospy.loginfo('%s: Succeeded' % self._action_name)
self._server.set_succeeded(self._result)
else:
rospy.loginfo('%s: Failed' % self._action_name)
self._server.set_aborted(self._result)
class GuardedMoveActionServer(object):
def __init__(self, name):
self._action_name = name
self._server = actionlib.SimpleActionServer(
self._action_name,
ow_lander.msg.GuardedMoveAction,
execute_cb=self.on_guarded_move_action,
auto_start=False)
self._server.start()
# Action Feedback/Result
self._fdbk = ow_lander.msg.GuardedMoveFeedback()
self._result = ow_lander.msg.GuardedMoveResult()
self._current_link_state = LinkStateSubscriber()
self._interface = MoveItInterface()
self._timeout = 0.0
self._estimated_plan_fraction_completed = 0.0
# ratio between guarded pre-guarded move trajectory and the whole trajectory
self._guarded_move_plan_ratio = 0.0
self.trajectory_async_executer = TrajectoryAsyncExecuter()
self.trajectory_async_executer.connect("arm_controller")
self.guarded_move_traj = RobotTrajectory()
self.ground_detector = GroundDetector()
self.pos = Point()
self.guarded_move_pub = rospy.Publisher('/guarded_move_result',
GuardedMoveFinalResult,
queue_size=10)
def handle_guarded_move_done(self, state, result):
"""
:type state: int
:type result: FollowJointTrajectoryResult
"""
ground_detected = state == GoalStatus.PREEMPTED
ground_position = self.ground_detector.ground_position if ground_detected else Point(
)
rospy.loginfo("Ground Detected ? {}".format(ground_detected))
self.guarded_move_pub.publish(ground_detected, 'base_link',
ground_position)
def handle_guarded_move_feedback(self, feedback):
"""
:type feedback: FollowJointTrajectoryFeedback
"""
self.trajectory_async_executer.stop_arm_if_fault(feedback)
# added to compensate for slower than arm movement tan planned
execution_time_tollerance = 0.1
if self.ground_detector.detect():
if (self._estimated_plan_fraction_completed <
self._guarded_move_plan_ratio + execution_time_tollerance):
self.ground_detector.reset()
else:
self.trajectory_async_executer.stop()
def _update_feedback(self):
self._ls = self._current_link_state._link_value
self._fdbk.current.x = self._ls.x
self._fdbk.current.y = self._ls.y
self._fdbk.current.z = self._ls.z
self._server.publish_feedback(self._fdbk)
def _update_motion(self, goal):
rospy.loginfo("Guarded move activity started")
self.guarded_move_traj, self._guarded_move_plan_ratio = all_action_trajectories.guarded_move_plan(
self._interface.move_arm, self._interface.robot,
self._interface.moveit_fk, goal)
if self.guarded_move_traj == False:
return
else:
n_points = len(self.guarded_move_traj.joint_trajectory.points)
start_time = self.guarded_move_traj.joint_trajectory.points[
0].time_from_start
end_time = self.guarded_move_traj.joint_trajectory.points[
n_points - 1].time_from_start
self._timeout = end_time - start_time
def on_guarded_move_action(self, goal):
self._update_motion(goal)
if self.guarded_move_traj == False:
self._server.set_aborted(self._result)
return
success = False
# detection
self.ground_detector.reset()
self.trajectory_async_executer.execute(
self.guarded_move_traj.joint_trajectory,
done_cb=self.handle_guarded_move_done,
active_cb=None,
feedback_cb=self.handle_guarded_move_feedback)
# Record start time
start_time = rospy.get_time()
def now_from_start(start):
return rospy.Duration(secs=rospy.get_time() - start)
while ((now_from_start(start_time) < self._timeout)):
self._update_feedback()
self._estimated_plan_fraction_completed = now_from_start(
start_time) / self._timeout
success = self.trajectory_async_executer.success(
) and self.trajectory_async_executer.wait()
if success:
self._result.final.x = self.ground_detector.ground_position.x
self._result.final.y = self.ground_detector.ground_position.y
self._result.final.z = self.ground_detector.ground_position.z
self._result.success = True
rospy.loginfo('%s: Succeeded' % self._action_name)
self._server.set_succeeded(self._result)
else:
rospy.loginfo('%s: Failed' % self._action_name)
self._server.set_aborted(self._result)
class UnstowActionServer(object):
def __init__(self, name):
self._action_name = name
self._server = actionlib.SimpleActionServer(
self._action_name,
ow_lander.msg.UnstowAction,
execute_cb=self.on_unstow_action,
auto_start=False)
self._server.start()
# Action Feedback/Result
self._fdbk = ow_lander.msg.UnstowFeedback()
self._result = ow_lander.msg.UnstowResult()
self._current_link_state = LinkStateSubscriber()
self._interface = MoveItInterface()
self._timeout = 0.0
self.trajectory_async_executer = TrajectoryAsyncExecuter()
self.trajectory_async_executer.connect("arm_controller")
def _update_feedback(self):
self._ls = self._current_link_state._link_value
self._fdbk.current.x = self._ls.x
self._fdbk.current.y = self._ls.y
self._fdbk.current.z = self._ls.z
self._server.publish_feedback(self._fdbk)
def _update_motion(self):
rospy.loginfo("Unstow arm activity started")
goal = self._interface.move_arm.get_current_pose().pose
goal = self._interface.move_arm.get_named_target_values("arm_unstowed")
self._interface.move_arm.set_joint_value_target(goal)
_, plan, _, _ = self._interface.move_arm.plan()
if len(plan.joint_trajectory.points) < 1:
return
else:
n_points = len(plan.joint_trajectory.points)
start_time = plan.joint_trajectory.points[0].time_from_start
end_time = plan.joint_trajectory.points[n_points -
1].time_from_start
self._timeout = end_time - start_time
return plan
def on_unstow_action(self, goal):
plan = self._update_motion()
if plan is None:
self._server.set_aborted(self._result)
return
success = False
self.trajectory_async_executer.execute(
plan.joint_trajectory,
done_cb=None,
active_cb=None,
feedback_cb=self.trajectory_async_executer.stop_arm_if_fault)
# Record start time
start_time = rospy.get_time()
def now_from_start(start):
# return rospy.get_time() - start
return rospy.Duration(secs=rospy.get_time() - start)
while ((now_from_start(start_time) < self._timeout)):
self._update_feedback()
success = self.trajectory_async_executer.success(
) and self.trajectory_async_executer.wait()
if success:
self._result.final.x = self._fdbk.current.x
self._result.final.y = self._fdbk.current.y
self._result.final.z = self._fdbk.current.z
rospy.loginfo('%s: Succeeded' % self._action_name)
self._server.set_succeeded(self._result)
else:
rospy.loginfo('%s: Failed' % self._action_name)
self._server.set_aborted(self._result)
class PathPlanningCommander(object):
def __init__(self):
super(PathPlanningCommander, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
self.arm_move_group = moveit_commander.MoveGroupCommander(
"arm", wait_for_servers=20.0)
self.limbs_move_group = moveit_commander.MoveGroupCommander(
"limbs", wait_for_servers=20.0)
self.grinder_move_group = moveit_commander.MoveGroupCommander(
"grinder", wait_for_servers=20.0)
self.trajectory_async_executer = TrajectoryAsyncExecuter()
# === SERVICE ACTIVITIES - Stow =============================
def handle_stop(self, req):
"""
:type req: class 'ow_lander.srv._Stow.StopRequest'
"""
self.log_started("Stop")
self.trajectory_async_executer.stop()
return self.log_finish_and_return("Stop")
# === SERVICE ACTIVITIES - Stow =============================
def handle_stow(self, req):
"""
:type req: class 'ow_lander.srv._Stow.StowRequest'
"""
self.log_started("Stow")
goal = self.arm_move_group.get_named_target_values("arm_stowed")
self.arm_move_group.set_joint_value_target(goal)
_, plan, _, _ = self.arm_move_group.plan()
if len(plan.joint_trajectory.points) == 0:
return False
self.trajectory_async_executer.execute(plan.joint_trajectory,
feedback_cb=None)
self.trajectory_async_executer.wait()
return self.log_finish_and_return("Stow")
# === SERVICE ACTIVITIES - Unstow =============================
def handle_unstow(self, req):
"""
:type req: class 'ow_lander.srv._Unstow.UnstowRequest'
"""
self.log_started("Unstow")
goal = self.arm_move_group.get_named_target_values("arm_unstowed")
self.arm_move_group.set_joint_value_target(goal)
_, plan, _, _ = self.arm_move_group.plan()
if len(plan.joint_trajectory.points) == 0:
return False
self.trajectory_async_executer.execute(plan.joint_trajectory,
feedback_cb=None)
self.trajectory_async_executer.wait()
return self.log_finish_and_return("Unstow")
# === SERVICE ACTIVITIES - deliver sample =============================
def handle_deliver_sample(self, req):
"""
:type req: class 'ow_lander.srv._DeliverSample.DeliverSampleRequest'
"""
self.log_started("Deliver Sample")
success = True
targets = [
"arm_deliver_staging_1", "arm_deliver_staging_2",
"arm_deliver_final"
]
for t in targets:
goal = self.arm_move_group.get_named_target_values(t)
self.arm_move_group.set_joint_value_target(goal)
_, plan, _, _ = self.arm_move_group.plan()
if len(plan.joint_trajectory.points) == 0:
success = False
break
self.trajectory_async_executer.execute(plan.joint_trajectory)
self.trajectory_async_executer.wait()
return self.log_finish_and_return("Deliver Sample", success)
# === SERVICE ACTIVITIES - Dig Linear Trench =============================
def handle_dig_circular(self, req):
"""
:type req: class 'ow_lander.srv._DigCircular.DigCircularRequest'
"""
self.log_started("Dig Cicular")
dig_circular_args = activity_full_digging_traj.arg_parsing_circ(req)
success = activity_full_digging_traj.dig_circular(
self.arm_move_group, self.limbs_move_group, dig_circular_args,
self.switch_controllers)
return self.log_finish_and_return("Dig Circular", success)
# === SERVICE ACTIVITIES - Dig Linear Trench =============================
def handle_dig_linear(self, req):
"""
:type req: class 'ow_lander.srv._DigLinear.DigLinearRequest'
"""
self.log_started("Dig Linear")
dig_linear_args = activity_full_digging_traj.arg_parsing_lin(req)
success = activity_full_digging_traj.dig_linear(
self.arm_move_group, dig_linear_args)
return self.log_finish_and_return("Dig Linear", success)
def switch_controllers(self, start_controller, stop_controller):
rospy.wait_for_service('/controller_manager/switch_controller')
success = False
try:
switch_controller = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
success = switch_controller([start_controller], [stop_controller],
2, False, 1.0)
except rospy.ServiceException as e:
rospy.logerror("switch_controllers error: %s" % e)
finally:
# This sleep is a workaround for "start point deviates from current robot
# state" error on dig_circular trajectory execution.
rospy.sleep(0.2)
return success
# === SERVICE ACTIVITIES - Grind =============================
def handle_grind(self, req):
"""
:type req: class 'ow_lander.srv._Grind.GrindRequest'
"""
self.log_started("Grind")
grind_args = activity_grind.arg_parsing(req)
success = self.switch_controllers('grinder_controller',
'arm_controller')
if not success:
return False, "Failed"
success = activity_grind.grind(self.grinder_move_group, grind_args)
self.switch_controllers('arm_controller', 'grinder_controller')
return self.log_finish_and_return("Grind", success)
def handle_guarded_move_done(self, state, result):
"""
:type state: int
:type result: FollowJointTrajectoryResult
"""
ground_detected = state == GoalStatus.PREEMPTED
ground_position = self.ground_detector.ground_position if ground_detected else Point(
)
rospy.loginfo("Ground Detected ? {}".format(ground_detected))
self.guarded_move_pub.publish(ground_detected, 'base_link',
ground_position)
def handle_guarded_move_feedback(self, feedback):
"""
:type feedback: FollowJointTrajectoryFeedback
"""
if self.ground_detector.detect():
self.trajectory_async_executer.stop()
# === SERVICE ACTIVITIES - guarded move =============================
def handle_guarded_move(self, req):
"""
:type req: class 'ow_lander.srv._GuardedMove.GuardedMoveRequest'
"""
self.log_started("Guarded Move")
guarded_move_args = activity_guarded_move.arg_parsing(req)
success = activity_guarded_move.pre_guarded_move(
self.arm_move_group, guarded_move_args)
if not success:
return False, "pre_guarded_move failed"
plan = activity_guarded_move.guarded_move_plan(self.arm_move_group,
guarded_move_args)
if len(plan.joint_trajectory.points) == 0:
return False, "guarded_move_plan failed"
self.ground_detector.reset()
self.trajectory_async_executer.execute(
plan.joint_trajectory,
done_cb=self.handle_guarded_move_done,
feedback_cb=self.handle_guarded_move_feedback)
# To preserve the previous behaviour we are adding a blocking call till the
# execution of the trajectory is completed
self.trajectory_async_executer.wait()
return self.log_finish_and_return("Guarded Move", success)
def run(self):
rospy.init_node('path_planning_commander', anonymous=True)
self.arm_faults_sub = rospy.Subscriber('/faults/arm_faults_status',
ArmFaults,
self.handle_arm_faults)
self.stop_srv = rospy.Service('arm/stop', Stop, self.handle_stop)
self.stow_srv = rospy.Service('arm/stow', Stow, self.handle_stow)
self.unstow_srv = rospy.Service('arm/unstow', Unstow,
self.handle_unstow)
self.dig_circular_srv = rospy.Service('arm/dig_circular', DigCircular,
self.handle_dig_circular)
self.dig_linear_srv = rospy.Service('arm/dig_linear', DigLinear,
self.handle_dig_linear)
self.deliver_sample_srv = rospy.Service('arm/deliver_sample',
DeliverSample,
self.handle_deliver_sample)
self.grind_srv = rospy.Service('arm/grind', Grind, self.handle_grind)
self.ground_detector = GroundDetector()
self.trajectory_async_executer.connect("arm_controller")
self.guarded_move_pub = rospy.Publisher('/guarded_move_result',
GuardedMoveFinalResult,
queue_size=10)
self.guarded_move_srv = rospy.Service('arm/guarded_move', GuardedMove,
self.handle_guarded_move)
rospy.loginfo("path_planning_commander has started!")
rospy.spin()
def log_started(self, activity_name):
rospy.loginfo("%s arm activity started", activity_name)
def log_finish_and_return(self, activity_name, success=True):
rospy.loginfo("%s arm activity completed", activity_name)
return success, "Done"
def handle_arm_faults(self, data):
"""
If system fault occurs, and it is an arm failure, an arm failure flag is set for the whole class
"""
if data.value:
self.trajectory_async_executer.stop()