sm_cleaning_wam.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_clean_board.msg import *
from iri_clean_board.srv import *
from estirabot_msgs.msg import *
from estirabot_msgs.srv import *
#from iri_wam_common_msgs.srv import *
#from geometry_msgs.msg import *
#import tf

# timer
import time

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

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


# define state Bar
class Planning(smach.State):
        def __init__(self):
                smach.State.__init__(self, outcomes=['next','finish','get_info','get_full_info','failed'],
                                     input_keys=['image_segm','pointcloud_segm','plane_coefficients'],
                                     output_keys=['poses','plane_coefficients','poses_2d_coordinates','secondary_arm'])
                self.surface_clean_perceptions = 0
        

        def execute(self, userdata):
                if userdata.pointcloud_segm.height == 0:
                        rospy.loginfo('Pointcloud isn\'t available yet')
                        return 'get_info'
                
                rospy.loginfo('Executing state Planning')
                rospy.wait_for_service('/iri_clean_plan/cleaning_plan/get_cleaning_plan')
                try:
                        get_cleaning_plan = rospy.ServiceProxy('/iri_clean_plan/cleaning_plan/get_cleaning_plan', PlanSrv)
                        resp = get_cleaning_plan(userdata.pointcloud_segm, userdata.image_segm, userdata.plane_coefficients)
                        if resp.is_clean:
                                if self.surface_clean_perceptions < 2:
                                        self.surface_clean_perceptions += 1
                                        return 'get_full_info'
                                else:
                                        return 'finish'
                        elif not resp.movement:
                                self.surface_clean_perceptions = 0
                                return 'get_info'
                        else:
                                self.surface_clean_perceptions = 0
                                userdata.poses = resp.poses
                                userdata.plane_coefficients = resp.plane_coefficients
                                userdata.poses_2d_coordinates = resp.poses_2d_coordinates
                                userdata.secondary_arm = resp.secondary_arm
                                return 'next'
                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'failed'



class MoveArm(smach.State):
        def __init__(self):
                smach.State.__init__(self, outcomes=['next','failed'],
                                        input_keys=['poses','plane_coefficients','poses_2d_coordinates','secondary_arm'])

        def execute(self, userdata):
                rospy.loginfo('Waiting for service')
                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, 1)
                        if resp.success:
                                print 'Executing service success'
                                return 'next'
                        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.'
                                return 'next'
                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'failed'


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

        def 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('smach_example_state_machine')

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

        # Open the container
        with sm:
                # Add states to the container
                smach.StateMachine.add('NeedInfo', NeedInfo(),
                                transitions={'next':'GetInfoAction', 
                                                'failed':'failedVision'},)
                                                
                smach.StateMachine.add('GetInfoAction', 
                                #SimpleActionState('/iri_clean_perception/get_board_information/get_board_info',
                                #                 GetBoardInfoAction,
                                #                 result_slots=['segmented_image', 
                                #                               'pointcloud',
                                #                               'plane_coefficients']),
                                                  #server_wait_timeout=rospy.Duration(10.0)),
                                ServiceState('/iri_clean_perception/get_board_information/get_board_info_srv',
                                                        BoardInfo,
                                                        response_slots=['segmented_image', 
                                                                        'pointcloud',
                                                                        'plane_coefficients']),
                                transitions={'succeeded':'Planning',
                                                'preempted':'failedVision',
                                                'aborted':'failedVision'},
                                remapping={'segmented_image':'image_segm',
                                                'pointcloud':'pointcloud_segm',
                                                'plane_coefficients':'plane_coefficients'})
                                                
                smach.StateMachine.add('Planning', Planning(), 
                                transitions={'next':'MoveArm', 
                                                'finish':'finishOk', 
                                                'get_full_info':'MoveArmBase',
                                                'get_info':'NeedInfo', 
                                                'failed':'failedPlanning'})
                smach.StateMachine.add('MoveArm', MoveArm(), 
                                transitions={'next':'NeedInfo', 
                                                'failed':'failedMoveArm'})
                smach.StateMachine.add('MoveArmBase', MoveArmBase(), 
                                transitions={'next':'NeedInfo', 
                                                'failed':'failedMoveArm'})

        # Execute SMACH plan
        outcome = sm.execute()



if __name__ == '__main__':
        start=time.time()
        main()
        finish=time.time()
        total_time = finish-start
        rospy.loginfo('Time taken: ' + str(total_time))
        print total_time