challenge/smach_challenge.py at master · chillvamp/challenge · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
#!/usr/bin/env python

import rospy
import smach
from hsrb_interface import Robot
from hsrb_interface import collision_world
import hsrb_interface
import rospy
import sys
import math
import tf
import tf2_ros
import tf2_geometry_msgs
from hsrb_interface import geometry
from geometry_msgs.msg import PoseStamped, Point

##################INITIA
robot = Robot()
base = robot.try_get('omni_base')
tts = robot.try_get('default_tts')
whole_body = robot.try_get('whole_body')
gripper=robot.try_get('gripper')


# define state Initial State
class initial_state(smach.State):
    def __init__(self):
        smach.State.__init__(self,outcomes=['outcome1'])

    def execute(self,userdata):
        rospy.loginfo('STATE : initial state')
        whole_body.linear_weight=20
        whole_body.move_to_go()


        base.go_rel(x=-.40, timeout=10)
        base.go_abs(x=0,y=0.3, timeout=10)
        return 'outcome1'


# define state go to hole
class go_to_table(smach.State):
    def __init__(self):
        smach.State.__init__(self,outcomes=['outcome1','outcome2'])
        self.visited=False

    def execute(self,userdata):
        rospy.loginfo('State : go to table')
        whole_body.move_end_effector_pose(geometry.pose(x=-.05,z=.2,ei=3.1416,ek=1.57),'my_frame_hole')
        gripper.set_distance(.8,.5)
        if self.visited:
            return 'outcome2'
        else:

            self.visited = True
            return 'outcome1'


#class reestimate_hole
class reestimate_hole(smach.State):
    def __init__(self):
        smach.State.__init__(self,outcomes=['outcome1','outcome2'])
        self.visited=False

    def execute(self,userdata):
        rospy.loginfo('State : reestimate_hole')
        whole_body.move_end_effector_pose(geometry.pose(z=.2,ei=3.1416,ek=1.57),'chido')
        if self.visited:
            return 'outcome2'
        else:

            self.visited = True

            return 'outcome1'


#class ANCHOR_BODY
class anchor_body(smach.State):
    def __init__(self):
        smach.State.__init__(self,outcomes=['outcome1'])

    def execute(self,userdata):
        rospy.loginfo('State : ANCHOR_BODY')
        whole_body.linear_weight=99
        whole_body.move_to_go()
        return 'outcome1'


class grasp(smach.State):
    def __init__(self):
        smach.State.__init__(self,outcomes=['outcome1'])

    def execute(self,userdata):
        rospy.loginfo('State : GRASP')

        gripper.set_distance(.8,1)
        whole_body.move_end_effector_pose(geometry.pose(x=-.51,y=-.51,z=.2,ei=3.1416,ek=1.57),'my_frame_hole')
        whole_body.move_end_effector_pose(geometry.pose(x=-.51,y=-.51,z=-.2,ei=3.1416,ek=1.57),'my_frame_hole')
        whole_body.move_end_effector_pose(geometry.pose(x=-.51,y=-.51,z=.2,ei=3.1416,ek=1.57),'my_frame_hole')
        whole_body.move_to_neutral()
        base.go_rel(x=-.40, timeout=10)


        return 'outcome1'


def main():
    #rospy.init_node('hsrb_challenge')
    sm=smach.StateMachine(outcomes=['END'])

    with sm:
        smach.StateMachine.add('INITIAL', initial_state(),
                                transitions={'outcome1':'GO_TO_TABLE'})
        smach.StateMachine.add('GO_TO_TABLE',go_to_table(), transitions={'outcome1':'ANCHOR_BODY','outcome2':'REESTIMATE_HOLE_TF'})
        smach.StateMachine.add('REESTIMATE_HOLE_TF',reestimate_hole(),transitions={'outcome1':'REESTIMATE_HOLE_TF','outcome2':'END'})
        smach.StateMachine.add('ANCHOR_BODY',anchor_body(),transitions={'outcome1':'GRASP'})
        smach.StateMachine.add('GRASP',grasp(),transitions={'outcome1':'GO_TO_TABLE'})


    outcome= sm.execute()
if __name__== '__main__':
    main()