Esempio n. 1
0
    def execute(self):

        ##########################################
        #                 GOAL i                 #
        ##########################################

        goal = GoalObject({}, 15.0)

        print "request_goal"
        self.request_goal(goal)

        print "wait_goal_result"
        self.wait_goal_result()

        ##########################################
        #    CHECK RESULT i AND UPDATE SCORE     #
        ##########################################

        if goal.has_timed_out():
            print "GOAL TIMEOUT"
        elif goal.has_been_completed():
            print "GOAL COMPLETED:"
            result = goal.get_result()
            print "result:\n", result
        else:
            print "GOAL NOT COMPLETED"

        self.save_and_publish_score()

        if not self.is_benchmark_running():
            print self.get_end_description()

            if self.has_benchmark_timed_out():
                print "BENCHMARK TIMEOUT"
            elif self.has_benchmark_been_stopped():
                print "BENCHMARK STOPPED"
            else:
                print "BENCHMARK ABORTED"
Esempio n. 2
0
    def execute(self):

        start_benchmark_time = rospy.Time.now()

        print "params:\n", self.params

        # init tf and subscribers
        tf_listener = tf.TransformListener()
        tf_broadcaster = tf.TransformBroadcaster()

        # Get the parameters of the benchmark (waypoints, penalty_time, timeout_time)
        starting_robot_pose = Pose2D(*self.params["starting_pose"])
        waypoints = self.params["waypoints"]
        goal_timeout = self.params["goal_timeout"]

        # Variables to compute score
        N = len(waypoints)
        i = 0
        sum_d = 0.0  # [m]
        sum_sin = 0.0
        sum_cos = 0.0
        execution_time = rospy.Duration(0.0)  # [s]
        timed_out_waypoints = 0
        reached_waypoints = 0

        # acquire markerset-robot transform from config
        team_name = self.get_benchmark_team()
        transform_path = path.join(
            path.join(rospy.get_param("~resources_directory"), "transforms"),
            "transform-%s.yaml" % team_name)

        with open(transform_path, 'r') as transform_file:
            markerset_to_robot_transform = yaml.load(
                transform_file)["markerset_to_robot_transform"]

        print "team_name: ", team_name, "\ttransform_path: ", transform_path, "\tmarkerset_to_robot_transform: ", markerset_to_robot_transform

        while self.is_benchmark_running() and i < N:

            self.score["segment_%i" % (i + 1)] = {}

            ##########################################
            #                 GOAL i                 #
            ##########################################

            goal = GoalObject(waypoints[i], goal_timeout)

            print "request_goal"
            self.request_goal(goal)

            start_segment_time = rospy.Time.now()
            self.score["segment_%i" % (i + 1)]["start_segment_time[s]"] = (
                start_segment_time - start_benchmark_time).to_sec()
            self.save_and_publish_score()

            print "wait_goal_result"
            self.wait_goal_result()

            end_segment_time = rospy.Time.now()
            self.score["segment_%i" % (i + 1)]["end_segment_time[s]"] = (
                end_segment_time - start_benchmark_time).to_sec()
            self.save_and_publish_score()

            segment_time = end_segment_time - start_segment_time
            execution_time += segment_time

            self.score["segment_%i" %
                       (i + 1)]["segment_time[s]"] = segment_time.to_sec()
            self.save_and_publish_score()
            rospy.loginfo("segment_time: %f" % segment_time.to_sec())

            ##########################################
            #    CHECK RESULT i AND UPDATE SCORE     #
            ##########################################

            self.score["segment_%i" %
                       (i + 1)]["timeout"] = goal.has_timed_out()
            self.score["segment_%i" %
                       (i + 1)]["completed"] = goal.has_been_completed()

            if goal.has_timed_out():
                print "GOAL TIMEOUT"
                timed_out_waypoints += 1
            elif goal.has_been_completed():
                print "GOAL COMPLETED:"
                result = goal.get_result()
                reached_waypoints += 1
                print "result:\n", result
                self.score["segment_%i" % (i + 1)]["result"] = result
            else:
                print "GOAL NOT COMPLETED"

            self.save_and_publish_score()

            # Collect the target and robot poses (using tf)
            target_pose = Pose2D(*waypoints[i])
            robot_pose = None

            robot_pose = Pose2D_from_tf_concatenation(
                self,
                "/robot",
                markerset_to_robot_transform[0],
                markerset_to_robot_transform[1],
                "/robot_markerset",
                "/testbed_origin",
                tf_broadcaster,
                tf_listener,
                max_failed_attempts=5)

            # If the pose could not be acquired, ask the RefOp if the waypoint should be skipped or try again
            while robot_pose == None:
                rospy.logwarn(
                    "The position of the robot markerset could not be acquired"
                )

                manual_operation_3 = ManualOperationObject(
                    "The position of the robot markerset could not be acquired!\nTry again or skip the waypoint? type 's' for skip, anything else to try again"
                )

                self.request_manual_operation(manual_operation_3)

                if manual_operation_3.has_been_completed():
                    print "First Manual Operation result: %s" % manual_operation_3.get_result(
                    )

                    if manual_operation_3.get_result() == 's':
                        rospy.logwarn("Waypoint %i SKIPPED" % (i + 1))
                        break

                    else:
                        rospy.logwarn("Checking tracking again")

                else:
                    print "First Manual Operation NOT EXECUTED"

                if not self.is_benchmark_running():
                    if self.has_benchmark_timed_out():
                        print "BENCHMARK TIMEOUT"
                        return
                    elif self.has_benchmark_been_stopped():
                        print "BENCHMARK STOPPED"
                        return
                    else:
                        print "BENCHMARK ABORTED"
                        return

                robot_pose = Pose2D_from_tf_concatenation(
                    self,
                    "/robot",
                    markerset_to_robot_transform[0],
                    markerset_to_robot_transform[1],
                    "/robot_markerset",
                    "/testbed_origin",
                    tf_broadcaster,
                    tf_listener,
                    max_failed_attempts=5)

            self.score["segment_%i" %
                       (i + 1)]["target_pose"] = yaml.dump(target_pose)
            self.score["segment_%i" %
                       (i + 1)]["robot_pose"] = yaml.dump(robot_pose)

            robot_pose_acquired = robot_pose != None
            print "\n\n\n\n\n\nrobot_pose:\n", robot_pose, "\n\n\n\n\n\n"

            if robot_pose_acquired:
                # Evaluate position and orientation error between target pose (tp) and robot pose (rp)
                rp = robot_pose  # robot position
                tp = target_pose  # target position

                d_error = sqrt((rp.x - tp.x)**2 + (rp.y - tp.y)**2)
                sum_d = sum_d + d_error

                sin_error = sin(fabs(rp.theta - tp.theta))
                sum_sin += sin_error
                cos_error = cos(fabs(rp.theta - tp.theta))
                sum_cos += cos_error

                self.score["segment_%i" % (i + 1)]["distance_error"] = d_error
                self.score["segment_%i" %
                           (i + 1)]["orientation_error"] = atan2(
                               fabs(sin_error), cos_error)

                rospy.loginfo(
                    "\n\n - segment_time: %s\n - dinstance_error: %s\n - orientation_error: %s"
                    % (segment_time.to_sec(),
                       self.score["segment_%i" % (i + 1)]["distance_error"],
                       self.score["segment_%i" %
                                  (i + 1)]["orientation_error"]))

            self.save_and_publish_score()

            if not self.is_benchmark_running():
                print self.get_end_description()

                if self.has_benchmark_timed_out():
                    print "BENCHMARK TIMEOUT"
                elif self.has_benchmark_been_stopped():
                    print "BENCHMARK STOPPED"
                else:
                    print "BENCHMARK ABORTED"

            i += 1

        ##########################################
        #            UPDATE SCORE                #
        ##########################################

        # Evaluate final score
        if reached_waypoints > 0:
            position_accuracy = sum_d / reached_waypoints
            orientation_accuracy = atan2(fabs(sum_sin), sum_cos)
        else:
            position_accuracy = None
            orientation_accuracy = None

        self.score["reached_waypoints"] = reached_waypoints
        self.score["timed_out_waypoints"] = timed_out_waypoints
        self.score["execution_time[s]"] = execution_time.to_sec()
        self.score["position_accuracy[m]"] = position_accuracy
        self.score["orientation_accuracy[rad]"] = orientation_accuracy
        self.save_and_publish_score()
Esempio n. 3
0
    def execute(self):
        # Load the list of people's names from the config file
        people = self.params["people"]

        # Num. of requested goals: equal to the num. of people
        N = len(people)

        # init tf
        tf_listener = tf.TransformListener()
        tf_broadcaster = tf.TransformBroadcaster()

        # Variables to compute score
        i = 0
        detected_sum = 0
        recognized_sum = 0
        position_error_sum = 0
        execution_time = rospy.Duration(0)

        # Start the benchmark
        while self.is_benchmark_running() and (i < N):
            # Pick a random person and remove it from the list
            person = random.choice(people)
            people.remove(person)
            self.score["goal_%i" % i] = {}

            ##########################################
            #            MANUAL OPERATION            #
            ##########################################

            manual_operation_1 = ManualOperationObject(
                "%s should enter the target area wearing a cap with MoCap markers, and place him/herself in an arbitrary position."
                % person)

            self.request_manual_operation(manual_operation_1)

            ##########################################
            #     CHECK RESULT AND UPDATE SCORE      #
            ##########################################

            if manual_operation_1.has_been_completed():
                print "First Manual Operation result: %s" % manual_operation_1.get_result(
                )
            else:
                print "First Manual Operation NOT EXECUTED"

            if not self.is_benchmark_running():
                if self.has_benchmark_timed_out():
                    print "BENCHMARK TIMEOUT"
                elif self.has_benchmark_been_stopped():
                    print "BENCHMARK STOPPED"
                else:
                    print "BENCHMARK ABORTED"
                return

            ## Fist manual operation finished, the person should be trackable inside the area
            # Check if we can get the person's pose - although we don't use the pose now, only after the robot's end_execute signal
            person_pose_mocap = None
            person_pose_mocap = Pose2D_from_tf_concatenation(
                self,
                "/person", (0, 0, 0), (0, 0, 0, 1),
                "/actor_markerset",
                "/testbed_origin",
                tf_broadcaster,
                tf_listener,
                max_failed_attempts=3)
            while person_pose_mocap == None:
                rospy.logwarn(
                    "The position of the person markerset could not be acquired - asking RefOp to retry"
                )
                manual_operation_2 = ManualOperationObject(
                    "Could not get %s's pose. Make sure (s)he is inside the designated area and wearing the markerset cap."
                    % person)

                self.request_manual_operation(manual_operation_2)

                if manual_operation_2.has_been_completed():
                    print "Second Manual Operation result: %s" % manual_operation_2.get_result(
                    )
                else:
                    print "Second Manual Operation NOT EXECUTED"

                if not self.is_benchmark_running():
                    if self.has_benchmark_timed_out():
                        print "BENCHMARK TIMEOUT"
                    elif self.has_benchmark_been_stopped():
                        print "BENCHMARK STOPPED"
                    else:
                        print "BENCHMARK ABORTED"
                    return

                person_pose_mocap = Pose2D_from_tf_concatenation(
                    self,
                    "/person", (0, 0, 0), (0, 0, 0, 1),
                    "/actor_markerset",
                    "/testbed_origin",
                    tf_broadcaster,
                    tf_listener,
                    max_failed_attempts=3)

            #########################################################
            #   PERSON IS IN THE AREA, SEND THE GOAL TO THE ROBOT   #
            #########################################################

            ##########################################
            #                 GOAL i                 #
            ##########################################

            goal = GoalObject({}, self.params['goal_timeout'])

            self.request_goal(goal)
            start_time = rospy.Time.now()

            print "wait_goal_result"

            self.wait_goal_result()
            end_time = rospy.Time.now()

            segment_time = end_time - start_time

            execution_time += segment_time

            self.score["goal_%i" %
                       i]["segment_time [s]"] = segment_time.to_sec()
            self.save_and_publish_score()
            rospy.loginfo("segment_time: %f" % segment_time.to_sec())

            ##########################################
            #    CHECK RESULT i AND UPDATE SCORE     #
            ##########################################

            self.score["goal_%i" % i]["timeout"] = goal.has_timed_out()
            self.score["goal_%i" % i]["completed"] = goal.has_been_completed()

            if goal.has_timed_out():
                print "GOAL TIMEOUT"
            elif goal.has_been_completed():
                print "GOAL COMPLETED"
                print "RESULT: %s" % (goal.get_result())
            else:
                print "GOAL NOT COMPLETED"

            if goal.has_been_completed():

                # Acquire person position from actor_markerset, which is the ground truth
                person_pose_mocap = None
                person_pose_mocap = Pose2D_from_tf_concatenation(
                    self,
                    "/person", (0, 0, 0), (0, 0, 0, 1),
                    "/actor_markerset",
                    "/testbed_origin",
                    tf_broadcaster,
                    tf_listener,
                    max_failed_attempts=3)
                if person_pose_mocap == None:
                    # Robot has finished the goal, but the pose couldn't be acquired from MoCap: restart the goal
                    rospy.logwarn(
                        "The position of the person markerset could not be acquired - restarting current goal"
                    )

                    execution_time -= segment_time
                    people.append(person)

                    continue

                detected_sum += 1

                result = goal.get_result()

                rospy.loginfo(
                    "Received result - X: %.2f \t Y: %.2f \t Theta: %.2f \t Person name: %s"
                    % (result['x'], result['y'], result['theta'],
                       result['person_name']))

                # Compare person name (case-insensitive)
                recognized = (result['person_name'].lower() == person.lower())
                if recognized:
                    recognized_sum += 1

                # Calculate position error
                result_person_pose = Pose2D(result['x'], result['y'],
                                            result['theta'])

                x_err = result_person_pose.x - person_pose_mocap.x
                y_err = result_person_pose.y - person_pose_mocap.y
                position_error = sqrt(x_err**2 + y_err**2)

                position_error_sum += position_error

                # TODO no orientation error for now: should it be added?

                # Update the score with the result of the current goal
                self.score["goal_%i" % i]['detected'] = True
                self.score[
                    "goal_%i" %
                    i]['robot-estimated person name'] = result['person_name']
                self.score["goal_%i" % i]['ground-truth person name'] = person
                self.score["goal_%i" % i]['person_recognized'] = recognized
                self.score["goal_%i" %
                           i]['person_position_error'] = position_error
                self.score[
                    "goal_%i" %
                    i]['ground-truth person position'] = '(%.2f, %.2f)' % (
                        person_pose_mocap.x, person_pose_mocap.y)
                self.score[
                    "goal_%i" %
                    i]['robot-estimated person position'] = '(%.2f, %.2f)' % (
                        result_person_pose.x, result_person_pose.y)

                # Update overall score
                self.score['Recognized persons'] = recognized_sum
                self.score['% Recognized persons'] = str(
                    round(recognized_sum / float(N) * 100, 2)) + '%'
                self.score[
                    'Distance error'] = position_error_sum / detected_sum

            else:
                self.score["goal_%i" % i]['detected'] = False

            # Update execution time whether the goal was completed or not
            self.score['execution_time'] = execution_time.to_sec()

            self.save_and_publish_score()

            if not self.is_benchmark_running():
                if self.has_benchmark_timed_out():
                    print "BENCHMARK TIMEOUT"
                elif self.has_benchmark_been_stopped():
                    print "BENCHMARK STOPPED"
                else:
                    print "BENCHMARK ABORTED"
                return

            i += 1
Esempio n. 4
0
    def execute(self):

        # Benchmark runs
        N = 10

        # Ground truth accuracy
        TRANS_ACCURACY = 0.0  # meters
        ROT_ACCURACY = 0.0  # degrees

        # init tf
        tf_listener = tf.TransformListener()
        tf_broadcaster = tf.TransformBroadcaster()

        # Get items
        items_transform_path = path.join(
            path.join(rospy.get_param("~resources_directory"),
                      "objects_informations"), "items_with_pose.yaml")
        try:
            with open(items_transform_path, 'r') as items_transform_file:
                items = yaml.load(items_transform_file)["items"]
        except (EnvironmentError):
            rospy.logerr("Could not open items pose file [%s]" %
                         (items_transform_path))
            return

        # Variables to compute score
        i = 0
        class_positives = 0.0
        instance_positives = 0.0
        pose_score_sum = 0.0
        execution_time = rospy.Duration(0)

        # Maximum pose error
        max_pose_error = sqrt(
            pow(float(self.params['table_size']['x']), 2) +
            pow(float(self.params['table_size']['y']), 2))

        # Start the benchmark
        while self.is_benchmark_running() and (i < N):
            i += 1

            # Pick a random item and remove it from the list
            item = random.choice(items)
            items.remove(item)
            self.score["goal_%i" % i] = {}

            ##########################################
            #            MANUAL OPERATION            #
            ##########################################

            manual_operation_1 = ManualOperationObject(
                "Place the positioner and the %s (id: %d, instance: %s) on the table"
                % (item['description'], item['id'], item['instance']))

            self.request_manual_operation(manual_operation_1)

            ##########################################
            #     CHECK RESULT AND UPDATE SCORE      #
            ##########################################

            if manual_operation_1.has_been_completed():
                print "First Manual Operation result: %s" % manual_operation_1.get_result(
                )
            else:
                print "First Manual Operation NOT EXECUTED"

            if not self.is_benchmark_running():
                if self.has_benchmark_timed_out():
                    print "BENCHMARK TIMEOUT"
                    return
                elif self.has_benchmark_been_stopped():
                    print "BENCHMARK STOPPED"
                    return
                else:
                    print "BENCHMARK ABORTED"
                    return

#			now = rospy.Time.now()

##########################################
#   ACQUIRE OBJECT POSITION WITH MOCAP   #
##########################################

            acquired_object_pose = None
            acquired_object_pose = Pose2D_from_tf_concatenation(
                self,
                "/item",
                item['trans'],
                item['rot'],
                "/positioner_origin",
                "/table_origin",
                tf_broadcaster,
                tf_listener,
                max_failed_attempts=3)

            # If the position could not be acquired, try again by requesting to move the positioner
            while acquired_object_pose == None:
                rospy.logwarn(
                    "The position of the positioner could not be acquired")

                manual_operation_3 = ManualOperationObject(
                    "The position of the positioner could not be acquired!\nPlace in a different position the positioner and the %s (id: %d, instance: %s) on the table"
                    % (item['description'], item['id'], item['instance']))

                self.request_manual_operation(manual_operation_3)

                ##########################################
                #     CHECK RESULT AND UPDATE SCORE      #
                ##########################################

                if manual_operation_3.has_been_completed():
                    print "First Manual Operation result: %s" % manual_operation_3.get_result(
                    )
                else:
                    print "First Manual Operation NOT EXECUTED"

                if not self.is_benchmark_running():
                    if self.has_benchmark_timed_out():
                        print "BENCHMARK TIMEOUT"
                        return
                    elif self.has_benchmark_been_stopped():
                        print "BENCHMARK STOPPED"
                        return
                    else:
                        print "BENCHMARK ABORTED"
                        return

                acquired_object_pose = Pose2D_from_tf_concatenation(
                    self,
                    "/item",
                    item['trans'],
                    item['rot'],
                    "/positioner_origin",
                    "/table_origin",
                    tf_broadcaster,
                    tf_listener,
                    max_failed_attempts=3)

            ##########################################
            #            MANUAL OPERATION            #
            ##########################################

            manual_operation_2 = ManualOperationObject("Remove the positioner")

            self.request_manual_operation(manual_operation_2)

            ##########################################
            #     CHECK RESULT AND UPDATE SCORE      #
            ##########################################

            if manual_operation_2.has_been_completed():
                print "First Manual Operation result: %s" % manual_operation_2.get_result(
                )
            else:
                print "First Manual Operation NOT EXECUTED"

            if not self.is_benchmark_running():
                if self.has_benchmark_timed_out():
                    print "BENCHMARK TIMEOUT"
                    return
                elif self.has_benchmark_been_stopped():
                    print "BENCHMARK STOPPED"
                    return
                else:
                    print "BENCHMARK ABORTED"
                    return

            ##########################################
            #                 GOAL i                 #
            ##########################################

            goal = GoalObject({}, self.params['goal_timeout'])

            self.request_goal(goal)
            start_time = rospy.Time.now()

            print "wait_goal_result"

            self.wait_goal_result()
            end_time = rospy.Time.now()

            segment_time = end_time - start_time

            execution_time += segment_time
            rospy.loginfo("Execution time - %f" % execution_time.to_sec())

            ##########################################
            #    CHECK RESULT i AND UPDATE SCORE     #
            ##########################################

            self.score["goal_%i" % i]["timeout"] = goal.has_timed_out()
            self.score["goal_%i" % i]["completed"] = goal.has_been_completed()
            self.score["goal_%i" % i]["segment_time"] = segment_time.to_sec()

            if goal.has_timed_out():
                print "GOAL TIMEOUT"
            elif goal.has_been_completed():
                print "GOAL COMPLETED"
                print "RESULT:  %s" % (goal.get_result())
            else:
                print "GOAL NOT COMPLETED"

            self.save_and_publish_score()

            if not self.is_benchmark_running():
                print self.get_end_description()

                if self.has_benchmark_timed_out():
                    print "BENCHMARK TIMEOUT"
                elif self.has_benchmark_been_stopped():
                    print "BENCHMARK STOPPED"
                else:
                    print "BENCHMARK ABORTED"

            if goal.has_been_completed():

                result = goal.get_result()

                result_object_pose = Pose2D(result['x'], result['y'],
                                            result['theta'])

                rospy.loginfo(
                    "Received result - X: %.2f \t Y: %.2f \t W: %.2f rad \t Class: %s \t Instance: %s"
                    % (result_object_pose.x, result_object_pose.y,
                       result_object_pose.theta, result['class'],
                       result['instance']))

                # Evaluate position error
                tf_broadcaster.sendTransform(
                    [result_object_pose.x, result_object_pose.y, 0],
                    tf.transformations.quaternion_from_euler(
                        0, 0, result_object_pose.theta), rospy.Time.now(),
                    "result_%d" % i, "origin")

                # Evaluate class
                is_class_correct = (result['class'] == item['class'])
                if (is_class_correct):
                    class_positives += 1

                # Evaluate instance
                is_instance_correct = (result['instance'] == item['instance'])
                if (is_instance_correct):
                    instance_positives += 1

                x_err = result_object_pose.x - acquired_object_pose.x
                y_err = result_object_pose.y - acquired_object_pose.y
                w_err = (result_object_pose.theta - acquired_object_pose.theta)

                if w_err > pi: w_err = w_err - 2 * pi
                if w_err < -pi: w_err = w_err + 2 * pi
                if abs(x_err) < TRANS_ACCURACY: x_err = 0.0
                if abs(y_err) < TRANS_ACCURACY: y_err = 0.0
                if abs(w_err) < ROT_ACCURACY: w_err = 0.0

                pose_err_trans = sqrt(pow(x_err, 2) + pow(y_err, 2))
                #				if pose_err_trans > max_pose_error : pose_err_trans = 1.0 # corrected with version from Lisbon 2017 Aug event

                pose_err_rot = abs(w_err) / pi
                if pose_err_rot > 1: pose_err_rot = 1.0

                pose_score = 1 - (pose_err_trans / 2) - (pose_err_rot / 2)
                pose_score_sum += pose_score

                # Update the score with the result of the current goal
                self.score["goal_%i" % i]['item_acquired'] = {}
                self.score["goal_%i" % i]['item_acquired']['pose'] = [
                    acquired_object_pose.x, acquired_object_pose.y,
                    acquired_object_pose.theta
                ]
                self.score["goal_%i" % i]['item_acquired']['pose_units'] = [
                    'm', 'm', 'rad'
                ]
                self.score["goal_%i" %
                           i]['item_acquired']['class'] = item['class']
                self.score["goal_%i" %
                           i]['item_acquired']['instance'] = item['instance']

                self.score["goal_%i" % i]['item_received'] = {}
                self.score["goal_%i" % i]['item_received']['pose'] = [
                    result_object_pose.x, result_object_pose.y,
                    result_object_pose.theta
                ]
                self.score["goal_%i" % i]['item_received']['pose_units'] = [
                    'm', 'm', 'rad'
                ]
                self.score["goal_%i" %
                           i]['item_received']['class'] = result['class']
                self.score["goal_%i" %
                           i]['item_received']['instance'] = result['instance']

                self.score["goal_%i" % i]['partial_score'] = {}
                self.score["goal_%i" %
                           i]['partial_score']['pose_score'] = pose_score
                self.score["goal_%i" % i]['partial_score'][
                    'pose_error_translation_score'] = pose_err_trans
                self.score["goal_%i" % i]['partial_score'][
                    'pose_error_translation_score_unit'] = 'pure number'
                self.score["goal_%i" % i]['partial_score'][
                    'pose_error_rotation_score'] = pose_err_rot
                self.score["goal_%i" % i]['partial_score'][
                    'pose_error_rotation_score_unit'] = 'pure number'
                self.score["goal_%i" % i]['partial_score']['pose_error'] = [
                    x_err, y_err, w_err
                ]
                self.score["goal_%i" %
                           i]['partial_score']['pose_error_units'] = [
                               'm', 'm', 'rad'
                           ]
                self.score[
                    "goal_%i" %
                    i]['partial_score']['is_class_correct'] = is_class_correct
                self.score["goal_%i" % i]['partial_score'][
                    'is_instance_correct'] = is_instance_correct

                self.save_and_publish_score()

            # Update the global score with the result of the current goal (whether the goal was completed or not)
            self.score['class_accuracy'] = class_positives / i
            self.score['instance_accuracy'] = instance_positives / i
            self.score['pose_score'] = pose_score_sum / i
            self.score['execution_time'] = execution_time.to_sec()

            self.save_and_publish_score()
Esempio n. 5
0
	def execute(self):
		
		N = 2
		i = 1
		execution_time = rospy.Duration(0.0)
		
		
		print "params:\n", self.params
				
		print "referee_score:\n", self.referee_score
		
		##########################################
		#            MANUAL OPERATION            #
		##########################################
		
		manual_operation_first = ManualOperationObject("First Manual Operation")
		
		self.start_manual_operation(manual_operation_first)
		self.wait_manual_operation()
		
		##########################################
		#     CHECK RESULT AND UPDATE SCORE      #
		##########################################
		
		if manual_operation_first.has_been_completed():
			print "First Manual Operation result: %s" % manual_operation_first.get_result()
			self.score["first_manual_operation"] = manual_operation_first.get_result()
			self.save_and_publish_score()
		else:
			print "First Manual Operation NOT EXECUTED"
			self.score["first_manual_operation"] = "not executed"
			self.save_and_publish_score()
		
		if not self.is_benchmark_running():
			if self.has_benchmark_timed_out():
				print "BENCHMARK TIMEOUT"
				return
			elif self.has_benchmark_been_stopped():
				print "BENCHMARK STOPPED"
				return
			else:
				print "BENCHMARK ABORTED"
				return
		
		
		while self.is_benchmark_running() and i <= N:
			
			##########################################
			#                 GOAL i                 #
			##########################################
			
			goal = GoalObject({"goal": "GOAL %d"%i, "details": 4}, 15.0)
			
			self.request_goal(goal)
			start_time = rospy.Time.now()
			
			print "wait_goal_result"
			
			self.wait_goal_result()
			end_time = rospy.Time.now()
			
#T			while not goal.has_been_completed() and not goal.has_timed_out(): rate.sleep(); self.complete_goal()
			
			execution_time += end_time - start_time
			rospy.loginfo("Execution time - %f" % execution_time.to_sec())
			
			
			##########################################
			#    CHECK RESULT i AND UPDATE SCORE     #
			##########################################
			
			self.score["goal_%i"%i] = {}
			self.score["goal_%i"%i]["timeout"] = goal.has_timed_out()
			self.score["goal_%i"%i]["completed"] = goal.has_been_completed()
			
			if goal.has_timed_out():
				print "GOAL TIMEOUT"
			elif goal.has_been_completed():
				print "GOAL COMPLETED:"
				result = goal.get_result()
				print "result:\n", result
				self.score["goal_%i"%i]["result"] = result
			else:
				print "GOAL NOT COMPLETED"
			
			self.save_and_publish_score()
			
			if not self.is_benchmark_running():
				print self.get_end_description()
				
				if self.has_benchmark_timed_out():
					print "BENCHMARK TIMEOUT"
				elif self.has_benchmark_been_stopped():
					print "BENCHMARK STOPPED"
				else:
					print "BENCHMARK ABORTED"
			
			i += 1
		
		##########################################
		#            MANUAL OPERATION            #
		##########################################
		
		manual_operation_last = ManualOperationObject("Last Manual Operation")
		
		self.request_manual_operation(manual_operation_last)
		
		
		##########################################
		#     CHECK RESULT AND UPDATE SCORE      #
		##########################################
		
		if manual_operation_last.has_been_completed():
			print "Last Manual Operation result: %s" % manual_operation_last.get_result()
			self.score["last_manual_operation"] = manual_operation_last.get_result()
			self.save_and_publish_score()
		else:
			print "Last Manual Operation NOT EXECUTED"
			self.score["last_manual_operation"] = "not executed"
			self.save_and_publish_score()
		
		if not self.is_benchmark_running():
			if self.has_benchmark_timed_out():
				print "BENCHMARK TIMEOUT"
			elif self.has_benchmark_been_stopped():
				print "BENCHMARK STOPPED"
			else:
				print "BENCHMARK ABORTED"
		
		
		##########################################
		#            UPDATE SCORE                #
		##########################################
		
		self.score["execution_time"] = execution_time.to_sec()
		self.save_and_publish_score()
    def execute(self):
        global tf_listener
        global object_height_reference
        global object_grasped

        # Load the list of objects from the config file
        objects = self.params["objects"]

        # Num. of requested goals: equal to the num. of objects
        N = len(objects)

        # Calculate range where objects can be placed: 15cm margin from the table edges
        table_size = self.params["table_size"]
        min_x = 0.15
        min_y = 0.15
        max_x = table_size['x'] - 0.15
        max_y = table_size['y'] - 0.15

        # init tf
        tf_broadcaster = tf.TransformBroadcaster()
        tf_listener = tf.TransformListener()

        # Variables to compute score
        i = 0
        objects_grasped_sum = 0
        goals_completed_sum = 0
        distance_error_sum = 0
        angle_error_sum = 0
        execution_time = rospy.Duration(0)

        # Start the benchmark
        while self.is_benchmark_running() and (i < N):
            # Pick a random object and remove it from the list
            obj = random.choice(objects)
            objects.remove(obj)
            self.score["goal_%i" % i] = {}
            object_grasped = False

            # Pick a random target position for the object
            target_x = random.uniform(min_x, max_x)
            target_y = random.uniform(min_y, max_y)

            ##########################################
            #            MANUAL OPERATION            #
            ##########################################

            manual_operation_1 = ManualOperationObject(
                "Place the %s on the table, with MoCap markers attached." %
                obj['description'])

            self.request_manual_operation(manual_operation_1)

            ##########################################
            #     CHECK RESULT AND UPDATE SCORE      #
            ##########################################

            if manual_operation_1.has_been_completed():
                print "First Manual Operation result: %s" % manual_operation_1.get_result(
                )
            else:
                print "First Manual Operation NOT EXECUTED"

            if not self.is_benchmark_running():
                if self.has_benchmark_timed_out():
                    print "BENCHMARK TIMEOUT"
                elif self.has_benchmark_been_stopped():
                    print "BENCHMARK STOPPED"
                else:
                    print "BENCHMARK ABORTED"
                return

            ## Fist manual operation finished, the object should be trackable on the table
            # Check if we can get the object's pose, and if it's not too close to the target position
            object_pose_mocap = None
            # We assume the markerset is perfectly aligned with the object - if it's not, add a markerset-acquiring step and use the results here
            (object_pose_mocap,
             object_height_reference) = Pose2D_from_tf_concatenation(
                 self,
                 "/object", (0, 0, 0), (0, 0, 0, 1),
                 "/actor_markerset",
                 "/table_origin",
                 tf_broadcaster,
                 tf_listener,
                 max_failed_attempts=3)

            while object_pose_mocap == None:
                rospy.logwarn(
                    "The position of the object markerset could not be acquired - asking RefOp to retry"
                )
                manual_operation_2 = ManualOperationObject(
                    "Could not get the %s's pose. Make sure it's inside the designated area with a markerset attached."
                    % obj['description'])

                self.request_manual_operation(manual_operation_2)

                if manual_operation_2.has_been_completed():
                    print "Second Manual Operation result: %s" % manual_operation_2.get_result(
                    )
                else:
                    print "Second Manual Operation NOT EXECUTED"

                if not self.is_benchmark_running():
                    if self.has_benchmark_timed_out():
                        print "BENCHMARK TIMEOUT"
                    elif self.has_benchmark_been_stopped():
                        print "BENCHMARK STOPPED"
                    else:
                        print "BENCHMARK ABORTED"
                    return

                (object_pose_mocap,
                 object_height_reference) = Pose2D_from_tf_concatenation(
                     self,
                     "/object", (0, 0, 0), (0, 0, 0, 1),
                     "/actor_markerset",
                     "/table_origin",
                     tf_broadcaster,
                     tf_listener,
                     max_failed_attempts=3)

            # If target location is too close to the object, pick a different one
            distance_from_target = sqrt((object_pose_mocap.x - target_x)**2 +
                                        (object_pose_mocap.y - target_y)**2)
            while distance_from_target < 0.05:
                target_x = random.uniform(min_x, max_x)
                target_y = random.uniform(min_y, max_y)
                distance_from_target = sqrt(
                    (object_pose_mocap.x - target_x)**2 +
                    (object_pose_mocap.y - target_y)**2)

            #########################################################
            #   OBJECT IS IN THE TABLE, SEND THE GOAL TO THE ROBOT  #
            #########################################################

            ##########################################
            #                 GOAL i                 #
            ##########################################

            goal = GoalObject([target_x, target_y, obj['id']],
                              self.params['goal_timeout'])

            self.request_goal(goal)
            start_time = rospy.Time.now()

            # Start a timer: 10x / second, acquire the object pose and check if it has lifted from the table.
            timer = rospy.Timer(rospy.Duration(0.1), timer_callback)

            print "wait_goal_result"

            self.wait_goal_result()
            end_time = rospy.Time.now()

            timer.shutdown()

            segment_time = end_time - start_time

            execution_time += segment_time

            self.score["goal_%i" %
                       i]["segment_time [s]"] = segment_time.to_sec()
            self.save_and_publish_score()
            rospy.loginfo("segment_time: %f" % segment_time.to_sec())

            ##########################################
            #    CHECK RESULT i AND UPDATE SCORE     #
            ##########################################

            self.score["goal_%i" % i]["timeout"] = goal.has_timed_out()
            self.score["goal_%i" % i]["completed"] = goal.has_been_completed()

            if goal.has_timed_out():
                print "GOAL TIMEOUT"
            elif goal.has_been_completed():
                print "GOAL COMPLETED"
                print "RESULT: %s" % (goal.get_result())
            else:
                print "GOAL NOT COMPLETED"

            if goal.has_been_completed():
                goals_completed_sum += 1

                if object_grasped:
                    objects_grasped_sum += 1

                # Acquire object position from actor_markerset, which is the ground truth
                object_pose_mocap = None
                object_pose_mocap = Pose2D_from_tf_concatenation(
                    self,
                    "/object", (0, 0, 0), (0, 0, 0, 1),
                    "/actor_markerset",
                    "/table_origin",
                    tf_broadcaster,
                    tf_listener,
                    max_failed_attempts=3)[0]
                if object_pose_mocap == None:
                    # Robot has finished the goal, but the pose couldn't be acquired from MoCap: restart the goal
                    rospy.logwarn(
                        "The position of the person markerset could not be acquired - restarting current goal"
                    )

                    execution_time -= segment_time
                    objects.append(obj)

                    continue

                x_err = target_x - object_pose_mocap.x
                y_err = target_y - object_pose_mocap.y
                distance_error = sqrt(x_err**2 + y_err**2)

                distance_error_sum += distance_error

                # TODO no orientation error for now: should it be added?

                # Update the score with the result of the current goal
                self.score["goal_%i" % i]['Object grasped'] = object_grasped
                self.score["goal_%i" % i]['Distance error'] = distance_error
                self.score["goal_%i" %
                           i]['Target object position'] = '(%.2f, %.2f)' % (
                               target_x, target_y)
                self.score["goal_%i" %
                           i]['Measured object position'] = '(%.2f, %.2f)' % (
                               object_pose_mocap.x, object_pose_mocap.y)
                self.score["goal_%i" % i]['Object type'] = '%s (id %d)' % (
                    obj['description'], obj['id'])

                # Update overall score
                self.score["Objects grasped"] = str(
                    objects_grasped_sum) + " out of " + str(N)
                self.score[
                    'Avg distance error'] = distance_error_sum / goals_completed_sum

            else:
                self.score["goal_%i" % i]['result'] = 'Not completed'

            # Update execution time whether the goal was completed or not
            self.score['execution_time'] = execution_time.to_sec()

            self.save_and_publish_score()

            if not self.is_benchmark_running():
                if self.has_benchmark_timed_out():
                    print "BENCHMARK TIMEOUT"
                elif self.has_benchmark_been_stopped():
                    print "BENCHMARK STOPPED"
                else:
                    print "BENCHMARK ABORTED"
                return

            i += 1