def make_model_state_msg(model_name=None, pose=None, scale=None, twist=None, reference_frame=None): """ ModelState messages factory """ msg = ModelState() msg.model_name = model_name if (model_name is not None) else 'model_name' if pose is None: pose = Pose() pose.position = Point(0, 0, 0) pose.orientation = Quaternion(0, 0, 0, 1) msg.pose = pose msg.scale = scale if (scale is not None) else Vector3(1, 1, 1) msg.twist = twist if (twist is not None) else Twist( Vector3(0, 0, 0), Vector3(0, 0, 0)) if reference_frame is None or reference_frame == '': msg.reference_frame = 'world' else: msg.reference_frame = reference_frame return msg
def reset_object_pose(object_name, pose): """ Resets the give object's name to the new given pose """ current_model_state = get_state_srv.call(object_name, "world") new_model_state = ModelState(model_name=object_name, pose=pose) new_model_state.scale = current_model_state.scale new_model_state.twist = current_model_state.twist new_model_state.reference_frame = 'world' set_state_srv.call(new_model_state)