def __make_mesh(self, name, pose, filename):
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]
point.y = vertex[1]
point.z = vertex[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def make_mesh(name, pose, filename, scale=(1, 1, 1)):
'''
returns a CollisionObject with the mesh defined in the path.
mostly copied from https://github.com/ros-planning/moveit_commander/blob/kinetic-devel/src/moveit_commander/planning_scene_interface.py
'''
print("Creating mesh with file from", filename)
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face) == 3:
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def __make_mesh(self, name, pose, filename, scale = (1, 1, 1)):
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]*scale[0]
point.y = vertex[1]*scale[1]
point.z = vertex[2]*scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def publishCO(self, points, id, is_table=False):
pts = np.asarray(points)
dx, dy, dz = np.max(pts, axis=0) - np.min(pts, axis=0)
max_x, max_y, max_z = np.max(pts, axis=0)
min_x, min_y, min_z = np.min(pts, axis=0)
collision_object = CollisionObject()
collision_object.id = id
collision_object.header = self.lastHeader
object_shape = SolidPrimitive()
object_shape.type = SolidPrimitive.BOX
object_shape.dimensions.append(dx)
object_shape.dimensions.append(dy)
shape_pose = Pose()
shape_pose.position.x = min_x + dx / 2
shape_pose.position.y = min_y + dy / 2
if is_table:
object_shape.dimensions.append(max_z)
shape_pose.position.z = max_z / 2
else:
object_shape.dimensions.append(dz)
shape_pose.position.z = min_z + dz / 2
shape_pose.orientation.w = 1.0
collision_object.primitives.append(object_shape)
collision_object.primitive_poses.append(shape_pose)
collision_object.operation = CollisionObject.ADD
self.pub_collision.publish(collision_object)
def add_box_obstacle(self, size, name, pose):
"""
Adds a rectangular prism obstacle to the planning scene
Inputs:
size: 3x' ndarray; (x, y, z) size of the box (in the box's body frame)
name: unique name of the obstacle (used for adding and removing)
pose: geometry_msgs/PoseStamped object for the CoM of the box in relation to some frame
"""
# Create a CollisionObject, which will be added to the planning scene
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
# Create a box primitive, which will be inside the CollisionObject
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = size
# Fill the collision object with primitive(s)
co.primitives = [box]
co.primitive_poses = [pose.pose]
# Publish the object
self._planning_scene_publisher.publish(co)
def pub_obj(self, obj_type, pos, dim):
m = Marker()
m.id = self.obj_types.index(obj_type)
m.ns = "simple_tabletop"
m.type = Marker.CUBE
m.pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1))
m.header = Header(0, rospy.Time(0), "base_link")
m.scale = Vector3(*dim)
m.color = self.colors[obj_type]
mtext = deepcopy(m)
mtext.type = Marker.TEXT_VIEW_FACING
mtext.id += 100
mtext.text = obj_type
M = MarkerArray([m, mtext])
self.pub.publish(M)
#moveit collision objects
co = CollisionObject()
co.header = deepcopy(m.header)
co.id = obj_type
obj_shape = SolidPrimitive()
obj_shape.type = SolidPrimitive.BOX
obj_shape.dimensions = list(dim)
co.primitives.append(obj_shape)
obj_pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1))
co.primitive_poses = [obj_pose]
self.pub_co.publish(co)
def move_object(self, name, pose):
co = CollisionObject()
co.operation = CollisionObject.MOVE
co.id = name
co.header = pose.header
co.primitive_poses = [pose.pose]
#sphere.dimensions = [radius] # future
self.__submit(co, attach=False)
def __make_mesh_custom(self, name, pose, mesh):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
return co
def __init__(self, speed):
self.speed = speed
# Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the robot
self.robot = moveit_commander.RobotCommander()
# Initialize the planning scene
self.scene = moveit_commander.PlanningSceneInterface()
self.scene_publisher = rospy.Publisher('/collision_object',
CollisionObject,
queue_size=10)
# Instantiate a move group
self.group = moveit_commander.MoveGroupCommander(GROUP_NAME)
# Set the maximum time MoveIt will try to plan before giving up
self.group.set_planning_time(.25)
# Set maximum velocity scaling
self.group.set_max_velocity_scaling_factor(1.0)
self.group.set_max_acceleration_scaling_factor(1.0)
# For displaying plans to RVIZ
self.display_pub = rospy.Publisher('/move_group/display_planned_path',
DisplayTrajectory,
queue_size=10)
print(self.group.get_current_pose())
print(self.group.get_end_effector_link())
print(self.group.get_pose_reference_frame())
print(self.group.get_goal_tolerance())
# Create a CollisionObject, which will be added to the planning scene
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = 'table'
co.header = TABLE_CENTER.header
# Create a box primitive, which will be inside the CollisionObject
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = TABLE_SIZE
# Fill the collision object with primitive(s)
co.primitives = [box]
co.primitive_poses = [TABLE_CENTER.pose]
# Publish the object (don't know why but need to publish twice)
self.scene_publisher.publish(co)
rospy.sleep(0.5)
self.scene_publisher.publish(co)
rospy.sleep(0.5)
def __make_cylinder(name, pose, height, radius):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = [height, radius]
co.primitives = [cylinder]
co.primitive_poses = [pose.pose]
return co
def __make_box(self, name, pose, size):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list(size)
co.primitives = [box]
co.primitive_poses = [pose.pose]
return co
def __make_cylinder(name, pose, height, radius):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = [height, radius]
co.primitives = [cylinder]
co.primitive_poses = [pose.pose]
return co
def __make_box(self, name, pose, size):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list(size)
co.primitives = [box]
co.primitive_poses = [pose.pose]
return co
def __make_sphere(self, name, pose, radius):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [radius]
co.primitives = [sphere]
co.primitive_poses = [pose.pose]
return co
def __make_sphere(self, name, pose, radius):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [radius]
co.primitives = [sphere]
co.primitive_poses = [pose.pose]
return co
def __make_cylinder(self, name, pose, size):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cyl = SolidPrimitive()
cyl.type = SolidPrimitive.CYLINDER
cyl.dimensions = list(size)
co.primitives = [cyl]
co.primitive_poses = [pose.pose]
return co
def add_plane(self, name, pose, normal=(0, 0, 1), offset=0):
""" Add a plane to the planning scene """
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
p = Plane()
p.coef = list(normal)
p.coef.append(offset)
co.planes = [p]
co.plane_poses = [pose.pose]
self._pub_co.publish(co)
def add_plane(self, name, pose, normal = (0, 0, 1), offset = 0):
""" Add a plane to the planning scene """
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
p = Plane()
p.coef = list(normal)
p.coef.append(offset)
co.planes = [p]
co.plane_poses = [pose.pose]
self._pub_co.publish(co)
def find_block(self):
"""
Find block and add it to the scene
"""
# Get necessary values
while not self.object_location:
print "Waiting for object location..."
rospy.sleep(1.0)
object_location = self.object_location
image_size = self.left_camera_size
while not self.current_pose:
print "Waiting for current pose values..."
rospy.sleep(1.0)
current_pose = self.current_pose.position
endeffector_height = current_pose.z - self.table_height
# Store values for calculation
Pp = [object_location.x, object_location.y, object_location.z]
Cp = [image_size[0] / 2, image_size[1] / 2, 0]
Bp = [current_pose.x, current_pose.y, current_pose.z]
Go = [-0.0230, 0.0110, 0.1100]
cc = [0.0021, 0.0021, 0.0000]
d = [endeffector_height, endeffector_height, 0.0000]
# Calculate block's position in workspace
# workspace = (Pp - Cp) * cc * d + Bp + Go
pixel_difference = map(operator.sub, Pp, Cp)
camera_constant = map(operator.mul, cc, d)
pixel_2_real = map(operator.mul, pixel_difference, camera_constant)
pixel_2_real[2] = endeffector_height * -1
workspace_without_gripper = map(operator.add, pixel_2_real, Bp)
workspace = map(operator.add, workspace_without_gripper, Go)
# Add block to scene
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = workspace_without_gripper[0]
p.pose.position.y = workspace_without_gripper[1]
p.pose.position.z = workspace_without_gripper[2] + 0.06
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "block"
co.header = p.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list((0.1, 0.1, 0.1))
co.primitives = [box]
co.primitive_poses = [p.pose]
pub_co = rospy.Publisher("collision_object", CollisionObject, latch=True)
pub_co.publish(co)
return
def update_collision_box(id,
position,
orientation=(1, 0, 0, 0),
dimensions=(1, 1, 1),
operation=CollisionObject.ADD):
pose = make_pose(position, orientation, robot.get_planning_frame())
co = CollisionObject()
co.operation = operation
co.id = id
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = dimensions
co.primitives = [box]
co.primitive_poses = [pose.pose]
planning_scene_publisher.publish(co)
def __make_mesh(name, pose, filename, scale=(1, 1, 1)):
co = CollisionObject()
if pyassimp is False:
raise MoveItCommanderException(
"Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt"
)
scene = pyassimp.load(filename)
if not scene.meshes or len(scene.meshes) == 0:
raise MoveItCommanderException("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise MoveItCommanderException("There are no faces in the mesh")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, "__len__"):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, "indices"):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [
face.indices[0],
face.indices[1],
face.indices[2],
]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException(
"Unable to build triangles from mesh due to mesh object structure"
)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def publishResults(self, result):
for i, obj in enumerate(result.scene.objects):
id = "%s%d" % (obj.name, i)
dx, dy, dz = (0.1, ) * 3 # determine from model
collision_object = CollisionObject()
collision_object.id = id
collision_object.header = self.lastHeader
object_shape = SolidPrimitive()
object_shape.type = SolidPrimitive.BOX
object_shape.dimensions.append(dx)
object_shape.dimensions.append(dy)
object_shape.dimensions.append(dz)
collision_object.primitives.append(object_shape)
shape_pose = obj.pose
collision_object.primitive_poses.append(shape_pose)
collision_object.operation = CollisionObject.ADD
self.pub_collision.publish(collision_object)
def add_cylinder(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
# Cylinders are special - they are not supported directly by
# moveit_commander. It must be published manually to collision scene
cyl = CollisionObject()
cyl.operation = CollisionObject.ADD
cyl.id = object_id
cyl.header = stamped_pose.header
prim = SolidPrimitive()
prim.type = SolidPrimitive.CYLINDER
prim.dimensions = [size[0], size[1]]
cyl.primitives = [prim]
cyl.primitive_poses = [stamped_pose.pose]
# self.scene_pub.publish(cyl)
marker = self.make_marker_stub(stamped_pose,
[size[1] * 2, size[2] * 2, size[0]],
color=color)
marker.type = Marker.CYLINDER
self.publish_marker(object_id, marker)
sleep(0.5)
logdebug("Loaded " + object_id +
" as cylindrical collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def __make_mesh(name, pose, filename, scale=(1, 1, 1)):
co = CollisionObject()
if pyassimp is False:
raise MoveItCommanderException(
"Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt")
scene = pyassimp.load(filename)
if not scene.meshes or len(scene.meshes) == 0:
raise MoveItCommanderException("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise MoveItCommanderException("There are no faces in the mesh")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, '__len__'):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, 'indices'):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face.indices[0],
face.indices[1],
face.indices[2]]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure")
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def attach_sphere(link, name, pose, radius, touch_links=[]):
aco = AttachedCollisionObject()
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [radius]
co.primitives = [sphere]
co.primitive_poses = [pose.pose]
aco.object = co
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
scene._pub_aco.publish(aco)
def attach_cylinder(link, name, pose, height, radius, touch_links=[]):
aco = AttachedCollisionObject()
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = [height, radius]
co.primitives = [cylinder]
co.primitive_poses = [pose.pose]
aco.object = co
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
scene._pub_aco.publish(aco)
def add_obstacle(position, operation=CollisionObject.ADD):
planning_scene_publisher = rospy.Publisher('/collision_object',
CollisionObject,
queue_size=10)
scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
rospy.sleep(2)
pose = make_pose(position, [1, 0, 0, 0], robot.get_planning_frame())
co = CollisionObject()
co.operation = operation
co.id = "obstacle"
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = (0.5, .7, 0.1)
co.primitives = [box]
co.primitive_poses = [pose.pose]
planning_scene_publisher.publish(co)
def add_arbitrary_obstacle(size, id, pose, planning_scene_publisher, scene,
robot, operation):
"""
Adds an arbitrary rectangular prism obstacle to the planning scene.
This is currently only for the ground plane
size: numpy array of size 3 (x,y,z dimensions)
id: string (object id/name)
pose: PoseStamped objecct (objects pose with respect to world frame)
planning_scene_publisher: ros Publisher('/collision_object', CollisionObject)
scene: PlanningSceneInterface
robot: RobotCommander
operation: currently just use CollisionObject.ADD
"""
co = CollisionObject()
co.operation = operation
co.id = id
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = size
co.primitives = [box]
co.primitive_poses = [pose.pose]
planning_scene_publisher.publish(co)
"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())
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]
scene = moveit_commander.PlanningSceneInterface()
pub = rospy.Publisher('/collision_object', CollisionObject, queue_size=10)
TABLE_CENTER = PoseStamped()
TABLE_CENTER.header.frame_id = 'world'
TABLE_CENTER.pose = Pose(position=Point(0, -1.37, .76))
TABLE_CENTER.pose.orientation.w = 1
TABLE_SIZE = [1.525, 2.74, 0.0201]
# Create a CollisionObject, which will be added to the planning scene
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = 'table'
co.header = TABLE_CENTER.header
# Create a box primitive, which will be inside the CollisionObject
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = TABLE_SIZE
# Fill the collision object with primitive(s)
co.primitives = [box]
co.primitive_poses = [TABLE_CENTER.pose]
print(co)
count = 0
while not rospy.is_shutdown():
pub.publish(co)
def __init__(self):
"""
Initialize class
"""
# Overall MoveIt initialization
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
# MoveIt scene initialization
self.scene = moveit_commander.PlanningSceneInterface()
self.table_height = -0.320
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0.8
p.pose.position.y = 0.025
p.pose.position.z = -0.6
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "table"
co.header = p.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list((0.75, 1.25, 0.55))
co.primitives = [box]
co.primitive_poses = [p.pose]
pub_co = rospy.Publisher("collision_object", CollisionObject, latch=True)
pub_co.publish(co)
# Moveit group initialization
self.group = moveit_commander.MoveGroupCommander("left_arm")
self.group.set_goal_position_tolerance(0.12)
self.group.set_goal_orientation_tolerance(0.10)
# Gripper initialization
self.left_gripper = baxter_interface.Gripper("left", CHECK_VERSION)
self.left_gripper.reboot()
self.left_gripper.calibrate()
# Camera initialization
self.left_camera = baxter_interface.CameraController("left_hand_camera")
self.right_camera = baxter_interface.CameraController("right_hand_camera")
self.head_camera = baxter_interface.CameraController("head_camera")
self.right_camera.close()
self.head_camera.close()
self.left_camera.open()
self.left_camera.resolution = [1280, 800]
self.left_camera_size = [800, 1280]
_camera_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self._on_camera)
# Open CV initialization
cv2.namedWindow("Original", cv2.WINDOW_NORMAL)
cv2.moveWindow("Original", 1990, 30)
cv2.namedWindow("Thresholded", cv2.WINDOW_NORMAL)
cv2.moveWindow("Thresholded", 3270, 30)
self.low_h = 165
self.high_h = 179
self.low_s = 70
self.high_s = 255
self.low_v = 60
self.high_v = 255
self.bridge = CvBridge()
# Object location initialization
self.object_location = None
# Subscribe to pose at all times
self.current_pose = None
_pose_sub = rospy.Subscriber("/robot/limb/left/endpoint_state", EndpointState, self.set_current_pose)
return
