def set_object(self, object_name, object_pose=[0.0, 1.0, 0.8, 1.0, 0.0, 0.0, 0.0]):
object = Object()
object.name = object_name
object.pose.header.stamp = rospy.Time.now()
object.pose.header.frame_id = "/BASE"
object.pose.pose.pose.position.x = object_pose[0]
object.pose.pose.pose.position.y = object_pose[1]
object.pose.pose.pose.position.z = object_pose[2]
object.pose.pose.pose.orientation.w = object_pose[3]
object.pose.pose.pose.orientation.x = object_pose[4]
object.pose.pose.pose.orientation.y = object_pose[5]
object.pose.pose.pose.orientation.z = object_pose[6]
self.add_object(object)
def set_object_pose(object_name, x, y, z, qw, qx, qy, qz, real_object_name=""):
if real_object_name == "":
real_object_name = object_name
global object_poses
global world_state
obj = Object()
obj.name = real_object_name
obj.pose.header.frame_id = "/BASE"
obj.pose.pose.pose.position.x = x
obj.pose.pose.pose.position.y = y
obj.pose.pose.pose.position.z = z
obj.pose.pose.pose.orientation.w = qw
obj.pose.pose.pose.orientation.x = qx
obj.pose.pose.pose.orientation.y = qy
obj.pose.pose.pose.orientation.z = qz
world_state.add_object(obj)
def remove_object(self, object_name):
obj = Object()
obj.name = object_name
self.publish_object(obj, ObjectState.REMOVE)
return True
