def to_collision_object_msg(self, action):
"""
Creates a ROS collision object message from this instance
Parameters
----------
action : moveit_msg/CollisionObject operation
Defines which action/opertation should be performed
(add, remove, append, move)
Returns
-------
moveit_msgs/CollisionObject
"""
msg = moveit_msgs.CollisionObject()
msg.header.frame_id = self.__frame_id
planes = []
plane_poses = []
primitives = []
primitive_poses = []
for p in self.__primitives:
if p.kind is CollisionPrimitiveKind.plane:
plane_poses.append(p.pose.to_pose_msg())
planes.append(p.to_shape_msg())
else:
primitive_poses.append(p.pose.to_pose_msg())
primitives.append(p.to_shape_msg())
msg.planes = planes
msg.plane_poses = plane_poses
msg.primitives = primitives
msg.primitive_poses = primitive_poses
msg.meshes = []
msg.mesh_poses = []
msg.operation = action
return msg
def BasicMicoEnvironment():
## all objects in one message
env_objects = moveit_msgs.CollisionObject()
env_objects.header.stamp = rospy.Time.now()
env_objects.header.frame_id = "/world"
env_objects.id = "table_camera_user"
#table
# table = shape_msgs.SolidPrimitive()
# table.type = shape_msgs.SolidPrimitive.BOX
# table.dimensions.append(2.0)
# y = 1.2
# table.dimensions.append(y)
# table.dimensions.append(0.05)
# table_pose = geometry_msgs.Pose()
# table_pose.position.y = - y/2.0
# table_pose.position.z = - 0.03
# table_pose.orientation.w = 1.0
#camera
camera = shape_msgs.SolidPrimitive()
camera.type = shape_msgs.SolidPrimitive.BOX
camera.dimensions.append(0.3)
camera.dimensions.append(0.08)
camera.dimensions.append(0.06)
camera_pose = geometry_msgs.Pose()
camera_pose.position.x = 0.581
camera_pose.position.y = 0.065
camera_pose.position.z = 0.05 / 2.0 + 0.376
camera_pose.orientation.z = 0.3827
camera_pose.orientation.w = -0.9239
#camera pole
camera_pole = shape_msgs.SolidPrimitive()
camera_pole.type = shape_msgs.SolidPrimitive.CYLINDER
camera_pole.dimensions.append(0.376) #height
camera_pole.dimensions.append(0.02) #radius
camera_pole_pose = geometry_msgs.Pose()
camera_pole_pose.position.x = 0.581
camera_pole_pose.position.y = 0.065
camera_pole_pose.position.z = 0.376 / 2.0
camera_pole_pose.orientation.w = 1.0
#user area
user_area = shape_msgs.SolidPrimitive()
user_area.type = shape_msgs.SolidPrimitive.BOX
user_area.dimensions.append(1.2)
user_area.dimensions.append(0.05)
user_area.dimensions.append(0.6)
user_area_pose = geometry_msgs.Pose()
user_area_pose.position.x = 0.0
user_area_pose.position.y = 0.3
user_area_pose.position.z = 0.3
user_area_pose.orientation.w = 1.0
# env_objects.primitives.append(table) #one can also append more objects under the same id
env_objects.primitives.append(camera)
env_objects.primitives.append(camera_pole)
env_objects.primitives.append(user_area)
# env_objects.primitive_poses.append(table_pose)
env_objects.primitive_poses.append(camera_pose)
env_objects.primitive_poses.append(camera_pole_pose)
env_objects.primitive_poses.append(user_area_pose)
env_objects.operation = moveit_msgs.CollisionObject.ADD
pub_coll_obj = rospy.Publisher('/collision_object',
moveit_msgs.CollisionObject,
queue_size=1)
count = 0
rate = rospy.Rate(50)
while not rospy.is_shutdown() and count < 100:
pub_coll_obj.publish(env_objects)
count += 1
rate.sleep()
def main():
rospy.init_node('add_collison_objects')
parser = argparse.ArgumentParser(
description=
'Attach an object from the household objects database to the robot in the MoveIt planning scene'
)
parser.add_argument('--detach',
dest='detach',
action='store_true',
default=False,
help='Detach all objects from the robot')
args = parser.parse_args()
pub_att_col_obj = rospy.Publisher(ATTACHED_COLLISION_OBJECT_TOPIC,
moveit_msgs.AttachedCollisionObject,
latch=True)
while (pub_att_col_obj.get_num_connections() <
1) and not rospy.is_shutdown():
rospy.loginfo("Waiting for planning scene listening to '" +
str(ATTACHED_COLLISION_OBJECT_TOPIC) + "' ...")
rospy.sleep(0.5)
att_col_obj = moveit_msgs.AttachedCollisionObject()
att_col_obj.object.header.stamp = rospy.Time.now()
att_col_obj.link_name = "palm_link"
if args.detach:
att_col_obj.object.operation = moveit_msgs.CollisionObject.REMOVE
rospy.loginfo('Detaching objects ...')
else:
rospy.loginfo("Waiting for '" + str(OBJECT_INFORMATION_TOPIC) +
"' service ... ")
rospy.wait_for_service(OBJECT_INFORMATION_TOPIC)
rospy.loginfo("'" + str(OBJECT_INFORMATION_TOPIC) +
"' service available.")
info_response = object_recognition_srvs.GetObjectInformationResponse()
srv_client = rospy.ServiceProxy(
OBJECT_INFORMATION_TOPIC,
object_recognition_srvs.GetObjectInformation())
try:
info_request = object_recognition_srvs.GetObjectInformationRequest(
)
info_request.type.key = "18807" # Coke can
info_request.type.db = '{"host":"localhost","module":"object_recognition_tabletop","name":"household_objects-0.6","password":"******","port":"5432","type":"ObjectDbSqlHousehold","user":"******"'
info_response = srv_client.call(info_request)
except rospy.ServiceException, e:
rospy.logerr("Service did not process request: " + str(e))
return
rospy.loginfo("Object information retrieved.")
''' prepare collision object message'''
collision_object = moveit_msgs.CollisionObject()
collision_object.header.stamp = rospy.Time.now()
collision_object.header.frame_id = "/base_footprint"
collision_object.id = info_response.information.name
collision_object.type = info_request.type
object_shape = shape_msgs.msg.SolidPrimitive()
object_shape.type = shape_msgs.msg.SolidPrimitive.CYLINDER
object_shape.dimensions.append(0.1) # CYLINDER_HEIGHT
object_shape.dimensions.append(0.05) # CYLINDER_RADIUS
collision_object.primitives.append(object_shape)
shape_pose = geometry_msgs.msg.Pose()
shape_pose.position.x = 1.0
shape_pose.position.y = 0.0
shape_pose.position.z = 0.0 + object_shape.dimensions[
0] / 2 # reference is at the centre of the mesh
shape_pose.orientation.w = 1.0
collision_object.primitive_poses.append(shape_pose)
collision_object.meshes.append(
info_response.information.ground_truth_mesh)
mesh_pose = geometry_msgs.msg.Pose()
mesh_pose.position.x = 1.0
mesh_pose.position.y = 0.0
mesh_pose.position.z = 0.0 # reference is at the bottom of the mesh
mesh_pose.orientation = shape_pose.orientation
collision_object.mesh_poses.append(mesh_pose)
collision_object.operation = moveit_msgs.CollisionObject.ADD
att_col_obj.object = collision_object
att_col_obj.touch_links.append("lower_arm_link")
att_col_obj.touch_links.append("wrist_link")
att_col_obj.touch_links.append("palm_link")
att_col_obj.touch_links.append("gripper_link")
att_col_obj.touch_links.append("gripper_center_link")
att_col_obj.touch_links.append("gripper_camera_link")
att_col_obj.touch_links.append("finger_left_knuckle_1_link")
att_col_obj.touch_links.append("finger_left_knuckle_2_link")
att_col_obj.touch_links.append("finger_right_knuckle_1_link")
att_col_obj.touch_links.append("finger_right_knuckle_2_link")
rospy.loginfo('Attaching object ...')
def main():
rospy.init_node('add_collison_objects')
parser = argparse.ArgumentParser(description='Adds an object from household objects database to the MoveIt planning scene')
parser.add_argument('pos_x', type = float, help='X value of the object position')
parser.add_argument('pos_y', type = float, help='Y value of the object position')
parser.add_argument('pos_z', type = float, help='Z value of the object position')
parser.add_argument('--remove', dest='remove', action='store_true', default=False, help='Remove all objects from the scene')
args=parser.parse_args()
pub_collision_object = rospy.Publisher(COLLISION_OBJECT_TOPIC,
moveit_msgs.CollisionObject,
latch = True)
while (pub_collision_object.get_num_connections() < 1) and not rospy.is_shutdown():
rospy.loginfo("Waiting for planning scene listening to '" + str(COLLISION_OBJECT_TOPIC) + "' ...")
rospy.sleep(0.5)
collision_object = moveit_msgs.CollisionObject()
collision_object.header.stamp = rospy.Time.now()
collision_object.header.frame_id = "/base_footprint";
if args.remove:
collision_object.operation = moveit_msgs.CollisionObject.REMOVE
rospy.loginfo('Removing object ...')
else:
rospy.loginfo("Waiting for '" + str(OBJECT_INFORMATION_TOPIC) + "' service ... ")
rospy.wait_for_service(OBJECT_INFORMATION_TOPIC)
rospy.loginfo("'" + str(OBJECT_INFORMATION_TOPIC) + "' service available.")
info_response = object_recognition_srvs.GetObjectInformationResponse()
srv_client = rospy.ServiceProxy(OBJECT_INFORMATION_TOPIC, object_recognition_srvs.GetObjectInformation())
try:
info_request = object_recognition_srvs.GetObjectInformationRequest()
info_request.type.key = "18807" # Coke can
info_request.type.db = '{"host":"localhost","module":"object_recognition_tabletop","name":"household_objects-0.6","password":"******","port":"5432","type":"ObjectDbSqlHousehold","user":"******"'
info_response = srv_client.call(info_request)
except rospy.ServiceException, e:
rospy.logerr("Service did not process request: " + str(e))
return
rospy.loginfo("Object information retrieved.")
''' prepare collision object message'''
collision_object.id = info_response.information.name
collision_object.type = info_request.type
''' add a solid primitive shape '''
object_shape = shape_msgs.msg.SolidPrimitive()
object_shape.type = shape_msgs.msg.SolidPrimitive.CYLINDER
object_shape.dimensions.append(0.1) # CYLINDER_HEIGHT
object_shape.dimensions.append(0.05) # CYLINDER_RADIUS
collision_object.primitives.append(object_shape)
shape_pose = geometry_msgs.msg.Pose()
shape_pose.position.x = args.pos_x
shape_pose.position.y = args.pos_y
shape_pose.position.z = args.pos_z + object_shape.dimensions[0] / 2 # reference is at the centre of the mesh
shape_pose.orientation.w = 1.0
collision_object.primitive_poses.append(shape_pose)
''' add a mesh from the household objects database '''
collision_object.meshes.append(info_response.information.ground_truth_mesh)
mesh_pose = geometry_msgs.msg.Pose()
mesh_pose.position.x = args.pos_x
mesh_pose.position.y = args.pos_y
mesh_pose.position.z = args.pos_z # reference is at the bottom of the mesh
mesh_pose.orientation.w = 1.0
collision_object.mesh_poses.append(mesh_pose)
collision_object.operation = moveit_msgs.CollisionObject.ADD
rospy.loginfo('Adding object ...')
def execute(self, userdata):
max_confidence = 0.0
object_index = 0
if len(userdata.recognised_objects.objects) is not len(
userdata.recognised_object_names):
rospy.logerr(
"Number of recognised objects does not match object names!")
return 'error'
for object in userdata.recognised_objects.objects:
if object.confidence > max_confidence:
max_confidence = object.confidence
object_index = userdata.recognised_objects.objects.index(
object)
userdata.pick_object_name = userdata.recognised_object_names[
object_index]
userdata.pick_object_pose = geometry_msgs.PoseStamped()
userdata.pick_object_pose.header = userdata.recognised_objects.objects[
object_index].pose.header
userdata.pick_object_pose.pose = userdata.recognised_objects.objects[
object_index].pose.pose.pose
pick_collision_object = moveit_msgs.CollisionObject()
pick_collision_object.header = userdata.pick_object_pose.header
pick_collision_object.id = userdata.pick_object_name
pick_collision_object.type = userdata.recognised_objects.objects[
object_index].type
shape = shape_msgs.SolidPrimitive()
shape.type = shape_msgs.SolidPrimitive.CYLINDER
shape.dimensions.append(0.20) # CYLINDER_HEIGHT
shape.dimensions.append(0.05) # CYLINDER_RADIUS
shape_pose = geometry_msgs.Pose()
shape_pose.position.x = 0.15
quat = tf.transformations.quaternion_from_euler(-math.pi / 2, 0.0, 0.0)
shape_pose.orientation = geometry_msgs.Quaternion(*quat)
pick_collision_object.primitives.append(shape)
pick_collision_object.primitive_poses.append(shape_pose)
pick_collision_object.meshes.append(
userdata.objects_info[object_index].information.ground_truth_mesh)
pick_collision_object.mesh_poses.append(userdata.pick_object_pose.pose)
userdata.pick_collision_object = pick_collision_object
rospy.loginfo("Collision object:")
rospy.loginfo(pick_collision_object.header)
rospy.loginfo(pick_collision_object.primitive_poses)
rospy.loginfo(pick_collision_object.mesh_poses)
rospy.loginfo("Object '" + str(userdata.pick_object_name) +
"' has been selected among all recognised objects")
rospy.loginfo("Object's pose:")
rospy.loginfo(userdata.pick_object_pose)
angle = math.atan2(userdata.pick_object_pose.pose.position.y,
userdata.pick_object_pose.pose.position.x)
dist = math.sqrt(
math.pow(userdata.pick_object_pose.pose.position.x, 2) +
math.pow(userdata.pick_object_pose.pose.position.y, 2))
userdata.pick_object_pose.pose.position.x = dist * math.cos(angle)
userdata.pick_object_pose.pose.position.y = dist * math.sin(angle)
userdata.pick_object_pose.pose.position.z = userdata.pick_object_pose.pose.position.z
yaw = angle
roll = math.pi / 2
pitch = 0.0
quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
userdata.pick_object_pose.pose.orientation = geometry_msgs.Quaternion(
*quat)
# rospy.loginfo("Object's pose after adaption:")
# rospy.loginfo(userdata.pick_object_pose)
return 'prepared'
def main():
rospy.init_node('add_collision_objects')
parser = argparse.ArgumentParser(
description='Add objects into the MoveIt planning scene')
parser.add_argument('--clean',
dest='clean',
action='store_true',
default=False,
help='Remove all objects from the scene')
args = parser.parse_args()
pub_collision_object = rospy.Publisher("/korus/collision_object",
moveit_msgs.CollisionObject,
latch=True)
rospy.sleep(0.5)
''' publish table '''
collision_object = moveit_msgs.CollisionObject()
collision_object.header.stamp = rospy.Time.now()
collision_object.header.frame_id = "/base_footprint"
collision_object.id = "table"
# collision_object.type =
object_shape = shape_msgs.msg.SolidPrimitive()
object_shape.type = shape_msgs.msg.SolidPrimitive.BOX
object_shape.dimensions.append(0.6) # BOX_X
object_shape.dimensions.append(1.0) # BOX_Y
object_shape.dimensions.append(0.65) # BOX_Z
# object_shape.dimensions.append(0.4) # BOX_Z
collision_object.primitives.append(object_shape)
object_pose = geometry_msgs.msg.Pose()
object_pose.position.x = 1
object_pose.position.y = 0.0
object_pose.position.z = 0.325
# object_pose.position.z = 0.2
object_pose.orientation.w = 1.0
collision_object.primitive_poses.append(object_pose)
if args.clean:
collision_object.operation = moveit_msgs.CollisionObject.REMOVE
rospy.loginfo('Removing table ...')
else:
collision_object.operation = moveit_msgs.CollisionObject.ADD
rospy.loginfo('Adding table ...')
pub_collision_object.publish(collision_object)
rospy.sleep(0.5)
''' publish object '''
collision_object = moveit_msgs.CollisionObject()
collision_object.header.stamp = rospy.Time.now()
collision_object.header.frame_id = "/base_footprint"
collision_object.id = "pickup_object"
# collision_object.type =
object_shape = shape_msgs.msg.SolidPrimitive()
object_shape.type = shape_msgs.msg.SolidPrimitive.CYLINDER
object_shape.dimensions.append(0.20) # CYLINDER_HEIGHT
object_shape.dimensions.append(0.025) # CYLINDER_RADIUS
collision_object.primitives.append(object_shape)
object_pose = geometry_msgs.msg.Pose()
object_pose.position.x = 0.8
object_pose.position.y = 0.0
object_pose.position.z = 0.77
object_pose.orientation.w = 1.0
collision_object.primitive_poses.append(object_pose)
if args.clean:
collision_object.operation = moveit_msgs.CollisionObject.REMOVE
rospy.loginfo('Removing object ...')
else:
collision_object.operation = moveit_msgs.CollisionObject.ADD
rospy.loginfo('Adding object ...')
pub_collision_object.publish(collision_object)
# rospy.spin()
return
def rave_env_to_ros(env):
pub = make_planning_scene_pub()
ps = mm.PlanningScene()
with open(osp.join(pbc.miscdata_dir, "planning_scene_prototype.yaml"),
"r") as fh:
d = yaml.load(fh)
rm.fill_message_args(ps, d)
cos = ps.world.collision_objects
for body in env.GetBodies():
if body.IsRobot():
rstate = ps.robot_state
rave_joint_vals = body.GetDOFValues()
rave_joint_names = [joint.GetName() for joint in body.GetJoints()]
ros_joint_names = []
ros_joint_values = []
for name in ROS_JOINT_NAMES:
if name in rave_joint_names:
i = rave_joint_names.index(name)
ros_joint_values.append(rave_joint_vals[i])
ros_joint_names.append(name)
rstate.joint_state.position = ros_joint_values
rstate.joint_state.name = ros_joint_names
rstate.multi_dof_joint_state.header.frame_id = 'odom_combined'
rstate.multi_dof_joint_state.header.stamp = rospy.Time.now()
rstate.multi_dof_joint_state.joint_names = ['world_joint']
rstate.multi_dof_joint_state.joint_transforms = [
RosTransformFromRaveMatrix(body.GetTransform())
]
else:
for link in body.GetLinks():
co = mm.CollisionObject()
co.operation = co.ADD
co.id = body.GetName()
co.header.frame_id = "odom_combined"
co.header.stamp = rospy.Time.now()
for geom in link.GetGeometries():
trans = RosPoseFromRaveMatrix(body.GetTransform().dot(
link.GetTransform().dot(geom.GetTransform())))
if geom.GetType() == openravepy.GeometryType.Trimesh:
mesh = sm.Mesh()
rave_mesh = geom.GetCollisionMesh()
for pt in rave_mesh.vertices:
mesh.vertices.append(gm.Point(*pt))
for tri in rave_mesh.indices:
mt = sm.MeshTriangle()
mt.vertex_indices = tri
mesh.triangles.append(mt)
co.meshes.append(mesh)
co.mesh_poses.append(trans)
else:
shape = sm.SolidPrimitive()
shape.type = shape.BOX
shape.dimensions = geom.GetBoxExtents() * 2
co.primitives.append(shape)
co.primitive_poses.append(trans)
cos.append(co)
pub.publish(ps)
return ps