Mastering_ROS_for_Robotics_Programming 配套代码

《Mastering ROS for Robotics Programming》一书的代码。

#!/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个文件信息