sm_cleaning.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

# 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.logdebug('Executing state NeedInfo')
                return 'next'


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

        def execute(self, userdata):
                if userdata.pointcloud_segm.height == 0:
                        rospy.loginfo('Pointcloud isn\'t available yet')
                        return 'get_info'
                
                rospy.logdebug('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.segmented_image_sm, userdata.plane_coefficients)
                        if resp.is_clean:
                                return 'finish'
                        elif not resp.movement:
                                return 'get_info'
                        else:
                                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 ComputeStateFromImage(smach.State):
        def __init__(self):
                smach.State.__init__(self, outcomes=['succeeded','preempted','aborted','is_clean'],
                                     input_keys=['segmented_image_sm','segmented_pointcloud_sm','plane_coefficients_sm'],
                                     output_keys=['ellipses','distances','max_widths','min_widths','sparse','num_objects','arm_position','segmented_pointcloud_sm','plane_coefficients_sm'])
                self.service_topic = '/iri_clean_plan/cleaning_plan/get_state_representation'
        
        def execute(self, userdata):
                rospy.logdebug('Executing state ComputeStateFromImage')
                rospy.wait_for_service(self.service_topic)
                try:
                        get_state = rospy.ServiceProxy(self.service_topic, EllipsesStateRepresentation)
                        resp = get_state(userdata.segmented_image_sm)
                        if resp.is_clean:
                                return 'is_clean'
                        else:
                                userdata.ellipses = resp.ellipses
                                userdata.distances = resp.distances
                                userdata.max_widths = resp.max_widths
                                userdata.min_widths = resp.min_widths
                                userdata.sparse = resp.sparse
                                userdata.num_objects = resp.num_objects
                                userdata.arm_position = resp.arm_position
                                
                                return 'succeeded'
                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'aborted'


class PradaPlanning(smach.State):
        def __init__(self):
                smach.State.__init__(self, outcomes=['succeeded','preempted','aborted','no_plan'],
                                     input_keys=['distances','max_widths','min_widths','sparse','num_objects','arm_position','ellipses','segmented_pointcloud_sm','plane_coefficients_sm'],
                                     output_keys=['target_ellipses','actions','ellipses','segmented_pointcloud_sm','plane_coefficients_sm'])
                self.service_topic = '/iri_clean_plan/prada_planner/getBestAction'
        
        def 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(userdata.distances, userdata.max_widths, userdata.min_widths, userdata.sparse, userdata.num_objects, userdata.arm_position)
                        
                        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(smach.State):
        def __init__(self):
                smach.State.__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','plane_coefficients','poses_2d_coordinates','secondary_arm'])
                self.service_topic = '/iri_clean_plan/cleaning_plan/get_trajectories_from_plan'
        
        def execute(self, userdata):
                rospy.logdebug('Executing state PradaPlanning')
                rospy.wait_for_service(self.service_topic)
                try:
                        get_cleaning_plan = rospy.ServiceProxy(self.service_topic, PlanToTrajectories)
                        resp = get_cleaning_plan(userdata.segmented_pointcloud_sm, userdata.plane_coefficients_sm, userdata.target_ellipses_sm, userdata.actions_sm)
                        
                        if resp.movement:
                                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 'succeeded'
                        else:
                                return 'no_movements'
                
                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'aborted'


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.logdebug('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 calculate_trajectories_cb(userdata, request):
        #traj_request = PlanToTrajectories().Request
        request.pointcloud_in = userdata.segmented_pointcloud_sm
        request.plane_coefficients = userdata.plane_coefficients_sm
        request.target_ellipses = userdata.target_ellipses_sm
        request.actions = userdata.actions_sm
        return request


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':'ComputeStateFromImage',
                                                'preempted':'failedVision',
                                                'aborted':'failedVision'},
                                remapping={'segmented_image':'segmented_image_sm',
                                                'pointcloud':'segmented_pointcloud_sm',
                                                'plane_coefficients':'plane_coefficients_sm'})
                
                smach.StateMachine.add('ComputeStateFromImage', ComputeStateFromImage(),
                                transitions={'succeeded':'PradaPlanning',
                                                'preempted':'failedVision',
                                                'aborted':'failedVision',
                                                'is_clean':'MoveArmBase'},
                                remapping={'ellipses':'ellipses_sm'})
                
                smach.StateMachine.add('PradaPlanning',
                                #ServiceState('/iri_clean_plan/prada_planner/getBestAction',
                                                #PradaPlanSrv,
                                                #input_keys=['segmented_pointcloud_sm'],
                                                #output_keys=['segmented_pointcloud_sm'],
                                                #request_slots=['distances','max_widths','min_widths','sparse','num_objects','arm_position'],
                                                #response_slots=['succeeded','target_ellipses','actions']),
                                PradaPlanning(),
                                transitions={'succeeded':'CalculateTrajectories',
                                                'preempted':'failedVision',
                                                'aborted':'failedVision',
                                                'no_plan':'NeedInfo'},
                                remapping={'target_ellipses':'target_ellipses_sm',
                                           'actions':'actions_sm'})
                
                smach.StateMachine.add('CalculateTrajectories', 
                                CalculateTrajectories(),
                                #ServiceState('/iri_clean_plan/cleaning_plan/get_trajectories_from_plan',
                                                #PlanToTrajectories,
                                                ##request_slots=['segmented_pointcloud_sm','plane_coefficients_sm','target_ellipses_sm','actions_sm'],
                                                #request_cb = calculate_trajectories_cb,
                                                #input_keys=['segmented_pointcloud_sm','plane_coefficients_sm','target_ellipses_sm','actions_sm']),
                                                ##response_slots=['poses','plane_coefficients','poses_2d_coordinates','secondary_arm']),
                                transitions={'succeeded':'MoveArm',
                                                'preempted':'failedVision',
                                                'aborted':'failedVision',
                                                'no_movements':'NeedInfo'})
                
                smach.StateMachine.add('Planning', Planning(), 
                                transitions={'next':'MoveArm', 
                                                #'finish':'finishOk', 
                                                'finish':'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__':
        main()