def __init__(self, feetFollowerWithCorrection, robot, corba = None):
     ErrorEstimationStrategy.__init__(self,
                                      robot, feetFollowerWithCorrection)
     self.errorEstimator = ErrorMerger('error_merger')
     self.feetFollowerWithCorrection = feetFollowerWithCorrection
     self.robot = robot
class MotionPlanErrorEstimationStrategy(ErrorEstimationStrategy):
    errorEstimatorId = 0

    localizationPlannedBody = 'waist'
    errorEstimators = []

    # reference toward the motion plan, added after this object
    # construction and before starting the error estimation.
    motionPlan = None

    def __init__(self, feetFollowerWithCorrection, robot, corba = None):
        ErrorEstimationStrategy.__init__(self,
                                         robot, feetFollowerWithCorrection)
        self.errorEstimator = ErrorMerger('error_merger')
        self.feetFollowerWithCorrection = feetFollowerWithCorrection
        self.robot = robot

    def start(self, interactive = False):
        for control in self.motionPlan.control:
            name = 'error_estimator' + \
                str(MotionPlanErrorEstimationStrategy.errorEstimatorId)
            MotionPlanErrorEstimationStrategy.errorEstimatorId += 1
            self.errorEstimator.addErrorEstimation(name)

            estimator = None
            if interactive:
                estimator = control.interactiveStart(name,
                                                     self.feetFollowerWithCorrection)
            else:
                estimator = control.start(name, self.feetFollowerWithCorrection)

            if type(estimator) == ErrorEstimator:
                plug(estimator.error,
                     self.errorEstimator.signal("error_" + name))
                self.errorEstimators.append(estimator)
            else:
                # If this is not an error estimator, we suppose it is a constant
                # value that can be used to set the signal.
                self.errorEstimator.signal("error_" + name).value = estimator

            self.errorEstimator.signal("weight_" + name).value = \
                (control.weight,)

            if self.motionPlan.trace and type(estimator) == ErrorEstimator:
                addTrace(self.motionPlan.robot,
                         self.motionPlan.trace,
                         name, 'error')
                addTrace(self.motionPlan.robot,
                         self.motionPlan.trace,
                         name, 'dbgPositionWorldFrame')
                addTrace(self.motionPlan.robot,
                         self.motionPlan.trace,
                         name, 'dbgPlanned')
                addTrace(self.motionPlan.robot,
                         self.motionPlan.trace,
                         name, 'dbgIndex')

        if self.motionPlan.trace:
            addTrace(self.motionPlan.robot,
                     self.motionPlan.trace,
                     self.errorEstimator.name, 'error')
        return True

    def interactiveStart(self):
        return self.start(interactive = True)

    def __str__(self):
        return "motion plan error estimation strategy"