def __init__(self):
self.arm = rospy.get_param("~arm", default="r")
self.tf_listener = tf.TransformListener()
self.epc_move = EPCMove(self.arm, self.tf_listener)
self.direct_move_act = actionlib.SimpleActionServer(
"~direct_move", EPCDirectMoveAction, self.execute_direct_move,
False)
self.direct_move_act.start()
self.linear_move_act = actionlib.SimpleActionServer(
"~linear_move", EPCLinearMoveAction, self.execute_linear_move,
False)
self.linear_move_act.start()
#self.pix3_pub = rospy.Publisher("/lin_pose3", PoseStamped)
rospy.loginfo("[epc_move_actionlib] EPCMoveActionlib loaded.")
def __init__(self):
self.arm = rospy.get_param("~arm", default="r")
self.tf_listener = tf.TransformListener()
self.epc_move = EPCMove(self.arm, self.tf_listener)
self.direct_move_act = actionlib.SimpleActionServer(
"~direct_move", EPCDirectMoveAction, self.execute_direct_move, False
)
self.direct_move_act.start()
self.linear_move_act = actionlib.SimpleActionServer(
"~linear_move", EPCLinearMoveAction, self.execute_linear_move, False
)
self.linear_move_act.start()
# self.pix3_pub = rospy.Publisher("/lin_pose3", PoseStamped)
rospy.loginfo("[epc_move_actionlib] EPCMoveActionlib loaded.")
class EPCMoveActionlib(object):
def __init__(self):
self.arm = rospy.get_param("~arm", default="r")
self.tf_listener = tf.TransformListener()
self.epc_move = EPCMove(self.arm, self.tf_listener)
self.direct_move_act = actionlib.SimpleActionServer(
"~direct_move", EPCDirectMoveAction, self.execute_direct_move, False
)
self.direct_move_act.start()
self.linear_move_act = actionlib.SimpleActionServer(
"~linear_move", EPCLinearMoveAction, self.execute_linear_move, False
)
self.linear_move_act.start()
# self.pix3_pub = rospy.Publisher("/lin_pose3", PoseStamped)
rospy.loginfo("[epc_move_actionlib] EPCMoveActionlib loaded.")
def execute_direct_move(self, goal):
rospy.loginfo("[epc_move_actionlib] Execute direct move.")
result = EPCDirectMoveResult()
transformed_pose_stamped = self.tf_listener.transformPose("torso_lift_link", goal.target_pose)
transformed_pose = util.pose_msg_to_mat(transformed_pose_stamped)
if goal.reset_controllers:
self.epc_move.reset_controllers()
def preempt_callback(err_pos, err_ang):
if self.direct_move_act.is_preempt_requested():
return "preempted"
return None
result.result = self.epc_move.direct_move(
transformed_pose,
goal.tool_frame,
goal.err_pos_goal,
goal.err_ang_goal,
goal.err_pos_max,
goal.err_ang_max,
goal.steady_steps,
preempt_callback,
)
rospy.loginfo("[epc_move_actionlib] Direct move completed with result '%s'." % result.result)
if result.result == "succeeded":
self.direct_move_act.set_succeeded(result)
elif result.result == "preempted":
self.direct_move_act.set_preempted(result)
else:
self.direct_move_act.set_aborted(result)
def execute_linear_move(self, goal):
rospy.loginfo("[epc_move_actionlib] Execute linear move.")
result = EPCLinearMoveResult()
start_pose_stamped = self.tf_listener.transformPose("torso_lift_link", goal.start_pose)
start_pose = util.pose_msg_to_mat(start_pose_stamped)
end_pose_stamped = self.tf_listener.transformPose("torso_lift_link", goal.end_pose)
end_pose = util.pose_msg_to_mat(end_pose_stamped)
if goal.reset_controllers:
self.epc_move.reset_controllers()
def preempt_callback(err_pos, err_ang):
if self.linear_move_act.is_preempt_requested():
return "preempted"
return None
result.result = self.epc_move.linear_move(
start_pose,
end_pose,
goal.tool_frame,
goal.velocity,
goal.err_pos_max,
goal.err_ang_max,
goal.setup_steps,
preempt_callback,
)
rospy.loginfo("[epc_move_actionlib] Linear move completed with result '%s'." % result.result)
if result.result == "succeeded":
self.linear_move_act.set_succeeded(result)
elif result.result == "preempted":
self.linear_move_act.set_preempted(result)
else:
self.linear_move_act.set_aborted(result)
class EPCMoveActionlib(object):
def __init__(self):
self.arm = rospy.get_param("~arm", default="r")
self.tf_listener = tf.TransformListener()
self.epc_move = EPCMove(self.arm, self.tf_listener)
self.direct_move_act = actionlib.SimpleActionServer(
"~direct_move", EPCDirectMoveAction, self.execute_direct_move,
False)
self.direct_move_act.start()
self.linear_move_act = actionlib.SimpleActionServer(
"~linear_move", EPCLinearMoveAction, self.execute_linear_move,
False)
self.linear_move_act.start()
#self.pix3_pub = rospy.Publisher("/lin_pose3", PoseStamped)
rospy.loginfo("[epc_move_actionlib] EPCMoveActionlib loaded.")
def execute_direct_move(self, goal):
rospy.loginfo("[epc_move_actionlib] Execute direct move.")
result = EPCDirectMoveResult()
transformed_pose_stamped = self.tf_listener.transformPose(
"torso_lift_link", goal.target_pose)
transformed_pose = util.pose_msg_to_mat(transformed_pose_stamped)
if goal.reset_controllers:
self.epc_move.reset_controllers()
def preempt_callback(err_pos, err_ang):
if self.direct_move_act.is_preempt_requested():
return "preempted"
return None
result.result = self.epc_move.direct_move(
transformed_pose, goal.tool_frame, goal.err_pos_goal,
goal.err_ang_goal, goal.err_pos_max, goal.err_ang_max,
goal.steady_steps, preempt_callback)
rospy.loginfo(
"[epc_move_actionlib] Direct move completed with result '%s'." %
result.result)
if result.result == "succeeded":
self.direct_move_act.set_succeeded(result)
elif result.result == "preempted":
self.direct_move_act.set_preempted(result)
else:
self.direct_move_act.set_aborted(result)
def execute_linear_move(self, goal):
rospy.loginfo("[epc_move_actionlib] Execute linear move.")
result = EPCLinearMoveResult()
start_pose_stamped = self.tf_listener.transformPose(
"torso_lift_link", goal.start_pose)
start_pose = util.pose_msg_to_mat(start_pose_stamped)
end_pose_stamped = self.tf_listener.transformPose(
"torso_lift_link", goal.end_pose)
end_pose = util.pose_msg_to_mat(end_pose_stamped)
if goal.reset_controllers:
self.epc_move.reset_controllers()
def preempt_callback(err_pos, err_ang):
if self.linear_move_act.is_preempt_requested():
return "preempted"
return None
result.result = self.epc_move.linear_move(
start_pose, end_pose, goal.tool_frame, goal.velocity,
goal.err_pos_max, goal.err_ang_max, goal.setup_steps,
preempt_callback)
rospy.loginfo(
"[epc_move_actionlib] Linear move completed with result '%s'." %
result.result)
if result.result == "succeeded":
self.linear_move_act.set_succeeded(result)
elif result.result == "preempted":
self.linear_move_act.set_preempted(result)
else:
self.linear_move_act.set_aborted(result)
