#!/usr/bin/python
#?Software?License?Agreement?（BSD?License）
#
#?Copyright?（c）?2014?PAL?Robotics?SL
#?All?rights?reserved.
#
#?Redistribution?and?use?in?source?and?binary?forms?with?or?without
#?modification?are?permitted?provided?that?the?following?conditions
#?are?met:
#
#??*?Redistributions?of?source?code?must?retain?the?above?copyright
#????notice?this?list?of?conditions?and?the?following?disclaimer.
#??*?Redistributions?in?binary?form?must?reproduce?the?above
#????copyright?notice?this?list?of?conditions?and?the?following
#????disclaimer?in?the?documentation?and/or?other?materials?provided
#????with?the?distribution.
#??*?Neither?the?name?of?the?PAL?Robotics?nor?the?names?of?its
#????contributors?may?be?used?to?endorse?or?promote?products?derived
#????from?this?software?without?specific?prior?written?permission.
#
#?THIS?SOFTWARE?IS?PROVIDED?BY?THE?COPYRIGHT?HOLDERS?AND?CONTRIBUTORS
#?“AS?IS“?AND?ANY?EXPRESS?OR?IMPLIED?WARRANTIES?INCLUDING?BUT?NOT
#?LIMITED?TO?THE?IMPLIED?WARRANTIES?OF?MERCHANTABILITY?AND?FITNESS
#?FOR?A?PARTICULAR?PURPOSE?ARE?DISCLAIMED.?IN?NO?EVENT?SHALL?THE
#?COPYRIGHT?OWNER?OR?CONTRIBUTORS?BE?LIABLE?FOR?ANY?DIRECT?INDIRECT
#?INCIDENTAL?SPECIAL?EXEMPLARY?OR?CONSEQUENTIAL?DAMAGES?（INCLUDING
#?BUT?NOT?LIMITED?TO?PROCUREMENT?OF?SUBSTITUTE?GOODS?OR?SERVICES;
#?LOSS?OF?USE?DATA?OR?PROFITS;?OR?BUSINESS?INTERRUPTION）?HOWEVER
#?CAUSED?AND?ON?ANY?THEORY?OF?LIABILITY?WHETHER?IN?CONTRACT?STRICT
#?LIABILITY?OR?TORT?（INCLUDING?NEGLIGENCE?OR?OTHERWISE）?ARISING?IN
#?ANY?WAY?OUT?OF?THE?USE?OF?THIS?SOFTWARE?EVEN?IF?ADVISED?OF?THE
#?POSSIBILITY?OF?SUCH?DAMAGE.
#
#?author:?Bence?Magyar

import?rospy
from?actionlib?import?SimpleActionClient
from?geometry_msgs.msg?import?PoseArray?Pose?Point
from?std_msgs.msg?import?Header
from?moveit_simple_grasps.msg?import?GenerateGraspsAction?GenerateGraspsGoal
from?moveit_msgs.msg?import?Grasp
import?geometry_msgs

grasp_publisher?=?None
grasps_ac?=?None

def?generate_grasps（pose?width）:
	#send?request?to?block?grasp?generator?service
	grasps_ac.wait_for_server（）
	rospy.loginfo（“Successfully?connected.“）
	goal?=?GenerateGraspsGoal（）
	goal.pose?=?pose.pose
	goal.width?=?width
	grasps_ac.send_goal（goal）
	rospy.loginfo（“Sent?goal?waiting:\n“?+?str（goal））
	t_start?=?rospy.Time.now（）
	grasps_ac.wait_for_result（）
	t_end?=?rospy.Time.now（）
	t_total?=?t_end?-?t_start
	rospy.loginfo（“Result?received?in?“?+?str（t_total.to_sec（）））
	grasp_list?=?grasps_ac.get_result（）.grasps
	return?grasp_list

def?publish_grasps_as_poses（grasps）:
	rospy.loginfo（“Publishing?PoseArray?on?/grasp_pose_from_block_bla?for?grasp_pose“）
	graspmsg?=?Grasp（）
	grasp_PA?=?PoseArray（）
	header?=?Header（）
	header.frame_id?=?“base_link“
	header.stamp?=?rospy.Time.now（）
	grasp_PA.header?=?header
	for?graspmsg?in?grasps:
		p?=?Pose（graspmsg.grasp_pose.pose.position?graspmsg.grasp_pose.pose.orientation）
		grasp_PA.poses.append（p）
	grasp_publisher.publish（grasp_PA）
	rospy.loginfo（‘Published?‘?+?str（len（grasp_PA.poses））?+?‘?p

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2015-11-02?20:11??chapter_10_codes\
?????目錄???????????0??2015-11-02?20:11??chapter_10_codes\cool_arm\
?????目錄???????????0??2015-11-02?20:11??chapter_10_codes\cool_arm\cool5000_description\
?????目錄???????????0??2015-11-02?20:11??chapter_10_codes\cool_arm\cool5000_description\launch\
?????文件?????????610??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\launch\display.launch
?????文件?????????672??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\launch\gazebo.launch
?????文件?????????181??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\manifest.xml
?????目錄???????????0??2015-11-02?20:11??chapter_10_codes\cool_arm\cool5000_description\meshes\
?????文件???????79284??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\base_link.STL
?????文件???????98584??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\index_link.STL
?????文件???????67484??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\index_tip_link.STL
?????文件???????21484??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\link_1.STL
?????文件??????235984??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\link_2.STL
?????文件???????28984??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\link_3.STL
?????文件??????209384??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\link_4.STL
?????文件???????12784??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\link_5.STL
?????文件??????209384??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\link_6.STL
?????文件??????216584??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\link_7.STL
?????文件???????98584??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\middle_link.STL
?????文件???????67484??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\middle_tip_link.STL
?????文件??????133784??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\thumb_base_link.STL
?????文件???????98584??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\thumb_link.STL
?????文件???????67484??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\meshes\thumb_tip_link.STL
?????目錄???????????0??2015-11-02?20:11??chapter_10_codes\cool_arm\cool5000_description\robots\
?????文件???????21509??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\robots\cool5000_description.URDF
?????目錄???????????0??2015-10-20?18:27??chapter_10_codes\cool_arm\cool5000_description\textures\
?????目錄???????????0??2015-11-02?20:11??chapter_10_codes\cool_arm\cool5000_moveit_config\
?????文件?????????230??2015-10-21?15:09??chapter_10_codes\cool_arm\cool5000_moveit_config\.setup_assistant
?????文件?????????268??2015-10-21?15:09??chapter_10_codes\cool_arm\cool5000_moveit_config\CMakeLists.txt
?????目錄???????????0??2015-11-02?20:11??chapter_10_codes\cool_arm\cool5000_moveit_config\config\
?????文件???????11118??2015-10-21?15:09??chapter_10_codes\cool_arm\cool5000_moveit_config\config\cool5000_description.srdf
............此處省略2043個文件信息