def test_with_arm(self):
exception_raised = False
exception_string = ''
try:
state_saver = SrStateSaverUnsafe("test_name", "arm")
except Exception as e:
exception_raised = True
exception_string = str(e)
self.assertTrue(exception_raised)
self.assertEqual(exception_string, "Group right_arm not found.")
def test_no_name(self):
exception_raised = False
exception_string = ''
try:
state_saver = SrStateSaverUnsafe('')
except Exception as e:
exception_raised = True
exception_string = str(e)
self.assertTrue(exception_raised)
self.assertEqual(exception_string, "Cannot save with empty name.")
def test_with_both(self):
exception_raised = False
exception_string = ''
try:
state_saver = SrStateSaverUnsafe("test_name", "both")
except Exception as e:
exception_raised = True
exception_string = str(e)
self.assertTrue(exception_raised)
self.assertEqual(exception_string, "'No hand found.' 'No arm found.'")
def test_with_both(self):
exception_raised = False
exception_string = ''
try:
state_saver = SrStateSaverUnsafe("test_name", "both")
except Exception as e:
exception_raised = True
exception_string = str(e)
self.assertTrue(exception_raised)
self.assertEqual(
exception_string, "Unable to construct robot model. Please make"
" sure all needed information is on the parameter server.")
If WHAT_TO_SAVE is omitted, it defaults to "both".
"""
if "__main__" == __name__:
rospy.init_node("state_saver")
if len(argv) <= 1 or "" == argv[1]:
rospy.logerr("You didn't enter a name.")
exit(-1)
which = 'all'
hand_h = False
if len(argv) > 3:
if argv[2] == "hand_h":
hand_h = True
which = argv[3]
elif len(argv) > 2:
which = argv[2]
if which == "hand_h":
hand_h = True
which = "all"
if which == "all":
gs = SrStateSaverUnsafe(argv[1] + "_hand", "hand", hand_h=hand_h)
gs = SrStateSaverUnsafe(argv[1] + "_arm", "arm", hand_h=hand_h)
gs = SrStateSaverUnsafe(argv[1] + "_both", "both", hand_h=hand_h)
else:
gs = SrStateSaverUnsafe(argv[1] + "_" + which, which, hand_h=hand_h)
You have been warned :)
To use this script:
rosrun sr_robot_commander grasp_saver_unsafe.py NAME_TO_SAVE WHAT_TO_SAVE
NAME_TO_SAVE is the name that will be used for the state in the database.
N.B. IF A STATE ALREADY EXISTS WITH THAT NAME IT WILL BE OVERWRITTEN WITH NO WARNING!
WHAT_TO_SAVE specifies which joints will be included. It can be "arm","hand" or "both".
If WHAT_TO_SAVE is omitted, it defaults to "both".
"""
if "__main__" == __name__:
rospy.init_node("state_saver")
name = rospy.get_param("~name")
which = rospy.get_param("~which", "all")
hand_h = rospy.get_param("~hand_h", False)
save_target = rospy.get_param("~save_target", False)
if which == "all":
gs = SrStateSaverUnsafe(name + "_hand", "hand", hand_h=hand_h, save_target=save_target)
gs = SrStateSaverUnsafe(name + "_arm", "arm", hand_h=hand_h, save_target=save_target)
gs = SrStateSaverUnsafe(name + "_both", "both", hand_h=hand_h, save_target=save_target)
else:
gs = SrStateSaverUnsafe(name + "_" + which, which, hand_h=hand_h, save_target=save_target)
from moveit_msgs.srv import SaveRobotStateToWarehouse as SaveState
from sensor_msgs.msg import JointState
from moveit_msgs.msg import RobotState
from sr_utilities.hand_finder import HandFinder
from sr_robot_commander.sr_robot_state_saver import SrStateSaverUnsafe
if "__main__" == __name__:
if select_to_save == 'arm_and_hand_save':
which = 'both'
elif select_to_save == 'arm_save':
which = 'arm'
else:
which = 'hand'
gs = SrStateSaverUnsafe(pose_name, which)