ft_move_as.py
Go to the documentation of this file.
00001 #!/usr/bin/python
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() #+ pub_period
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: #Total number of poses in list
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()


assistive_teleop
Author(s): Phillip M. Grice, Advisor: Prof. Charlie Kemp, Lab: The Healthcare Robotoics Lab at Georgia Tech.
autogenerated on Wed Nov 27 2013 11:35:34