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