Go to the documentation of this file.00001
00002
00003 import math
00004
00005 import roslib; roslib.load_manifest('assistive_teleop')
00006 import rospy
00007 import actionlib
00008 from geometry_msgs.msg import PoseStamped, WrenchStamped
00009 from assistive_teleop.msg import FtMoveAction, FtMoveFeedback, FtMoveResult
00010
00011
00012 class FtMoveServer(object):
00013 def __init__(self):
00014 self.ft_move_server = actionlib.SimpleActionServer('ft_move_action', FtMoveAction, self.ft_move, False)
00015 self.ft_move_server.start()
00016 rospy.Subscriber('/netft_gravity_zeroing/wrench_zeroed', WrenchStamped, self.get_netft_state)
00017 self.pose_out = rospy.Publisher('/l_cart/command_pose', PoseStamped)
00018
00019 def get_netft_state(self, ws):
00020 self.netft_wrench = ws
00021 self.ft_mag = math.sqrt(ws.wrench.force.x**2 + ws.wrench.force.y**2 + ws.wrench.force.z**2)
00022
00023 def ft_move(self, goal):
00024 if goal.ignore_ft:
00025 rospy.loginfo("Moving WITHOUT monitoring FT Sensor")
00026 else:
00027 rospy.loginfo("Moving while monitoring Force-Torque Sensor")
00028 update_rate=rospy.Rate(200)
00029 pub_period = rospy.Duration(0.05)
00030 pose_count = 0
00031 result = FtMoveResult()
00032 feedback = FtMoveFeedback()
00033 feedback.total = len(goal.poses)
00034 previous_pose_sent = rospy.Time.now()
00035 while not rospy.is_shutdown():
00036 if not self.ft_move_server.is_active():
00037 rospy.loginfo("FtMoveAction Goal No Longer Active")
00038 break
00039
00040 if self.ft_move_server.is_new_goal_available():
00041 self.ft_move_server.accept_new_goal()
00042
00043 if self.ft_move_server.is_preempt_requested():
00044 self.ft_move_server.set_preempted()
00045 rospy.loginfo("Force-Torque Move Action Server Goal Preempted")
00046 break
00047
00048 if not goal.ignore_ft:
00049 if self.ft_mag > goal.force_thresh:
00050 result.contact = True
00051 result.all_sent = False
00052 self.ft_move_server.set_aborted(result, "Contact Detected")
00053 rospy.loginfo("Force-Torque Move Action Aborted: Contact Detected")
00054 break
00055
00056 if rospy.Time.now()-previous_pose_sent >= pub_period:
00057 feedback.current = pose_count
00058 self.ft_move_server.publish_feedback(feedback)
00059 previous_pose_sent = rospy.Time.now()
00060 self.pose_out.publish(goal.poses[pose_count])
00061 pose_count += 1
00062
00063 if pose_count >= feedback.total:
00064 result.contact = False
00065 result.all_sent = True
00066 self.ft_move_server.set_succeeded(result)
00067 break
00068 update_rate.sleep()
00069
00070 if __name__ == '__main__':
00071 rospy.init_node('ft_move_action_server')
00072 ftms = FtMoveServer()
00073 while not rospy.is_shutdown():
00074 rospy.spin()