def __init__(self, namespace=None):
if namespace is None:
namespace = rospy.get_namespace()
if not namespace.endswith('/'):
namespace = namespace+'/'
self.parameters = HandeyeCalibrationParameters.init_from_parameter_server(namespace)
angle_delta = math.radians(rospy.get_param('~rotation_delta_degrees', 25))
translation_delta = rospy.get_param('~translation_delta_meters', 0.1)
max_velocity_scaling = rospy.get_param('~max_velocity_scaling', 0.5)
max_acceleration_scaling = rospy.get_param('~max_acceleration_scaling', 0.5)
print "[Debug] HandeyeServerRobot params are :{},{},{}".format(self.parameters.move_group,
self.parameters.move_group_namespace,namespace)
self.parameters.move_group = rospy.get_param('move_group_name', "gluon")
self.local_mover = CalibrationMovements(move_group_name=self.parameters.move_group,
move_group_namespace=self.parameters.move_group_namespace,
max_velocity_scaling=max_velocity_scaling,
max_acceleration_scaling=max_acceleration_scaling,
angle_delta=angle_delta, translation_delta=translation_delta)
# setup services and topics
self.check_starting_position_service = rospy.Service(namespace+hec.CHECK_STARTING_POSE_TOPIC, ehm.srv.CheckStartingPose,
self.check_starting_position)
self.enumerate_target_poses_service = rospy.Service(namespace+hec.ENUMERATE_TARGET_POSES_TOPIC,
ehm.srv.EnumerateTargetPoses, self.enumerate_target_poses)
self.select_target_pose_service = rospy.Service(namespace+hec.SELECT_TARGET_POSE_TOPIC, ehm.srv.SelectTargetPose,
self.select_target_pose)
self.plan_to_selected_target_pose_service = rospy.Service(namespace+hec.PLAN_TO_SELECTED_TARGET_POSE_TOPIC,
ehm.srv.PlanToSelectedTargetPose,
self.plan_to_selected_target_pose)
self.execute_plan_service = rospy.Service(namespace+hec.EXECUTE_PLAN_TOPIC, ehm.srv.ExecutePlan, self.execute_plan)
def set_namespace(self, namespace):
self.parameters = HandeyeCalibrationParameters.init_from_parameter_server(namespace)
# init services: sampling
rospy.wait_for_service(hec.GET_SAMPLE_LIST_TOPIC)
self.get_sample_proxy = rospy.ServiceProxy(hec.GET_SAMPLE_LIST_TOPIC, ehm.srv.TakeSample)
rospy.wait_for_service(hec.TAKE_SAMPLE_TOPIC)
self.take_sample_proxy = rospy.ServiceProxy(hec.TAKE_SAMPLE_TOPIC, ehm.srv.TakeSample)
rospy.wait_for_service(hec.REMOVE_SAMPLE_TOPIC)
self.remove_sample_proxy = rospy.ServiceProxy(hec.REMOVE_SAMPLE_TOPIC, ehm.srv.RemoveSample)
# init services: calibration
rospy.wait_for_service(hec.LIST_ALGORITHMS_TOPIC)
self.list_algorithms_proxy = rospy.ServiceProxy(hec.LIST_ALGORITHMS_TOPIC, ehm.srv.ListAlgorithms)
rospy.wait_for_service(hec.SET_ALGORITHM_TOPIC)
self.set_algorithm_proxy = rospy.ServiceProxy(hec.SET_ALGORITHM_TOPIC, ehm.srv.SetAlgorithm)
rospy.wait_for_service(hec.COMPUTE_CALIBRATION_TOPIC)
self.compute_calibration_proxy = rospy.ServiceProxy(hec.COMPUTE_CALIBRATION_TOPIC, ehm.srv.ComputeCalibration)
rospy.wait_for_service(hec.SAVE_CALIBRATION_TOPIC)
self.save_calibration_proxy = rospy.ServiceProxy(hec.SAVE_CALIBRATION_TOPIC, std_srvs.srv.Empty)
if not self.parameters.freehand_robot_movement:
# init services: robot movement
rospy.wait_for_service(hec.CHECK_STARTING_POSE_TOPIC)
self.check_starting_pose_proxy = rospy.ServiceProxy(hec.CHECK_STARTING_POSE_TOPIC, ehm.srv.CheckStartingPose)
rospy.wait_for_service(hec.ENUMERATE_TARGET_POSES_TOPIC)
self.enumerate_target_poses_proxy = rospy.ServiceProxy(hec.ENUMERATE_TARGET_POSES_TOPIC, ehm.srv.EnumerateTargetPoses)
rospy.wait_for_service(hec.SELECT_TARGET_POSE_TOPIC)
self.select_target_pose_proxy = rospy.ServiceProxy(hec.SELECT_TARGET_POSE_TOPIC, ehm.srv.SelectTargetPose)
rospy.wait_for_service(hec.PLAN_TO_SELECTED_TARGET_POSE_TOPIC)
self.plan_to_selected_target_pose_proxy = rospy.ServiceProxy(hec.PLAN_TO_SELECTED_TARGET_POSE_TOPIC, ehm.srv.PlanToSelectedTargetPose)
rospy.wait_for_service(hec.EXECUTE_PLAN_TOPIC)
self.execute_plan_proxy = rospy.ServiceProxy(hec.EXECUTE_PLAN_TOPIC, ehm.srv.ExecutePlan)
def __init__(self, namespace=None):
if namespace is None:
namespace = rospy.get_namespace()
self.parameters = HandeyeCalibrationParameters.init_from_parameter_server(
namespace)
self.sampler = HandeyeSampler(handeye_parameters=self.parameters)
self.calibration_backends = {
'OpenCV': HandeyeCalibrationBackendOpenCV()
}
self.calibration_algorithm = 'OpenCV/Tsai-Lenz'
# setup calibration services and topics
self.list_algorithms_service = rospy.Service(hec.LIST_ALGORITHMS_TOPIC,
ehm.srv.ListAlgorithms,
self.list_algorithms)
self.set_algorithm_service = rospy.Service(hec.SET_ALGORITHM_TOPIC,
ehm.srv.SetAlgorithm,
self.set_algorithm)
self.get_sample_list_service = rospy.Service(hec.GET_SAMPLE_LIST_TOPIC,
ehm.srv.TakeSample,
self.get_sample_lists)
self.take_sample_service = rospy.Service(hec.TAKE_SAMPLE_TOPIC,
ehm.srv.TakeSample,
self.take_sample)
self.remove_sample_service = rospy.Service(hec.REMOVE_SAMPLE_TOPIC,
ehm.srv.RemoveSample,
self.remove_sample)
self.compute_calibration_service = rospy.Service(
hec.COMPUTE_CALIBRATION_TOPIC, ehm.srv.ComputeCalibration,
self.compute_calibration)
self.save_calibration_service = rospy.Service(
hec.SAVE_CALIBRATION_TOPIC, std_srvs.srv.Empty,
self.save_calibration)
# Useful for secondary input sources (e.g. programmable buttons on robot)
self.take_sample_topic = rospy.Subscriber(hec.TAKE_SAMPLE_TOPIC,
std_msgs.msg.Empty,
self.take_sample)
self.compute_calibration_topic = rospy.Subscriber(
hec.REMOVE_SAMPLE_TOPIC, std_msgs.msg.Empty,
self.remove_last_sample)
self.last_calibration = None
def __init__(self, namespace=None):
if namespace is None:
namespace = rospy.get_namespace()
self.parameters = HandeyeCalibrationParameters.init_from_parameter_server(
namespace)
self.sampler = HandeyeSampler(handeye_parameters=self.parameters)
self.calibration_service = HandeyeCalibrationServiceVisp()
# setup calibration services and topics
self.get_sample_list_service = rospy.Service(hec.GET_SAMPLE_LIST_TOPIC,
ehm.srv.TakeSample,
self.get_sample_lists)
self.take_sample_service = rospy.Service(hec.TAKE_SAMPLE_TOPIC,
ehm.srv.TakeSample,
self.take_sample)
self.remove_sample_service = rospy.Service(hec.REMOVE_SAMPLE_TOPIC,
ehm.srv.RemoveSample,
self.remove_sample)
self.compute_calibration_service = rospy.Service(
hec.COMPUTE_CALIBRATION_TOPIC, ehm.srv.ComputeCalibration,
self.compute_calibration)
self.save_calibration_service = rospy.Service(
hec.SAVE_CALIBRATION_TOPIC, std_srvs.srv.Empty,
self.save_calibration)
# Useful for secondary input sources (e.g. programmable buttons on robot)
self.take_sample_topic = rospy.Subscriber(hec.TAKE_SAMPLE_TOPIC,
std_msgs.msg.Empty,
self.take_sample)
self.compute_calibration_topic = rospy.Subscriber(
hec.REMOVE_SAMPLE_TOPIC, std_msgs.msg.Empty,
self.remove_last_sample)
self.last_calibration = None