Skip to content

Commit f044159

Browse files
committed
Commneted action points for manipulation pipeline.
1 parent 7209f99 commit f044159

File tree

2 files changed

+98
-66
lines changed

2 files changed

+98
-66
lines changed

scripts/Controller.py renamed to scripts/controller.py

Lines changed: 12 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -38,14 +38,14 @@ def __init__(self):
3838
self.IK_ORIENTATION_TOLERANCE = np.pi/9
3939
self.MIN_CLOSING_GAP = 0.002
4040

41-
def set_camera_angles(self, pan_rad, tilt_rad):
41+
def set_camera_angles(self, angles):
4242
pan_msg = Float64()
43-
pan_msg.data = pan_rad
43+
pan_msg.data = angles[0]
4444
rospy.loginfo('Going to camera pan: {} rad'.format(pan_rad))
4545
self.pan_pub.publish(pan_msg)
4646

4747
tilt_msg = Float64()
48-
tilt_msg.data = tilt_rad
48+
tilt_msg.data = angles[1]
4949
rospy.loginfo('Going to camera tilt: {} rad'.format(tilt_rad))
5050
self.tilt_pub.publish(tilt_msg)
5151
rospy.sleep(4)
@@ -107,17 +107,16 @@ def compute_ik(self, target_pose):
107107

108108
# Goes to the position given by FRAME and grabs the object from the top
109109
def go_to_pose(self, FRAME):
110-
while not rospy.is_shutdown():
111-
try:
112-
self.tf_listener.waitForTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time.now(), rospy.Duration(5.0))
113-
P, Q = self.tf_listener.lookupTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time(0))
114-
break
115-
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
116-
print(e)
117-
110+
try:
111+
self.tf_listener.waitForTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time.now(), rospy.Duration(5.0))
112+
P, Q = self.tf_listener.lookupTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time(0))
113+
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
114+
rospy.logwarn(e)
115+
return False
116+
118117
O = tf.transformations.euler_from_quaternion(Q)
119118
Q = tf.transformations.quaternion_from_euler(0, np.pi/2, O[2])
120-
119+
121120
poses = [Pose(Point(P[0], P[1], P[2]+0.20), Quaternion(Q[0], Q[1], Q[2], Q[3])),
122121
Pose(Point(P[0], P[1], P[2]+0.15), Quaternion(Q[0], Q[1], Q[2], Q[3]))]
123122

@@ -135,7 +134,7 @@ def go_to_pose(self, FRAME):
135134
rospy.sleep(8)
136135
else:
137136
return False
138-
self.close_gripper()
137+
return True
139138

140139
# checks if the object was grasped or not
141140
def check_grasp(self):

scripts/machine.py

Lines changed: 86 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -5,13 +5,19 @@
55
import smach
66
import smach_ros
77

8+
from controller import Controller
9+
10+
# Initialize controller
11+
ctrl = Controller()
812
# define state Idle
913
class Idle(smach.State):
1014
def __init__(self):
1115
smach.State.__init__(self, outcomes=['gotToolInput'], input_keys=['tool_id'], output_keys=['tool_id'])
1216

1317
def execute(self, userdata):
1418
userdata.tool_id = -1
19+
ctrl.set_camera_angles(ctrl.HOME_POS_CAMERA_01)
20+
ctrl.set_arm_joint_angles(ctrl.HOME_POS_MANIPULATOR_01)
1521
rospy.loginfo('Executing state IDLE')
1622
while(True):
1723
# TODO: Ask for Input
@@ -21,149 +27,176 @@ def execute(self, userdata):
2127

2228
# Return success
2329
return 'gotToolInput'
24-
30+
2531

2632

2733
# define state FindTool
2834
class FindTool(smach.State):
2935
def __init__(self):
30-
smach.State.__init__(self, outcomes=['foundTool'], input_keys=['tool_id'])
36+
smach.State.__init__(self, outcomes=['foundTool'], input_keys=['tool_id'], output_keys=['frame_name'])
3137

3238
def execute(self, userdata):
3339
rospy.loginfo('Executing state FINDTOOL')
3440
print(userdata.tool_id)
41+
## TODO: change the name of the variable frame_name
42+
userdata.frame_name = "random"
3543
return 'foundTool'
36-
44+
3745
# define state IK1
3846
class IK1(smach.State):
3947
def __init__(self):
40-
smach.State.__init__(self, outcomes=['noIK','foundIK'])
48+
smach.State.__init__(self, outcomes=['noIK','foundIK'], input_keys=['frame_name'])
4149

4250
def execute(self, userdata):
51+
success = ctrl.go_to_pose(userdata.frame_name)
4352
rospy.loginfo('Executing state IK1')
44-
return 'foundIK'
53+
return 'foundIK' if success else 'noIK'
4554

4655
# define state Move
47-
class Move(smach.State):
48-
def __init__(self):
49-
smach.State.__init__(self, outcomes=['reached'])
56+
# class Move(smach.State):
57+
# def __init__(self):
58+
# smach.State.__init__(self, outcomes=['reached'])
5059

51-
def execute(self, userdata):
52-
rospy.loginfo('Executing state IK1')
53-
return 'reached'
60+
# def execute(self, userdata):
61+
# rospy.loginfo('Executing state IK1')
62+
# return 'reached'
5463

5564
# define state Grab
5665
class Grab(smach.State):
5766
def __init__(self):
5867
smach.State.__init__(self, outcomes=['grabSuccess','grabFailure'])
5968

6069
def execute(self, userdata):
70+
ctrl.close_gripper()
6171
rospy.loginfo('Executing state IK1')
62-
return 'grabSuccess'
72+
if ctrl.check_grasp():
73+
return 'grabSuccess'
74+
else:
75+
return 'grabFailure'
6376

6477
# define state MoveHome2
6578
class MoveHome2(smach.State):
6679
def __init__(self):
6780
smach.State.__init__(self, outcomes=['reached'])
6881

6982
def execute(self, userdata):
83+
ctrl.set_camera_angles(ctrl.HOME_POS_CAMERA_02)
84+
ctrl.set_arm_joint_angles(ctrl.HOME_POS_MANIPULATOR_02)
7085
rospy.loginfo('Executing state MoveHome2')
7186
return 'reached'
7287

7388
# define state OreintCamera
74-
class OreintCamera(smach.State):
75-
def __init__(self):
76-
smach.State.__init__(self, outcomes=['reached'])
89+
# class OrientCamera(smach.State):
90+
# def __init__(self):
91+
# smach.State.__init__(self, outcomes=['reached'])
7792

78-
def execute(self, userdata):
79-
rospy.loginfo('Executing state OreintCamera')
80-
return 'reached'
93+
# def execute(self, userdata):
94+
# rospy.loginfo('Executing state OrientCamera')
95+
# return 'reached'
8196

8297
# define state FindAttention
8398
class FindAttention(smach.State):
99+
84100
def __init__(self):
85-
smach.State.__init__(self, outcomes=['giveTool'])
101+
smach.State.__init__(self, outcomes=['giveTool'], output_keys=['frame_name'])
86102

87103
def execute(self, userdata):
104+
## Call service to get the frame id of the hand centroid
105+
## TODO: change the name of the variable frame_name
106+
userdata.frame_name = "random"
88107
rospy.loginfo('Executing state FindAttention')
89108
return 'giveTool'
90109

91110
# define state IK2
92111
class IK2(smach.State):
93112
def __init__(self):
94-
smach.State.__init__(self, outcomes=['foundIK'])
113+
smach.State.__init__(self, outcomes=['foundIK'], input_keys=['frame_name'])
95114

96115
def execute(self, userdata):
116+
## Wait till IK not found. Change tolerance and call compute IK again
117+
## Break go to pose into find pose and compute IK functions
118+
## Make a different function with gripper pointing upwards and call it from here
119+
success = ctrl.go_to_pose(userdata.frame_name)
120+
# if not success
97121
rospy.loginfo('Executing state IK2')
98122
return 'foundIK'
99123

100-
# define state MoveGive
101-
class MoveGive(smach.State):
102-
def __init__(self):
103-
smach.State.__init__(self, outcomes=['reached'])
124+
# # define state MoveGive
125+
# class MoveGive(smach.State):
126+
# def __init__(self):
127+
# smach.State.__init__(self, outcomes=['reached'])
104128

105-
def execute(self, userdata):
106-
rospy.loginfo('Executing state MoveGive')
107-
return 'reached'
129+
# def execute(self, userdata):
130+
# rospy.loginfo('Executing state MoveGive')
131+
# return 'reached'
108132

109133
# define state ChangePID
110134
class ChangePID(smach.State):
111135
def __init__(self):
112-
smach.State.__init__(self, outcomes=['reached'])
136+
smach.State.__init__(self, outcomes=['changed'])
113137

114138
def execute(self, userdata):
139+
## Service call to change the PID values
115140
rospy.loginfo('Executing state ChangePID')
116-
return 'reached'
141+
return 'changed'
117142

118143
# define state OpenGripper
119144
class OpenGripper(smach.State):
120145
def __init__(self):
121-
smach.State.__init__(self, outcomes=['reached'])
146+
smach.State.__init__(self, outcomes=['opened'])
122147

123148
def execute(self, userdata):
149+
## Detect when to Open gripper
150+
self.open_gripper()
124151
rospy.loginfo('Executing state OpenGripper')
125-
return 'reached'
152+
return 'opened'
126153

127154
def main():
128155
rospy.init_node('attention_bot')
129156

157+
130158
# Create a SMACH state machine
131159
sm = smach.StateMachine(outcomes=['stop'])
132160
sm.userdata.tool_id = -1
133-
161+
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
162+
sis.start()
134163
# Open the container
135164
with sm:
136165
# Add states to the container
137-
smach.StateMachine.add('IDLE', Idle(),
166+
smach.StateMachine.add('IDLE', Idle(),
138167
transitions={'gotToolInput':'FINDTOOL'})
139-
smach.StateMachine.add('FINDTOOL', FindTool(),
168+
smach.StateMachine.add('FINDTOOL', FindTool(),
140169
transitions={'foundTool':'IK1'})
141-
smach.StateMachine.add('IK1', IK1(),
142-
transitions={'noIK':'stop','foundIK':'MOVE'})
143-
smach.StateMachine.add('MOVE', Move(),
144-
transitions={'reached':'GRAB'})
145-
smach.StateMachine.add('GRAB', Grab(),
170+
smach.StateMachine.add('IK1', IK1(),
171+
transitions={'noIK':'stop','foundIK':'GRAB'})
172+
# smach.StateMachine.add('MOVE', Move(),
173+
# transitions={'reached':'GRAB'})
174+
smach.StateMachine.add('GRAB', Grab(),
146175
transitions={'grabSuccess':'MOVEHOME2','grabFailure':'FINDTOOL'})
147-
smach.StateMachine.add('MOVEHOME2', MoveHome2(),
148-
transitions={'reached':'ORIENTCAMERA'})
149-
smach.StateMachine.add('ORIENTCAMERA', OreintCamera(),
176+
smach.StateMachine.add('MOVEHOME2', MoveHome2(),
150177
transitions={'reached':'ATTENTIONSEEKER'})
151-
smach.StateMachine.add('ATTENTIONSEEKER', FindAttention(),
178+
# smach.StateMachine.add('ORIENTCAMERA', OrientCamera(),
179+
# transitions={'reached':'ATTENTIONSEEKER'})
180+
smach.StateMachine.add('ATTENTIONSEEKER', FindAttention(),
152181
transitions={'giveTool':'IK2'})
153-
smach.StateMachine.add('IK2', IK2(),
154-
transitions={'foundIK':'MOVEGIVE'})
155-
smach.StateMachine.add('MOVEGIVE', MoveGive(),
156-
transitions={'reached':'CHANGEPID'})
157-
smach.StateMachine.add('CHANGEPID', ChangePID(),
158-
transitions={'reached':'OPENGRIPPER'})
159-
smach.StateMachine.add('OPENGRIPPER', OpenGripper(),
160-
transitions={'reached':'IDLE'})
161-
182+
smach.StateMachine.add('IK2', IK2(),
183+
transitions={'foundIK':'CHANGEPID'})
184+
# smach.StateMachine.add('MOVEGIVE', MoveGive(),
185+
# transitions={'reached':'CHANGEPID'})
186+
smach.StateMachine.add('CHANGEPID', ChangePID(),
187+
transitions={'changed':'OPENGRIPPER'})
188+
smach.StateMachine.add('OPENGRIPPER', OpenGripper(),
189+
transitions={'opened':'IDLE'})
190+
191+
162192

163193
# Execute SMACH plan
164194
outcome = sm.execute()
195+
sis.stop()
196+
197+
165198

166199

167200

168201
if __name__ == '__main__':
169-
main()
202+
main()

0 commit comments

Comments
 (0)