cleaning_iros13.py

Go to the documentation of this file.

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import roslib; roslib.load_manifest('iri_clean_board')
import rospy
import smach
import smach_ros

from smach_ros import SimpleActionState
from smach_ros import ServiceState

from actionlib import *
from actionlib.msg import *

from iri_common_smach import st_sleep
from iri_common_smach import timed_state
from iri_common_smach import timed_service_state

from iri_clean_board.srv import *
from estirabot_msgs.msg import *
from estirabot_msgs.srv import *
from iri_planning_msgs.srv import *

class NeedInfo(smach.State):
        def __init__(self):
                smach.State.__init__(self, outcomes=['next','failed'])
                self.counter = 0

        def execute(self, userdata):
                rospy.logdebug('Executing state NeedInfo')
                return 'next'


class PradaPlanning(timed_state.TimedState):
        def __init__(self):
                timed_state.TimedState.__init__(self, outcomes=['succeeded','preempted','aborted','no_plan'],
                                     input_keys=['rules_updated_sm'],
                                     output_keys=['target_ellipses','actions'])
                self.service_topic = '/iri_clean_plan/prada_planner/getBestAction'

        def timed_execute(self, userdata):
                rospy.logdebug('Executing state PradaPlanning')
                rospy.wait_for_service(self.service_topic)
                try:
                        get_cleaning_plan = rospy.ServiceProxy(self.service_topic, PradaPlanSrv)
                        resp = get_cleaning_plan(False)

                        if resp.succeeded:
                                userdata.target_ellipses = resp.target_ellipses
                                userdata.actions = resp.actions
                                return 'succeeded'
                        else:
                                return 'no_plan'

                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'aborted'


class CalculateTrajectories(timed_state.TimedState):
        def __init__(self):
                timed_state.TimedState.__init__(self, outcomes=['succeeded','preempted','aborted','no_movements'],
                                     input_keys=['target_ellipses_sm','actions_sm','ellipses_sm','segmented_pointcloud_sm','plane_coefficients_sm'],
                                     output_keys=['poses','poses_2d_coordinates','secondary_arm'])
                self.service_topic = '/iri_clean_plan/cleaning_plan/get_trajectories_from_plan'
                self.sequence_actions = ""

        def timed_execute(self, userdata):
                self.sequence_actions = self.sequence_actions + " - " + userdata.actions_sm[0]
                rospy.logdebug('Executing state CalculateTrajectories')
                rospy.loginfo('Will execute action: ' + userdata.actions_sm[0])
                rospy.wait_for_service(self.service_topic)
                try:
                        get_trajectories_from_plan = rospy.ServiceProxy(self.service_topic, PlanToTrajectories)
                        resp = get_trajectories_from_plan(userdata.segmented_pointcloud_sm, userdata.plane_coefficients_sm, userdata.target_ellipses_sm, userdata.actions_sm)

                        if resp.movement:
                                userdata.poses = resp.poses
                                userdata.poses_2d_coordinates = resp.poses_2d_coordinates
                                userdata.secondary_arm = resp.secondary_arm
                                return 'succeeded'
                        else:
                                return 'no_movements'

                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'aborted'


class GetStateRepresentation(timed_state.TimedState):
        def __init__(self):
                timed_state.TimedState.__init__(self, outcomes=['succeeded','preempted','aborted','is_clean','get_full_info'],
                                     input_keys=['segmented_image_sm','segmented_pointcloud_sm','plane_coefficients_sm'],
                                     output_keys=['dirty_areas','distances','state_string','arm_position','segmented_pointcloud_sm','plane_coefficients_sm'])
                self.service_topic = '/iri_clean_plan/cleaning_plan/get_dirty_areas_state_representation'
                self.surface_clean_perceptions = 0

        def timed_execute(self, userdata):
                rospy.logdebug('Executing state GetStateRepresentation')
                rospy.wait_for_service(self.service_topic)
                try:
                        get_state = rospy.ServiceProxy(self.service_topic, StateRepresentation)
                        resp = get_state(userdata.segmented_image_sm)

                        if resp.is_clean:
                                if self.surface_clean_perceptions < 2:
                                        self.surface_clean_perceptions += 1
                                        return 'get_full_info'
                                else:
                                        return 'is_clean'
                        else:
                                self.surface_clean_perceptions = 0
                                userdata.dirty_areas = resp.dirty_areas
                                userdata.distances = resp.distances
                                userdata.arm_position = resp.arm_position
                                userdata.state_string = resp.state_string
                                return 'succeeded'
                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'aborted'


class MoveArm(timed_state.TimedState):
        def __init__(self):
                timed_state.TimedState.__init__(self, outcomes=['succeeded','failed'],
                                        input_keys=['poses','plane_coefficients_sm','poses_2d_coordinates','secondary_arm'],
                                        output_keys=['movements_ok'])

        def timed_execute(self, userdata):
                rospy.logdebug('Waiting for service')
                rospy.loginfo('Executing action number ' + str(self.num_executions))
                rospy.wait_for_service('/iri_clean_plan/arm_movements_by_pose/clean_movement')
                try:
                        move_arm = rospy.ServiceProxy('/iri_clean_plan/arm_movements_by_pose/clean_movement', ArmMovementsPosesSrv)
                        resp = move_arm(userdata.poses, userdata.secondary_arm, userdata.plane_coefficients_sm, 1)

                        if resp.success:
                                print 'Executing service success'
                                userdata.movements_ok = True
                                return 'succeeded'
                        else:
                                # publish unreachable points
                                pub = rospy.Publisher('/iri_clean_perception/get_board_information/unreachable_position', ImagePoint)
                                for idx in resp.too_far_points_indexes:
                                        pub.publish(userdata.poses_2d_coordinates[idx])
                                print 'Executing service failed. Maybe IK.'
                                userdata.movements_ok = False
                                return 'succeeded'
                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'failed'


class MoveArmBase(timed_state.TimedState):
        def __init__(self):
                timed_state.TimedState.__init__(self, outcomes=['next','failed'])

        def timed_execute(self, userdata):
                rospy.loginfo('Waiting for service')
                rospy.wait_for_service('/iri_clean_plan/arm_movements_by_pose/clean_movement')
                try:
                        # make a empty service message
                        clean_mov = ArmMovementsPosesSrv()
                        clean_mov.movement_type = 0
                        clean_mov.poses = [];
                        move_arm = rospy.ServiceProxy('/iri_clean_plan/arm_movements_by_pose/clean_movement', ArmMovementsPosesSrv)
                        resp = move_arm(clean_mov.poses, [], [], 0)

                        if resp.success:
                                print 'Executing service success'
                                return 'next'
                        else:
                                print 'Executing service  failed'
                                return 'next'
                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'failed'


def main():
        rospy.init_node('iri_cleaning_state_machine')

        # Create a SMACH state machine
        sm = smach.StateMachine(outcomes=['finishOk','failedVision','failedPlanning','failedMoveArm'])

        st_get_info_camera = timed_service_state.TimedServiceState('/iri_clean_perception/get_board_information/get_board_info_srv',
                            BoardInfo,
                            response_slots=['segmented_image',
                                    'pointcloud',
                                    'plane_coefficients'])
        st_get_state_representation = GetStateRepresentation()
        st_prada_planning = PradaPlanning()
        st_calculate_trajectories = CalculateTrajectories()
        st_move_arm = MoveArm()
        st_move_arm_base = MoveArmBase()

        sleep_time = 1
        st_wait_seconds = st_sleep.WaitSeconds(sleep_time)

        # Open the container
        with sm:
                # Add states to the container
                smach.StateMachine.add('NeedInfo', NeedInfo(),
                                transitions={'next':'GetInfoCamera',
                                                'failed':'failedVision'},)

                smach.StateMachine.add('GetInfoCamera', st_get_info_camera,
                                transitions={'succeeded':'GetStateRepresentation',
                                                'preempted':'failedVision',
                                                'aborted':'failedVision'},
                                remapping={'segmented_image':'segmented_image_sm',
                                                'pointcloud':'segmented_pointcloud_sm',
                                                'plane_coefficients':'plane_coefficients_sm'})

                smach.StateMachine.add('GetStateRepresentation',
                                st_get_state_representation,
                                transitions={'is_clean':'finishOk',
                                                'get_full_info':'MoveArmBase',
                                                'succeeded':'PradaPlanning',
                                                'preempted':'failedVision',
                                                'aborted':'failedVision'},
                                remapping={'dirty_areas':'dirty_areas_sm',
                                           'distances':'distances_sm',
                                           'state_string':'state_string_sm'})

                smach.StateMachine.add('PradaPlanning',
                                st_prada_planning,
                                transitions={'succeeded':'CalculateTrajectories',
                                                'preempted':'failedPlanning',
                                                'aborted':'failedPlanning',
                                                'no_plan':'NeedInfo'},
                                remapping={'target_ellipses':'target_ellipses_sm',
                                           'actions':'actions_sm'})

                smach.StateMachine.add('CalculateTrajectories',
                                st_calculate_trajectories,
                                transitions={'succeeded':'MoveArm',
                                                'preempted':'failedVision',
                                                'aborted':'failedVision',
                                                'no_movements':'NeedInfo'})


                smach.StateMachine.add('MoveArm', st_move_arm,
                                transitions={'succeeded':'NeedInfo',
                                                'failed':'failedMoveArm'},
                                remapping={'movements_ok':'movements_ok_sm'})

                smach.StateMachine.add('MoveArmBase', st_move_arm_base,
                                transitions={'next':'WaitASecond',
                                                'failed':'failedMoveArm'})

                smach.StateMachine.add('WaitASecond', st_wait_seconds,
                                transitions={'finish':'NeedInfo'})

        # smach viewer
        sis = smach_ros.IntrospectionServer('iri_cleaning', sm, '/SM_LEARNING')
        sis.start()

        start_time = time.time()

        # Execute SMACH plan
        try:
                outcome = sm.execute()
        finally:
                sis.stop()
                total_time = time.time() - start_time

                # time
                time_perception = st_get_info_camera.get_execution_time()
                time_planning = st_prada_planning.get_execution_time()
                time_state = st_get_state_representation.get_execution_time()
                time_state_traj = st_calculate_trajectories.get_execution_time()
                time_moving = st_move_arm.get_execution_time()
                time_moving_home = st_move_arm_base.get_execution_time()

                # number and names of executed actions
                num_actions = st_move_arm.get_number_executions()
                sequence_actions = st_calculate_trajectories.sequence_actions

                # sum times
                time_state = time_state + time_state_traj

                rospy.loginfo('Time taken: ' + str(total_time))
                rospy.loginfo('Time taken perception: ' + str(time_perception))
                rospy.loginfo('Time taken state: ' + str(time_state))
                rospy.loginfo('Time taken planning: ' + str(time_planning))
                rospy.loginfo('Time taken moving: ' + str(time_moving))
                rospy.loginfo('Time taken moving home: ' + str(time_moving_home))
                rospy.loginfo('Number of actions: ' + str(num_actions))
                rospy.loginfo('Sequence of actions: ' + sequence_actions)
                
                if len(sys.argv) > 1:
                        file_name = sys.argv[2]
                        f = open(file_name, 'a')
                        f.write('\n')
                        f.write('Time taken: ' + str(total_time) + '\n')
                        f.write('Time taken perception: ' + str(time_perception) + '\n')
                        f.write('Time taken state: ' + str(time_state) + '\n')
                        f.write('Time taken planning: ' + str(time_planning) + '\n')
                        f.write('Time taken moving: ' + str(time_moving) + '\n')
                        f.write('Time taken moving home: ' + str(time_moving_home) + '\n')
                        f.write('Number of actions: ' + str(num_actions) + '\n')
                        f.write('Sequence of actions: ' + sequence_actions + '\n')



if __name__ == '__main__':
        main()