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)
