本文整理汇总了Python中moveit_commander.PlanningSceneInterface.add_box方法的典型用法代码示例。如果您正苦于以下问题:Python PlanningSceneInterface.add_box方法的具体用法?Python PlanningSceneInterface.add_box怎么用?Python PlanningSceneInterface.add_box使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类moveit_commander.PlanningSceneInterface
的用法示例。
在下文中一共展示了PlanningSceneInterface.add_box方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: main
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
def main():
rospy.init_node('moveit_py_place', anonymous=True)
#right_arm.set_planner_id("KPIECEkConfigDefault");
scene = PlanningSceneInterface()
robot = RobotCommander()
#group = MoveGroupCommander("head")
right_arm = MoveGroupCommander("right_arm")
#right_arm.set_planner_id("KPIECEkConfigDefault");
rospy.logwarn("cleaning world")
GRIPPER_FRAME = 'gripper_bracket_f2'
#scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.67
p.pose.position.y = -0.
p.pose.position.z = 0.75
scene.add_box("part", p, (0.07, 0.01, 0.2))
# move to a random target
#group.set_named_target("ahead")
#group.go()
#rospy.sleep(1)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part")
n_attempts += 1
print "Attempts pickup: ", n_attempts
rospy.sleep(0.2)
#robot.right_arm.pick("part")
#right_arm.go()
rospy.sleep(5)
示例2: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
#.........这里部分代码省略.........
rospy.sleep(2)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
#print target_pose
self.object_frames_pub.publish(target_pose)
rospy.sleep(2)
示例3: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
#.........这里部分代码省略.........
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
def _add_table(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.22
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (0.5, 0.4, 0.02))
return p.pose
def _add_coke_can(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.25
p.pose.position.y = 0
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.01, 0.01, 0.009))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
示例4: PoseStamped
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
rospy.sleep(1)
# create a pose
p = PoseStamped()
p.header.frame_id = "world" #robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.05
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# add a box there
scene.add_box("ground", p, (3, 3, 0.02))
# access some meshes
rospack = rospkg.RosPack()
resourcepath = rospack.get_path('regrasping_app')+"/../resources/"
# modify the pose
p.pose.position.x = 0.7
p.pose.position.y = 0.7
p.pose.position.z = 0.0
# add the cup
scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')
# modify the pose
p.pose.position.x = 0.72
p.pose.position.z = 0.05
示例5: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
#.........这里部分代码省略.........
rospy.logdebug("in AS: Closest cluster id is: " + str(closest_cluster_id))
#TODO visualize bbox
#TODO publish filtered pointcloud?
rospy.logdebug("BBOX: " + str(obj_bbox_dims))
########
self.update_feedback("Check reachability")
########
self.update_feedback("Generating grasps")
rospy.logdebug("Object pose before tf thing is: " + str(object_pose))
#transform pose to base_link, IS THIS NECESSARY?? should be a function in any case
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
#HACK remove orientation -> pose is aligned with parent(base_link)
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
# shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
# TODO remove this hack, fix it in table filtering
object_pose.pose.position.z += obj_bbox_dims[2] / 2.0
grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
# check if there are grasps, if not, abort
if len(grasp_list) == 0:
self.update_aborted("No grasps received")
return
if DEBUG_MODE: # TODO: change to dynamic param
publish_grasps_as_poses(grasp_list)
self.current_goal.publish_feedback(self.as_feedback)
########
self.update_feedback("Setup planning scene")
# Add object to grasp to planning scene
self.scene.add_box(self.current_side + "_hand_object", object_pose,
(obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
self.as_result.object_scene_name = self.current_side + "_hand_object"
########
self.update_feedback("Execute grasps")
pug = createPickupGoal(self.current_side + "_hand_object", grasp_list, group=goal_message_field.group)
rospy.loginfo("Sending pickup goal")
self.pickup_ac.send_goal(pug)
rospy.loginfo("Waiting for result...")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
########
self.update_feedback("finished")
self.as_result.object_pose = object_pose
self.as_result.error_code = result.error_code
self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if self.current_side == 'right':
self.right_hand_object = self.current_side + "_hand_object"
elif self.current_side == 'left':
self.left_hand_object = self.current_side + "_hand_object"
self.current_goal.set_succeeded(result=self.as_result)
else:
# Remove object as it has not been picked
self.scene.remove_world_object(self.current_side + "_hand_object")
self.update_aborted(text="MoveIt pick failed", manipulation_result=self.as_result)
else:
self.update_aborted("Goal fields not correctly fulfilled")
def place_operation(self):
"""Execute the place operation"""
if self.message_fields_ok():
示例6: PoseStamped
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("ball")
scene.remove_world_object("table")
scene.remove_world_object("ground_plane")
rospy.sleep(5)
print ">>>>> add scenes"
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = -1
p.pose.position.z = 0.2
p.pose.orientation.w = 1.0
scene.add_box("pole", p, (0.4, 0.1, 0.4))
p.pose.position.x = 0
p.pose.position.y = 0.8
p.pose.position.z = 0.1
p.pose.orientation.w = 1.0
scene.add_sphere("ball", p, 0.1)
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.1
scene.add_box("table", p, (1.0, 2.6, 0.2))
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.4
示例7: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
class GraspObjectServer:
def __init__(self, name):
# stuff for grasp planning
self.tf_listener = tf.TransformListener()
self.tf_broadcaster = tf.TransformBroadcaster()
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_objects = RecognizedObjectArray()
#rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
#pickup_ac.wait_for_server() # needed?
rospy.loginfo("Connecting to grasp generator AS")
self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
#grasps_ac.wait_for_server() # needed?
#planning scene for motion planning
self.scene = PlanningSceneInterface()
# blocking action server
self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
self.feedback = GraspObjectFeedback()
self.result = GraspObjectResult()
self.current_goal = None
self.grasp_obj_as.start()
def objects_callback(self, data):
rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
self.last_objects = data
def goal_callback(self, goal):
if self.current_goal:
goal.set_rejected() # "Server busy"
return
elif len(self.last_objects.objects) - 1 < goal.get_goal().target_id:
goal.set_rejected() # "No objects to grasp were received on the objects topic."
return
else:
#store and accept new goal
self.current_goal = goal
self.current_goal.set_accepted()
#run grasping state machine
self.grasping_sm()
#finished, get rid of goal
self.current_goal = None
def cancel_callback(self, goal):
#TODO stop motions?
self.current_goal.set_canceled()
def grasping_sm(self):
if self.current_goal:
self.update_feedback("running clustering")
(object_points, obj_bbox_dims,
object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(self.last_objects.objects[self.current_goal.get_goal().target_id].point_clouds[0])
#TODO visualize bbox
#TODO publish filtered pointcloud?
print obj_bbox_dims
########
self.update_feedback("check reachability")
########
self.update_feedback("generate grasps")
#transform pose to base_link
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(5))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
#HACK remove orientation -> pose is aligned with parent(base_link)
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
# shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
# TODO remove this hack, fix it in table filtering
object_pose.pose.position.z += obj_bbox_dims[2]/2.0
grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
# check if there are grasps, if not, abort
if len(grasp_list) == 0:
self.update_aborted("no grasps received")
return
self.publish_grasps_as_poses(grasp_list)
self.feedback.grasps = grasp_list
self.current_goal.publish_feedback(self.feedback)
self.result.grasps = grasp_list
########
self.update_feedback("setup planning scene")
#remove old objects
self.scene.remove_world_object("object_to_grasp")
# add object to grasp to planning scene
self.scene.add_box("object_to_grasp", object_pose,
(obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
self.result.object_scene_name = "object_to_grasp"
########
if self.current_goal.get_goal().execute_grasp:
self.update_feedback("execute grasps")
pug = self.createPickupGoal("object_to_grasp", grasp_list)
rospy.loginfo("Sending goal")
#.........这里部分代码省略.........
示例8: PlanningSceneInterface
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
if __name__ == '__main__':
rospy.init_node('scripting_example')
while rospy.get_time() == 0.0: pass
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rospy.sleep(1.0)
### Create a handle for the Move Group Commander
mgc = MoveGroupCommander("arm")
rospy.sleep(1.0)
### Add virtual obstacle
pose = gen_pose(pos=[-0.2, -0.1, 1.2])
psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
rospy.sleep(1.0)
### Move to stored joint position
mgc.set_named_target("left")
mgc.go()
### Move to Cartesian position
goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707])
mgc.go(goal_pose.pose)
### Move Cartesian linear
goal_pose.pose.position.z -= 0.1
(traj,frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True)
mgc.execute(traj)
示例9: PlanningSceneInterface
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
scene = PlanningSceneInterface()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = REFERENCE_LINK
p.header.stamp = rospy.Time.now()
quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
p.pose.orientation = Quaternion(*quat)
p.pose.position.x = 0.6
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# add table
scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2))
i = 1
while i <= 10:
gripper_pose = arm.get_current_pose(arm.get_end_effector_link())
# part
p.pose.position.x = gripper_pose.pose.position.x
p.pose.position.y = gripper_pose.pose.position.y
p.pose.position.z = gripper_pose.pose.position.z
# add part
scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1))
rospy.loginfo("Added object to world")
# attach object manually
arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS)
示例10: PoseStamped
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
rospy.loginfo("Cleaning world objects")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = '/base_link'
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.6
p.pose.position.y = 0.0
p.pose.position.z = 0.65
p.pose.orientation.w = 1.0
scene.add_box("table", p, (0.5, 1.5, 0.9))
p.pose.position.x = 0.4
p.pose.position.y = -0.3
p.pose.position.z = 1.15
angle = radians(80) # angles are expressed in radians
quat = quaternion_from_euler(0.0, 0.0, angle) # roll, pitch, yaw
p.pose.orientation = Quaternion(*quat.tolist())
#scene.add_box("part", p, (0.04, 0.04, 0.1))
scene.add_box("part", p, (0.03, 0.03, 0.1))
rospy.loginfo("Added object to world")
rospy.sleep(1)
示例11: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object("right_gripper_link", "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
right_arm.set_named_target("start1")
right_arm.go()
right_gripper.set_named_target("open")
right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.15
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.0130178
start_pose.pose.position.y = -0.125155
start_pose.pose.position.z = 0.597653
start_pose.pose.orientation.x = 0.0
start_pose.pose.orientation.y = 0.388109
start_pose.pose.orientation.z = 0.0
start_pose.pose.orientation.w = 0.921613
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
#grasps = self.make_grasps(start_pose)
#result = False
#n_attempts = 0
# repeat until will succeed
#while result == False:
#result = robot.right_arm.pick("part", grasps)
#n_attempts += 1
#print "Attempts: ", n_attempts
#rospy.sleep(0.2)
rospy.spin()
roscpp_shutdown()
示例12: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
def __init__(self):
"""
Setup ROS communication between the Kinect and Baxter
:return:
"""
# Initialise variables
self.rgb_img = None
self.depth_img = None
self.marker_box = None
self.marker_center_x = None
self.marker_center_y = None
self.marker_depth = None
self.bridge = CvBridge()
# First initialize moveit_commander and rospy.
print "============ Initialising Baxter"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('can_node', anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.left_arm = moveit_commander.MoveGroupCommander("left_arm")
self.right_arm = moveit_commander.MoveGroupCommander("right_arm")
self.right_arm_navigator = Navigator('right')
# Setup grippers
self.right_gripper = baxter_interface.Gripper('right')
# Setup the table in the scene
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('/move_group/monitored_planning_scene', PlanningScene)
table_id = 'table'
scene.remove_world_object(table_id)
rospy.sleep(1)
table_ground = -0.3
table_size = [4.0, 4.6, 0.1]
table_pose = PoseStamped()
table_pose.header.frame_id = self.robot.get_planning_frame()
table_pose.pose.position.x = 0.7
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
# Create a dictionary to hold object colors
self.colors = dict()
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.sendColors()
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
# We can get a list of all the groups in the robot
print "============ Right Pose:"
print self.right_arm.get_current_pose()
print "============ Left Pose:"
print self.left_arm.get_current_pose()
# Setup the subscribers and publishers
self.rgb_sub = rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.callback_rgb)
self.points_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.callback_points)
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=5)
self.screen_pub = rospy.Publisher('robot/xdisplay', Image, latch=True, queue_size=10)
self.right_hand_range_pub = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self.callback_range, queue_size=1)
self.left_cam_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self.callback_left_hand)
示例13: constructor
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
class PigeonPickAndPlace:
#Class constructor (parecido com java, inicializa o que foi instanciado)
def __init__(self):
# Retrieve params:
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016)
self._arm_group = rospy.get_param('~arm', 'arm_move_group')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
# Create planning scene where we will add the objects etc.
self._scene = PlanningSceneInterface()
# Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene (remove the old objects:
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
# TODO get the position of the detected object
self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name)
rospy.sleep(1.0)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Pick object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
def __del__(self):
# Clean the scene
self._scene.remove_world_object(self._grasp_object_name)
def _add_object_grasp(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame() #pose
p.header.stamp = rospy.Time.now()
rospy.loginfo(self._robot.get_planning_frame())
p.pose.position.x = 0.11 # 0.26599 # antigo = 0.75 - 0.01
p.pose.position.y = -0.31 # -0.030892 #antigo = 0.25 - 0.01
p.pose.position.z = 0.41339 #antigo = 1.00 + (0.3 + 0.03) / 2.0
#p.pose.orientation.x = 0.0028925
#p.pose.orientation.y = -0.0019311
#p.pose.orientation.z = -0.71058
#p.pose.orientation.w = 0.70361
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/,
# using the measure tape tool from meshlab.
# The box is the bounding box of the lego cylinder.
# The values are taken from the cylinder base diameter and height.
# the size is length (x),thickness(y),height(z)
# I don't know if the detector return this values of object
self._scene.add_box(name, p, (0.032, 0.016, 0.020))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
#.........这里部分代码省略.........
示例14: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
#.........这里部分代码省略.........
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
if obj_att is None:
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
# Publish targe's frame
#self.object_frames_pub.publish(target_pose)
示例15:
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
# p.pose.position.x = 0.8
# p.pose.position.y = -0.15
# p.pose.position.z = 0.85
# p.pose.orientation.w = 1.0
#scene.add_box("pole", p, (0.3, 0.05, 1.0))
# p.pose.position.x = 0.79
# p.pose.position.y = 0.15
# p.pose.position.z = 0.70
# add_result = scene.add_box("table", p, (0.6, 0.6, 0.15))
# print add_result
p.pose.position.x = 0.6
p.pose.position.y = 0.18
p.pose.position.z = 0.7
scene.add_box("part1", p, (0.05, 0.05, 0.1 ))
robot.right_arm.set_named_target("right_pregrasp")
robot.right_arm.go()
rospy.sleep(3)
gripper.gripper_open()
rospy.sleep(3)
# pick an object
robot.right_arm.pick("part1")
rospy.sleep(1)
gripper.gripper_close()
rospy.sleep(1)
robot.right_arm.set_named_target("right_normal")