00001
00002
00003 import roslib;
00004 roslib.load_manifest('zyonz_apps')
00005 import rospy
00006 import smach
00007 import smach_ros
00008 import tf
00009 from pprint import pprint
00010 import copy
00011
00012
00013 import os, sys
00014 _root_dir = os.path.dirname(os.path.dirname(os.path.realpath(__file__)))
00015 sys.path.insert(0, _root_dir)
00016
00017 from zyonz_common import *
00018 from smach import StateMachine, CBState, Iterator
00019 from smach_ros import ServiceState
00020 from actionlib import *
00021 from actionlib.msg import *
00022 from std_msgs.msg import String, Int8, Int32
00023 from sensor_msgs.msg import PointCloud2, JointState
00024 from geometry_msgs.msg import Transform, PoseStamped, Pose
00025 from zyonz_msgs.msg import chlorophyll_sampleAction, chlorophyll_sampleGoal
00026 from zyonz_msgs.srv import GetProbingSteps
00027 from iri_filter_jump_edge.srv import ApplyPointCloudFilter, ApplyPointCloudFilterRequest
00028 from zyonz_obtain_two_clustered_leaves.srv import ExtractClusters, ExtractClustersRequest
00029 from zyonz_obtain_roi_jump_edge_based.srv import ExtractTwoLeavesRoi, ExtractTwoLeavesRoiRequest
00030 from zyonz_nbv_geometric_simple.srv import GetNBVPose, GetNBVPoseRequest
00031 from smach_ros import ServiceState
00032
00033 class ObtainNBV(smach.State):
00034 def __init__(self):
00035 smach.State.__init__(self, outcomes = ['success','fail'],
00036 input_keys = ['viewpoint_pose', 'roi_pose'],
00037 output_keys = ['nbv_pose'])
00038 def execute(self, userdata):
00039 handler = rospy.ServiceProxy('/zyonz/zyonz_nbv_geometric_simple/get_nbv', GetNBVPose)
00040
00041 analysis_request = GetNBVPoseRequest()
00042 analysis_request.viewpoint = userdata.viewpoint_pose
00043 analysis_request.viewpoint_roi = userdata.roi_pose
00044
00045 try:
00046 response = handler(analysis_request)
00047 except rospy.ServiceException, e:
00048 return 'fail'
00049
00050 if (not response):
00051 return 'fail'
00052
00053 userdata.nbv_pose = response.nbv
00054 pprint(response.nbv)
00055
00056 return 'success'
00057
00058 class ObtainRoi(smach.State):
00059 def __init__(self):
00060 smach.State.__init__(self, outcomes = ['success','fail'],
00061 input_keys = ['residual_pc', 'cluster_a_img', 'cluster_b_img', 'residual_img'],
00062 output_keys = ['roi_pc', 'roi_img', 'roi_pose', 'viewpoint_pose'])
00063 def execute(self, userdata):
00064 handler = rospy.ServiceProxy('/zyonz/zyonz_obtain_roi_jump_edge_based/extract_two_leaves_roi', ExtractTwoLeavesRoi)
00065
00066 analysis_request = ExtractTwoLeavesRoiRequest()
00067 analysis_request.residual_pc = userdata.residual_pc
00068 analysis_request.cluster_a_img = userdata.cluster_a_img
00069 analysis_request.cluster_b_img = userdata.cluster_b_img
00070 analysis_request.residual_img = userdata.residual_img
00071
00072 try:
00073 response = handler(analysis_request)
00074 except rospy.ServiceException, e:
00075 return 'fail'
00076
00077 if (not response):
00078 return 'fail'
00079
00080 userdata.roi_pc = response.roi_pc
00081 userdata.roi_img = response.roi_img
00082 userdata.roi_pose = response.roi_pose
00083 userdata.viewpoint_pose = response.viewpoint_pose
00084
00085 return 'success'
00086
00087 class ObtainTwoClusters(smach.State):
00088 def __init__(self):
00089 smach.State.__init__(self, outcomes = ['success','fail'],
00090 input_keys = ['input'],
00091 output_keys = ['cluster_a', 'cluster_b', 'cluster_a_img', 'cluster_b_img'])
00092 def execute(self, userdata):
00093 handler = rospy.ServiceProxy('/zyonz/zyonz_obtain_two_clustered_leaves/extract_clusters', ExtractClusters)
00094
00095 analysis_request = ExtractClustersRequest()
00096 analysis_request.input = userdata.input
00097
00098 try:
00099 response = handler(analysis_request)
00100 except rospy.ServiceException, e:
00101 return 'fail'
00102
00103 if (not response):
00104 return 'fail'
00105
00106 userdata.cluster_a = response.cluster_a
00107 userdata.cluster_b = response.cluster_b
00108 userdata.cluster_a_img = response.cluster_a_img
00109 userdata.cluster_b_img = response.cluster_b_img
00110
00111 return 'success'
00112
00113 class ApplyFilterJumpEdge(smach.State):
00114 def __init__(self):
00115 smach.State.__init__(self, outcomes = ['success','fail'],
00116 input_keys = ['input_pc'],
00117 output_keys = ['flt_pc', 'res_pc', 'res_img'])
00118 def execute(self, userdata):
00119 handler = rospy.ServiceProxy('/zyonz/iri_filter_jump_edge/apply_jump_edge_filter', ApplyPointCloudFilter)
00120
00121 analysis_request = ApplyPointCloudFilterRequest()
00122 analysis_request.input_pc = userdata.input_pc
00123
00124 try:
00125 response = handler(analysis_request)
00126 except rospy.ServiceException, e:
00127 return 'fail'
00128
00129 if (not response):
00130 return 'fail'
00131
00132 userdata.flt_pc = response.filtered_pc
00133 userdata.res_pc = response.residual_pc
00134 userdata.res_img = response.residual_img
00135
00136 return 'success'
00137
00138 class FindProbingPoint(smach.State):
00139 def __init__(self):
00140 smach.State.__init__(self, outcomes = ['success','fail'],
00141 input_keys = ['pcl'],
00142 output_keys = ['probing_steps'])
00143 def execute(self, userdata):
00144 handler = rospy.ServiceProxy('/zyonz/skills/geometric_based_leaf_probing/leaf_probing_srv', GetProbingSteps)
00145
00146 try:
00147 response = handler(userdata.pcl)
00148 except rospy.ServiceException, e:
00149 return 'fail'
00150
00151 if (not response):
00152 return 'fail'
00153
00154 pub = rospy.Publisher('/debug/probing/grasp', PoseStamped, None, False, True)
00155 pub.publish(response.steps[0].probing)
00156
00157
00158 pre_pose_st = PoseStamped()
00159 pre_pose_st.header = response.steps[0].pre_probing.header
00160 pre_pose_st.pose = response.steps[0].pre_probing.poses[0]
00161 pub = rospy.Publisher('/debug/probing/pre_grasp', PoseStamped, None, False, True)
00162 pub.publish(pre_pose_st)
00163
00164
00165 userdata.probing_steps = response
00166
00167
00168 return 'success'
00169
00170 class TransformProbingPose(smach.State):
00171 def __init__(self):
00172 smach.State.__init__(self, outcomes = ['success','fail'],
00173 input_keys = ['pose_st','pre_pose_st'],
00174 output_keys = ['joints_to_position','pre_joints'])
00175
00176 def execute(self, userdata):
00177 try:
00178 handler = rospy.ServiceProxy('/zyonz/iri_wam_spad_ik/get_wam_ik',
00179 wamInverseKinematics)
00180 response = handler(userdata.pose_st)
00181
00182 if (response):
00183 userdata.joints_to_position = response.joints
00184
00185 response = handler(userdata.pre_pose_st)
00186
00187 if (response):
00188 userdata.pre_joints = response.joints
00189
00190 return 'success'
00191 except rospy.ServiceException, e:
00192 return 'fail'
00193
00194 return 'fail'
00195
00196
00197 class MoveApproachingPose(smach.State):
00198 def __init__(self):
00199 smach.State.__init__(self, outcomes = ['success','fail'],
00200 input_keys = ['pre_joints'])
00201
00202 def execute(self, userdata):
00203 return wam_joints_move(userdata.pre_joints)
00204
00205
00206 class MovePosition(smach.State):
00207 def __init__(self):
00208 smach.State.__init__(self, outcomes = ['success','fail'],
00209 input_keys = ['joints_to_position'])
00210
00211 def execute(self, userdata):
00212 return wam_joints_move(userdata.joints_to_position)
00213
00214 class SM_ZYONZ_ObtainTwoClusters():
00215 def __init__(self):
00216 self.sm = smach.StateMachine(outcomes = ['success','aborted'],
00217 input_keys = ['pcl_topic',
00218 'second_pcl'],
00219 output_keys = ['cluster_a',
00220 'cluster_b',
00221 'cluster_a_img',
00222 'cluster_b_img',
00223 'sm_res_pc',
00224 'sm_res_img'])
00225
00226 def build_sm(self):
00227 with self.sm:
00228
00229 smach.StateMachine.add('CAPTURE_PCL_01', GetPCL(),
00230 transitions = {'success' : 'APPLY_FILTER_JUMP_EDGE',
00231 'fail' : 'aborted'},
00232 remapping = {'pcl_topic' : 'pcl_topic',
00233 'pcl_RGB' : 'sm_pcl'})
00234
00235 smach.StateMachine.add('APPLY_FILTER_JUMP_EDGE', ApplyFilterJumpEdge(),
00236 transitions = {'success' : 'CAPTURE_PCL_02',
00237 'fail' : 'aborted'},
00238 remapping = {'input_pc' : 'sm_pcl' ,
00239 'flt_pc' : 'sm_flt_pc',
00240 'res_pc' : 'sm_res_pc',
00241 'res_img' : 'sm_res_img' })
00242
00243 smach.StateMachine.add('CAPTURE_PCL_02', GetPCL(),
00244 transitions = {'success' : 'OBTAIN_TWO_CLUSTERS',
00245 'fail' : 'aborted'},
00246 remapping = {'pcl_topic' : 'second_pcl',
00247 'pcl_RGB' : 'sm_pcl_02'})
00248
00249 smach.StateMachine.add('OBTAIN_TWO_CLUSTERS', ObtainTwoClusters(),
00250 transitions = {'success' : 'success',
00251 'fail' : 'aborted'},
00252 remapping = {'input' : 'sm_pcl_02' ,
00253 'cluster_a' : 'cluster_a',
00254 'cluster_b' : 'cluster_b',
00255 'cluster_a_img' : 'cluster_a_img',
00256 'cluster_b_img' : 'cluster_b_img' })
00257
00258 return self.sm
00259
00260
00261
00262 def construct_main_sm():
00263 sm = StateMachine(outcomes = ['succeeded','aborted'])
00264
00265 factory = ZyonzProbingSMFactory()
00266 sm_probing = factory.build_sm()
00267 move_factory = SM_WAM_MoveFromPose()
00268 sm_move_spad = move_factory.build_sm()
00269 move_factory = SM_WAM_MoveFromPose("/zyonz/iri_wam_camboard_ik/get_wam_ik")
00270 sm_move_camboard = move_factory.build_sm()
00271 obtain_factory = SM_ZYONZ_ObtainTwoClusters()
00272 sm_obtain_clusters = obtain_factory.build_sm()
00273
00274 sm.userdata.sm_ini_pose = PoseStamped();
00275 sm.userdata.sm_ini_pose.header.frame_id = "wam_link0"
00276 sm.userdata.sm_ini_pose.pose.position.x = 0.65
00277 sm.userdata.sm_ini_pose.pose.position.y = 0.00
00278 sm.userdata.sm_ini_pose.pose.position.z = 0.10
00279 sm.userdata.sm_ini_pose.pose.orientation.x = -0.7071
00280 sm.userdata.sm_ini_pose.pose.orientation.y = 0.7071
00281 sm.userdata.sm_ini_pose.pose.orientation.z = 0.0
00282 sm.userdata.sm_ini_pose.pose.orientation.w = 0.0
00283
00284 sm.userdata.sm_pcl_topic = "/zyonz/average_point_cloud_node/output"
00285 sm.userdata.sm_second_pcl = "/zyonz/outlier_removal/output"
00286
00287
00288 with sm:
00289 smach.StateMachine.add('GO_HOME', ZyonzGoHome(),
00290 transitions={'success': 'MOVE_TO_INI',
00291 'fail' : 'aborted'})
00292
00293 smach.StateMachine.add('MOVE_TO_INI', sm_move_camboard,
00294 transitions={'success' : 'OBTAIN_CLUSTERS_01',
00295 'aborted' : 'aborted'},
00296 remapping = {'pose_st' : 'sm_ini_pose'})
00297
00298 smach.StateMachine.add('OBTAIN_CLUSTERS_01', sm_obtain_clusters,
00299 transitions = {'success' : 'OBTAIN_ROI',
00300 'aborted' : 'aborted'},
00301 remapping = {'pcl_topic' : 'sm_pcl_topic',
00302 'second_pcl' : 'sm_second_pcl',
00303 'pcl_RGB' : 'sm_pcl',
00304 'cluster_a' : 'sm_cluster_a',
00305 'cluster_b' : 'sm_cluster_b',
00306 'cluster_a_img' : 'sm_cluster_a_img',
00307 'cluster_b_img' : 'sm_cluster_b_img' })
00308
00309 smach.StateMachine.add('OBTAIN_ROI', ObtainRoi(),
00310 transitions = {'success' : 'FIND_NBV',
00311 'fail' : 'aborted'},
00312 remapping = {'residual_pc' : 'sm_res_pc' ,
00313 'cluster_a_img' : 'sm_cluster_a_img',
00314 'cluster_b_img' : 'sm_cluster_b_img',
00315 'residual_img' : 'sm_res_img',
00316 'roi_pc' : 'sm_roi_pc',
00317 'roi_img' : 'sm_roi_img',
00318 'roi_pose' : 'sm_roi_pose',
00319 'viewpoint_pose' : 'sm_viewpoint_pose' })
00320
00321 smach.StateMachine.add('FIND_NBV', ObtainNBV(),
00322 transitions = {'success' : 'MOVE_TO_NBV',
00323 'fail' : 'aborted'},
00324 remapping = {'viewpoint_pose' : 'sm_viewpoint_pose',
00325 'roi_pose' : 'sm_roi_pose',
00326 'nbv_pose' : 'sm_nbv_pose' })
00327
00328 smach.StateMachine.add('MOVE_TO_NBV', sm_move_camboard,
00329 transitions={'success': 'OBTAIN_CLUSTERS_02',
00330 'aborted' : 'aborted'},
00331 remapping = {'pose_st' : 'sm_nbv_pose'})
00332
00333 smach.StateMachine.add('OBTAIN_CLUSTERS_02', sm_obtain_clusters,
00334 transitions = {'success' : 'MOVE_TO_INI_02',
00335 'aborted' : 'aborted'},
00336 remapping = {'pcl_topic' : 'sm_pcl_topic',
00337 'second_pcl' : 'sm_second_pcl',
00338 'pcl_RGB' : 'sm_pcl',
00339 'cluster_a' : 'sm_cluster_a',
00340 'cluster_b' : 'sm_cluster_b',
00341 'cluster_a_img' : 'sm_cluster_a_img',
00342 'cluster_b_img' : 'sm_cluster_b_img' })
00343
00344 smach.StateMachine.add('MOVE_TO_INI_02', sm_move_camboard,
00345 transitions={'success' : 'OBTAIN_CLUSTERS_03',
00346 'aborted' : 'aborted'},
00347 remapping = {'pose_st' : 'sm_ini_pose'})
00348
00349 smach.StateMachine.add('OBTAIN_CLUSTERS_03', sm_obtain_clusters,
00350 transitions = {'success' : 'FIND_PROBING_POINT',
00351 'aborted' : 'aborted'},
00352 remapping = {'pcl_topic' : 'sm_pcl_topic',
00353 'second_pcl' : 'sm_second_pcl',
00354 'pcl_RGB' : 'sm_pcl',
00355 'cluster_a' : 'sm_cluster_a',
00356 'cluster_b' : 'sm_cluster_b',
00357 'cluster_a_img' : 'sm_cluster_a_img',
00358 'cluster_b_img' : 'sm_cluster_b_img' })
00359
00360 smach.StateMachine.add('FIND_PROBING_POINT', FindProbingPoint(),
00361 transitions = {'success' : 'PROBING_ACTION',
00362 'fail' : 'OBTAIN_CLUSTERS_03'},
00363 remapping = {'pcl' : 'sm_cluster_a' ,
00364 'probing_steps' : 'sm_probing_msg' })
00365
00366 smach.StateMachine.add('PROBING_ACTION', sm_probing,
00367 transitions = {'succeeded' : 'GO_END'},
00368 remapping = {'probing_steps' : 'sm_probing_msg'})
00369
00370
00371 smach.StateMachine.add('GO_END', ZyonzGoHome(),
00372 transitions={'success': 'succeeded',
00373 'fail' : 'aborted'})
00374
00375 return sm
00376
00377 def main():
00378 rospy.init_node("zyonz_geometric_based_nbv")
00379
00380
00381 sm_main = construct_main_sm()
00382
00383
00384 sis = smach_ros.IntrospectionServer('zyonz_smach_base_probing', sm_main,
00385 '/zyonz_geometric_based_nbv_leaf_probing')
00386 sis.start()
00387
00388
00389 outcome = sm_main.execute()
00390
00391 rospy.spin()
00392 sis.stop()
00393
00394 if __name__ == "__main__":
00395 main()