class ObjectBroadcaster:
""" Given a detected object, request more information about it and publish the result as a CollisionObject """
def __init__(self,
topic='/collision_object',
diff_topic='/recognized_object_diff'):
self._publisher = rospy.Publisher(topic, CollisionObject)
self._diff_publisher = rospy.Publisher(diff_topic, PlanningScene)
self._index = 1
self._min_confidence = 0.5
self._lock = Lock()
self._get_object_info = rospy.ServiceProxy('get_object_info',
GetObjectInformation)
def broadcast_diff(self, objects):
planning_scene = PlanningScene()
planning_scene.is_diff = True
if isinstance(objects, collections.Iterable):
for ob in objects:
co = self._create_collision_object(ob)
planning_scene.world.collision_objects.append(co)
else:
co = self._create_collision_object(objects)
planning_scene.world.collision_objects.append(co)
self._diff_publisher.publish(planning_scene)
def _create_collision_object(self, ob):
if ob.confidence < self._min_confidence:
rospy.loginfo(
"Not publishing object of type %s because confidence %s < %s" %
(ob.type.key, str(ob.confidence), str(self._min_confidence)))
return
info = None
try:
info = self._get_object_info(ob.type).information
except rospy.ServiceException, e:
rospy.logwarn(
"Unable to retrieve object information for object of type\n%s"
% str(ob.type))
co = CollisionObject()
co.type = ob.type
co.operation = CollisionObject.ADD
co.header = ob.pose.header
if info:
if len(info.name) > 0:
co.id = info.name + '_' + str(self._bump_index())
else:
co.id = ob.type.key + '_' + str(self._bump_index())
if len(info.ground_truth_mesh.triangles) > 0:
co.meshes = [info.ground_truth_mesh]
else:
co.meshes = [ob.bounding_mesh]
co.mesh_poses = [ob.pose.pose.pose]
else:
rospy.loginfo("Did not find information for object %s:" %
(ob.type.key))
co.id = ob.type.key + '_' + str(self._bump_index())
co.meshes = [ob.bounding_mesh]
co.mesh_poses = [ob.pose.pose.pose]
if len(co.meshes[0].triangles) > 0:
rospy.loginfo("Publishing collision object %s with confidence %s" %
(co.id, str(ob.confidence)))
# hack to turn the mesh into a box (aabb)
#co.primitive_poses = co.mesh_poses
#co.mesh_poses = []
min_x = 1000000
min_y = 1000000
min_z = 1000000
max_x = -1000000
max_y = -1000000
max_z = -1000000
for v in co.meshes[0].vertices:
if v.x > max_x:
max_x = v.x
if v.y > max_y:
max_y = v.y
if v.z > max_z:
max_z = v.z
if v.x < min_x:
min_x = v.x
if v.y < min_y:
min_y = v.y
if v.z < min_z:
min_z = v.z
box_co = copy.deepcopy(co)
box_co.meshes[0].vertices = []
box_co.meshes[0].triangles = []
p = Point()
p.x = min_x
p.y = min_y
p.z = min_z
box_co.meshes[0].vertices.append(p)
p = Point()
p.x = max_x
p.y = min_y
p.z = min_z
box_co.meshes[0].vertices.append(p)
p = Point()
p.x = max_x
p.y = min_y
p.z = max_z
box_co.meshes[0].vertices.append(p)
p = Point()
p.x = min_x
p.y = min_y
p.z = max_z
box_co.meshes[0].vertices.append(p)
p = Point()
p.x = min_x
p.y = max_y
p.z = max_z
box_co.meshes[0].vertices.append(p)
p = Point()
p.x = min_x
p.y = max_y
p.z = min_z
box_co.meshes[0].vertices.append(p)
p = Point()
p.x = max_x
p.y = max_y
p.z = max_z
box_co.meshes[0].vertices.append(p)
p = Point()
p.x = max_x
p.y = max_y
p.z = min_z
box_co.meshes[0].vertices.append(p)
t = MeshTriangle()
t.vertex_indices[0] = 0
t.vertex_indices[1] = 1
t.vertex_indices[2] = 2
box_co.meshes[0].triangles.append(t)
t = MeshTriangle()
t.vertex_indices[0] = 2
t.vertex_indices[1] = 3
t.vertex_indices[2] = 0
box_co.meshes[0].triangles.append(t)
t = MeshTriangle()
t.vertex_indices[0] = 4
t.vertex_indices[1] = 3
t.vertex_indices[2] = 2
box_co.meshes[0].triangles.append(t)
t = MeshTriangle()
t.vertex_indices[0] = 2
t.vertex_indices[1] = 6
t.vertex_indices[2] = 4
box_co.meshes[0].triangles.append(t)
t = MeshTriangle()
t.vertex_indices[0] = 7
t.vertex_indices[1] = 6
t.vertex_indices[2] = 2
box_co.meshes[0].triangles.append(t)
t = MeshTriangle()
t.vertex_indices[0] = 2
t.vertex_indices[1] = 1
t.vertex_indices[2] = 7
box_co.meshes[0].triangles.append(t)
t = MeshTriangle()
t.vertex_indices[0] = 3
t.vertex_indices[1] = 4
t.vertex_indices[2] = 5
box_co.meshes[0].triangles.append(t)
t = MeshTriangle()
t.vertex_indices[0] = 5
t.vertex_indices[1] = 0
t.vertex_indices[2] = 3
box_co.meshes[0].triangles.append(t)
t = MeshTriangle()
t.vertex_indices[0] = 0
t.vertex_indices[1] = 5
t.vertex_indices[2] = 7
box_co.meshes[0].triangles.append(t)
t = MeshTriangle()
t.vertex_indices[0] = 7
t.vertex_indices[1] = 1
t.vertex_indices[2] = 0
box_co.meshes[0].triangles.append(t)
t = MeshTriangle()
t.vertex_indices[0] = 7
t.vertex_indices[1] = 5
t.vertex_indices[2] = 4
box_co.meshes[0].triangles.append(t)
t = MeshTriangle()
t.vertex_indices[0] = 4
t.vertex_indices[1] = 6
t.vertex_indices[2] = 7
box_co.meshes[0].triangles.append(t)
return box_co
def _create_collision_object_base(self, model_instance_name):
collision_object = CollisionObject()
collision_object.header.frame_id = 'world'
collision_object.id = '{}__link'.format(model_instance_name)
collision_object.operation = CollisionObject.ADD
return collision_object
def broadcast_one(self, ob):
if ob.confidence < self._min_confidence:
rospy.loginfo(
"Not publishing object of type %s because confidence %s < %s" %
(ob.type.key, str(ob.confidence), str(self._min_confidence)))
return
info = None
try:
info = self._get_object_info(ob.type).information
except rospy.ServiceException, e:
rospy.logwarn(
"Unable to retrieve object information for object of type\n%s"
% str(ob.type))
co = CollisionObject()
co.type = ob.type
co.operation = CollisionObject.ADD
co.header = ob.pose.header
if info:
if len(info.name) > 0:
co.id = info.name + '_' + str(self._bump_index())
else:
co.id = ob.type.key + '_' + str(self._bump_index())
if len(info.ground_truth_mesh.triangles) > 0:
co.meshes = [info.ground_truth_mesh]
else:
co.meshes = [ob.bounding_mesh]
co.mesh_poses = [ob.pose.pose.pose]
else:
if __name__ == "__main__":
rospy.init_node("simple_obstacle_pub")
root_frame = "/odom_combined"
pub = rospy.Publisher("obstacle_distance/registerObstacle", CollisionObject, queue_size = 1)
while pub.get_num_connections() < 1:
if rospy.is_shutdown():
exit(0)
rospy.logwarn("Please create a subscriber to '/arm_right/obstacle_distance/registerObstacle' topic (Type: moveit_msgs/CollisionObject)")
time.sleep(1.0)
rospy.loginfo("Continue ...")
# Publish a simple sphere
x = CollisionObject()
x.id = "Funny Sphere"
x.header.frame_id = root_frame
x.operation = CollisionObject.ADD
#x.operation = CollisionObject.REMOVE
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions.append(0.1) # radius
x.primitives.append(sphere)
pose = Pose()
pose.position.x = 0.35
pose.position.y = -0.45
pose.position.z = 0.8
pose.orientation.x = 0.0;
pose.orientation.y = 0.0;
def __init__(self):
#Specify a frame_id - transformation to root_frame of obstacle_distance node is handled in according subscriber callback
self.root_frame = rospy.get_param("chain_root_link")
self.obstacle_frame = rospy.get_namespace() + "interactive_box_frame"
self.pub = rospy.Publisher("obstacle_distance/registerObstacle",
CollisionObject,
queue_size=1,
latch=True)
self.br = tf.TransformBroadcaster()
while self.pub.get_num_connections() < 1:
rospy.logwarn(
"Please create a subscriber to '" + rospy.get_namespace() +
"/obstacle_distance/registerObstacle' topic (Type: moveit_msgs/CollisionObject)"
)
rospy.sleep(1.0)
rospy.loginfo("Continue ...")
# Compose CollisionObject (Box)
self.co = CollisionObject()
self.co.id = "Interactive Box"
self.co.header.frame_id = self.obstacle_frame
self.co.operation = CollisionObject.ADD
primitive = SolidPrimitive()
primitive.type = SolidPrimitive.SPHERE
primitive.dimensions = [0.07] # extent x, y, z
self.co.primitives.append(primitive)
pose = Pose()
pose.orientation.w = 1.0
self.co.primitive_poses.append(pose)
# Compose InteractiveMarker
self.interactive_obstacle_pose = PoseStamped()
self.interactive_obstacle_pose.header.stamp = rospy.Time.now()
self.interactive_obstacle_pose.header.frame_id = self.root_frame
self.interactive_obstacle_pose.pose.position.x = -0.5
self.interactive_obstacle_pose.pose.position.y = 0.3
self.interactive_obstacle_pose.pose.position.z = 0.8
self.interactive_obstacle_pose.pose.orientation.w = 1.0
self.ia_server = InteractiveMarkerServer("obstacle_marker_server")
self.int_marker = InteractiveMarker()
self.int_marker.header.frame_id = self.root_frame
self.int_marker.pose = self.interactive_obstacle_pose.pose
self.int_marker.name = "interactive_obstacle_marker"
self.int_marker.scale = 0.25
# Setup MarkerControls
control_3d = InteractiveMarkerControl()
control_3d.always_visible = True
control_3d.name = "move_rotate_3D"
control_3d.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
self.int_marker.controls.append(control_3d)
control = InteractiveMarkerControl()
control.always_visible = True
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "move_y"
control.orientation.x = 0
control.orientation.y = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "move_z"
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "rotate_y"
control.orientation.x = 0
control.orientation.y = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(deepcopy(control))
control.name = "rotate_z"
control.orientation.y = 0
control.orientation.z = 1
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(deepcopy(control))
self.ia_server.insert(self.int_marker, self.marker_fb)
self.ia_server.applyChanges()
# initial send
self.br.sendTransform(
(self.interactive_obstacle_pose.pose.position.x,
self.interactive_obstacle_pose.pose.position.y,
self.interactive_obstacle_pose.pose.position.z),
(self.interactive_obstacle_pose.pose.orientation.x,
self.interactive_obstacle_pose.pose.orientation.y,
self.interactive_obstacle_pose.pose.orientation.z,
self.interactive_obstacle_pose.pose.orientation.w),
rospy.Time.now(), self.obstacle_frame,
self.interactive_obstacle_pose.header.frame_id)
rospy.sleep(1.0)
self.pub.publish(self.co)
rospy.sleep(1.0)
#Define a box to be attached
primitive = SolidPrimitive()
primitive.type = primitive.BOX
primitive.dimensions = [0.045, 0.045, 0.08]
attached_object.object.primitives.append(primitive)
attached_object.object.primitive_poses.append(pose)
attached_object.object.operation = attached_object.object.ADD
planning_scene = PlanningScene(is_diff=True)
planning_scene.world.collision_objects.append(attached_object.object)
scene_pub.publish(planning_scene)
rospy.sleep(2)
remove_object = CollisionObject()
remove_object.id = "target"
remove_object.header.frame_id = "base_footprint"
remove_object.operation = remove_object.REMOVE
rospy.loginfo(
"Attaching the object to the right wrist and removing it from the world."
)
planning_scene.world.collision_objects.append(remove_object)
planning_scene.robot_state.attached_collision_objects.append(
attached_object)
scene_pub.publish(planning_scene)
rospy.sleep(2)
#ask the robot arm to grasp the target object
onine_arm.close_gripper()
#!/usr/bin/env python
import json
import os
import threading
import time
import rospy
import moveit_interface as vc_moveit
from geometry_msgs.msg import Pose
from std_msgs.msg import String
from moveit_msgs.msg import PlanningScene, ObjectColor, CollisionObject
from vc_ros_tutorials.srv import vc_ros_connector, vc_ros_connectorResponse
# global variable for moveit and vc connection
moveit_obj = vc_moveit.MoveItDemo()
collision_object_msg = CollisionObject()
dynamic_objects = []
planner_pub = rospy.Publisher("vc_ros_planner", String, queue_size=10)
planner_state = ""
def dynamic_object_pose(data):
# print data
global dynamic_objects
collision_object_pose = Pose()
data = data.data.split(';')
if len(data) == 2:
dynamic_object_name = data[0]
if "remove" not in data[1]:
pose_data = json.loads(data[1])
dynamic_object_pos = pose_data['position']
def link_to_collision_object(self, link, full_linkname):
mesh_path = None
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
linkparts = getattr(link, 'collisions')
if self.is_ignored(link.parent_model):
print("Ignoring link %s." % full_linkname)
return
collision_object = CollisionObject()
collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname)
for linkpart in linkparts:
if linkpart.geometry_type not in supported_geometry_types:
print(
"Element %s with geometry type %s not supported. Ignored."
% (full_linkname, linkpart.geometry_type))
continue
if linkpart.geometry_type == 'mesh':
scale = tuple(
float(val)
for val in linkpart.geometry_data['scale'].split())
for models_path in pysdf.models_paths:
resource = linkpart.geometry_data['uri'].replace(
'model://', models_path)
if os.path.isfile(resource):
mesh_path = resource
break
if mesh_path is not None:
link_pose_stamped = PoseStamped()
link_pose_stamped.pose = pysdf.homogeneous2pose_msg(
linkpart.pose)
if not self.make_mesh(collision_object, link_pose_stamped,
mesh_path, scale):
return None
elif linkpart.geometry_type == 'box':
scale = tuple(
float(val)
for val in linkpart.geometry_data['size'].split())
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = scale
collision_object.primitives.append(box)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
elif linkpart.geometry_type == 'sphere':
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [float(linkpart.geometry_data['radius'])]
collision_object.primitives.append(sphere)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
elif linkpart.geometry_type == 'cylinder':
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = tuple(
(float(linkpart.geometry_data['length']),
float(linkpart.geometry_data['radius'])))
collision_object.primitives.append(cylinder)
collision_object.primitive_poses.append(
pysdf.homogeneous2pose_msg(linkpart.pose))
return collision_object
def link_to_collision_object(link, full_linkname):
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
linkparts = getattr(link, 'collisions')
if is_ignored(link.parent_model):
print("Ignoring link %s." % full_linkname)
return
collision_object = CollisionObject()
collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname)
root_collision_model = get_root_collision_model(link)
link_pose_in_root_frame = pysdf.homogeneous2pose_msg(
concatenate_matrices(link.pose_world,
inverse_matrix(root_collision_model.pose_world)))
for linkpart in linkparts:
if linkpart.geometry_type not in supported_geometry_types:
print("Element %s with geometry type %s not supported. Ignored." %
(full_linkname, linkpart.geometry_type))
continue
if linkpart.geometry_type == 'mesh':
scale = tuple(
float(val) for val in linkpart.geometry_data['scale'].split())
for models_path in pysdf.models_paths:
test_mesh_path = linkpart.geometry_data['uri'].replace(
'model://', models_path)
if os.path.isfile(test_mesh_path):
mesh_path = test_mesh_path
break
if mesh_path:
link_pose_stamped = PoseStamped()
link_pose_stamped.pose = link_pose_in_root_frame
make_mesh(collision_object, full_linkname, link_pose_stamped,
mesh_path, scale)
else:
print("ERROR: No mesh found for '%s' in element '%s'." %
(linkpart.geometry_data['uri'], full_linkname))
elif linkpart.geometry_type == 'box':
scale = tuple(
float(val) for val in linkpart.geometry_data['size'].split())
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = scale
collision_object.primitives.append(box)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'sphere':
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius'])
collision_object.primitives.append(sphere)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'cylinder':
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = tuple(
(2.0 * float(linkpart.geometry_data['radius']),
float(linkpart.geometry_data['length'])))
collision_object.primitives.append(cylinder)
collision_object.primitive_poses.append(link_pose_in_root_frame)
#print('CollisionObject for %s:\n%s' % (full_linkname, collision_object))
return collision_object
def __init__(self):
self.seq = 0
self.tf_pub = tf.TransformBroadcaster()
# Preset joint state information
self.joint_names = [
'r_shoulder_lift_joint', 'r_front_wheel_joint',
'r_gripper_right_finger_joint', 'l_shoulder_pan_joint',
'l_gripper_left_finger_base_joint', 'l_wrist_2_joint',
'r_gripper_right_finger_base_joint',
'l_gripper_mid_finger_base_joint', 'l_gripper_mid_finger_joint',
'r_wrist_2_joint', 'r_gripper_left_finger_joint',
'r_gripper_left_finger_base_joint', 'l_wrist_3_joint',
'l_gripper_right_finger_joint', 'l_elbow_joint',
'r_gripper_mid_finger_base_joint', 'r_shoulder_pan_joint',
'r_wrist_3_joint', 'l_shoulder_lift_joint', 'r_rear_wheel_joint',
'r_elbow_joint', 'l_gripper_right_finger_base_joint',
'l_gripper_left_finger_joint', 'l_rear_wheel_joint',
'r_wrist_1_joint', 'l_wrist_1_joint', 'r_gripper_mid_finger_joint',
'l_front_wheel_joint'
]
self.default_pose = [
-1.3024941653962576, 0.0, 0.0, 1.2151680370199998, 0.0,
-1.1008140645600006, 0.0, 0.0, 0.0, 2.2992189762402173,
0.0026895523773320003, -0.0006283185307176531, -0.8256105484199994,
0.0, -2.05585823016, 0.0, -0.7340859109337838, 1.4271237788102449,
-1.6210618074000003, 0.20294688542190054, 1.5361204651811313, 0.0,
0.0, 0.0, -2.0823833025971066, -2.5773626100600002, 0.0, 0.0
]
# These are the preset positions for the various TOM objects. These are
# reference frames used for computing features. These are the ones
# associated with the main TOM dataset.
self.box = (0.67051013617, -0.5828498549, -0.280936861547)
self.squeeze_area = (0.542672622341, 0.013907504104, -0.466499112972)
self.trash = (0.29702347941, 0.0110837137159, -0.41238342306)
self.orange1 = (0.641782207489, -0.224464386702, -0.523829042912)
self.orange2 = (0.69, -0.31, -0.523829042912)
self.orange3 = (0.68, -0.10, -0.523829042912)
# Rotation frame for all of these is pointing down at the table.
self.rot = (0, 0, 0, 1)
self.gripper_open = True
#self.box = pm.toMsg(pm.fromTf((box, rot)))
#self.trash = pn.toMsg(pm.fromTf((trash, rot)))
#self.squeeze_area = pm.toMsg(pm.fromTf((trash, rot)))
self.qs = {}
for name, pose in zip(self.joint_names, self.default_pose):
self.qs[name] = pose
self.reset_srv = rospy.Service('tom_sim/reset', EmptySrv,
self.reset_cb)
self.js_pub = rospy.Publisher('joint_states',
JointState,
queue_size=1000)
self.ps_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=1000)
self.co_pub = rospy.Publisher('collision_object',
CollisionObject,
queue_size=1000)
self.cmd_sub = rospy.Subscriber('joint_states_cmd', JointState,
self.cmd_cb)
# Get the first planning scene, containing the loaded geometry and meshes
# and all that.
rospy.wait_for_service('/get_planning_scene')
primitive_table = SolidPrimitive(type=SolidPrimitive.BOX,
dimensions=[0.6, 1.4, 0.7])
#pose_table = Pose(position=Point(0.85,0.,-0.06))
pose_table = Pose(position=Point(0.85, 0., -0.06))
primitive_trash = SolidPrimitive(type=SolidPrimitive.BOX,
dimensions=[0.2, 0.2, 0.16])
#pose_trash = Pose(position=Point(0.65,-0.55,0.35))
pose_trash = Pose(position=Point(0.65, -0.55, 0.35))
table = CollisionObject(id="table",
primitives=[primitive_table, primitive_trash],
primitive_poses=[pose_table, pose_trash])
table.operation = CollisionObject.ADD
table.header.frame_id = "odom_combined"
# Collision objects
self.obstacles = [table]
def plane(): wall_object = CollisionObject() wall_object.operation = wall_object.ADD wall_object.id = "wall_box" wall_object.header.frame_id = "base_link" wall_pose = Pose() wall_pose.orientation.w = 1 wall_pose.position.z = 0.15 wall_pose.position.x = -0.18 wall_pose.position.y = 0 wall = SolidPrimitive() wall.type = SolidPrimitive.BOX wall.dimensions = [0.01, 0.27, 0.3] wall_object.primitives = [wall] wall_object.primitive_poses.append(wall_pose) # ---------------------------------- wall2_object = CollisionObject() wall2_object.operation = wall_object.ADD wall2_object.id = "wall2_box" wall2_object.header.frame_id = "base_link" wall2_pose = Pose() wall2_pose.orientation.w = 1 wall2_pose.position.z = 0.15 wall2_pose.position.x = -0.05 wall2_pose.position.y = -0.15 wall2 = SolidPrimitive() wall2.type = SolidPrimitive.BOX wall2.dimensions = [0.24, 0.01, 0.3] wall2_object.primitives = [wall2] wall2_object.primitive_poses.append(wall2_pose) # ---------------------------------- wall3_object = CollisionObject() wall3_object.operation = wall_object.ADD wall3_object.id = "wall3_box" wall3_object.header.frame_id = "base_link" wall3_pose = Pose() wall3_pose.orientation.w = 1 wall3_pose.position.z = 0.15 wall3_pose.position.x = -0.05 wall3_pose.position.y = 0.15 wall3 = SolidPrimitive() wall3.type = SolidPrimitive.BOX wall3.dimensions = [0.24, 0.01, 0.3] wall3_object.primitives = [wall3] wall3_object.primitive_poses.append(wall3_pose) # ---------------------------------- base1_object = CollisionObject() base1_object.operation = base1_object.ADD base1_object.id = "base1_box" base1_object.header.frame_id = "base_link" base1_pose = Pose() base1_pose.orientation.w = 1 base1_pose.position.y = 0.0 base1_pose.position.x = -0.07 base1_pose.position.z = 0.03 base1 = SolidPrimitive() base1.type = SolidPrimitive.BOX base1.dimensions = [0.22, 0.27, 0.01] base1_object.primitives = [base1] base1_object.primitive_poses.append(base1_pose) # ---------------------------------- base2_object = CollisionObject() base2_object.operation = base1_object.ADD base2_object.id = "base2_box" base2_object.header.frame_id = "base_link" base2_pose = Pose() base2_pose.orientation.w = 1 base2_pose.position.y = 0.0 base2_pose.position.x = -0.07 base2_pose.position.z = 0.35 base2 = SolidPrimitive() base2.type = SolidPrimitive.BOX base2.dimensions = [0.20, 0.27, 0.01] base2_object.primitives = [base2] base2_object.primitive_poses.append(base2_pose) planning_scene = PlanningScene() planning_scene.is_diff = True planning_scene.world.collision_objects.append(wall_object) planning_scene.world.collision_objects.append(wall2_object) planning_scene.world.collision_objects.append(base1_object) planning_scene.world.collision_objects.append(base2_object) client(planning_scene)
#!/usr/bin/python
import rospy
import moveit_python
from moveit_msgs.msg import CollisionObject
from geometry_msgs.msg import Pose, Point, Quaternion
from shape_msgs.msg import SolidPrimitive
from std_msgs.msg import Header
rospy.init_node("moveit_table_scene")
scene = moveit_python.PlanningSceneInterface("base_link")
#pub = rospy.Publisher("/move_group/monitored_planning_scene", CollisionObject, queue_size=1)
table = CollisionObject()
# Header
h = Header()
h.stamp = rospy.Time.now()
# Shape
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = [0.808, 1.616, 2.424]
# Pose
position = Point(*[1.44, 0.0, 0.03])
orient = Quaternion(*[-0.510232, 0.49503, 0.515101, 0.478832])
# Create Collision Object
scene.addSolidPrimitive("table", box, Pose(*[position, orient]))
def add_order_bin(x, y):
# Necessary parameters
bin_width, bin_depth, bin_height = 0.65, 0.4, 0.43
interior_width, interior_depth, interior_height = 0.45, 0.29, 0.18
wall_width, wall_depth = (bin_width - interior_width) / 2, (
bin_depth - interior_depth) / 2
base_height = bin_height - interior_height
objects = []
poses = []
# Create Bottom
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[bin_depth, bin_width, base_height],
))
poses.append(
Pose(
position=Point(x=x, y=y, z=base_height / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create left side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[bin_depth, wall_width, interior_height],
))
poses.append(
Pose(
position=Point(x=x,
y=y - interior_width / 2 - wall_width / 2,
z=base_height + interior_height / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create right side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[bin_depth, wall_width, interior_height],
))
poses.append(
Pose(
position=Point(x=x,
y=y + interior_width / 2 + wall_width / 2,
z=base_height + interior_height / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create near side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[wall_depth, interior_width, interior_height],
))
poses.append(
Pose(
position=Point(x=x - interior_depth / 2 - wall_depth / 2,
y=y,
z=base_height + interior_height / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
# Create far side
objects.append(
SolidPrimitive(
type=SolidPrimitive.BOX,
dimensions=[wall_depth, interior_width, interior_height],
))
poses.append(
Pose(
position=Point(x=x + interior_depth / 2 + wall_depth / 2,
y=y,
z=base_height + interior_height / 2),
orientation=Quaternion(x=0, y=0, z=0, w=1),
))
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "order_bin"
co.header.frame_id = "/base_link"
co.header.stamp = rospy.Time.now()
co.primitives = objects
co.primitive_poses = poses
scene._pub_co.publish(co)
def grasp(self,
pose,
radius,
height,
frame_id='base_link',
name='pringles',
axial_res=6,
angle_res=10):
# Cilindro de colision
obj = CollisionObject()
obj.header.stamp = rospy.Time.now()
obj.header.frame_id = frame_id
obj.id = name
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = [height, radius]
obj.primitives.append(cylinder)
obj.primitive_poses.append(pose)
obj.operation = CollisionObject.ADD
# Grasp
limb = self.get_limb(pose)
limb.arm.generate_grasp(obj, axial_res, angle_res)
possible_grasp = limb.arm.get_grasp()
limb.arm.set_position_named('home')
rospy.sleep(2.0)
limb.arm.set_position_named('premanip_1')
rospy.sleep(3.0)
if not possible_grasp.ik_solutions:
rospy.logerr('No se generaron grasps')
return
pregrasp_joints = possible_grasp.ik_solutions[
2 * possible_grasp.order[0]].positions
grasp_joints = possible_grasp.ik_solutions[2 * possible_grasp.order[0]
+ 1].positions
result = limb.arm.set_joint(
pregrasp_joints) # Movimiento con planificador
if (result.error_code.val == MoveItErrorCodes.SUCCESS):
limb.gripper.open(effort=300)
rospy.sleep(1.0)
limb.arm.move_joint(
grasp_joints,
interval=2.5) # Movimiento con collisiones permitidas
limb.arm.wait()
rospy.sleep(0.5)
limb.gripper.close(effort=300)
rospy.sleep(1.0)
else:
rospy.sleep(2.0)
limb.arm.set_position_named('premanip_1')
rospy.sleep(2.0)
