public void UpdatePrimitive(CollisionPrimitiveMsg msg)
{
//don't update primitive if new msg is same as old one
if (!collisionPrimitiveMsg.ToYAMLString().Equals(msg.ToYAMLString()))
{
collisionPrimitiveMsg = msg;
UpdateExistingPrimitive();
}
}
private IEnumerator WaitAndPublishTransformChange()
{
yield return(new WaitForSeconds(2f));
Debug.Log(collisionPrimitiveMsg.GetName() + " TRANSFORM HAS CHANGED");
primitive.transform.hasChanged = false;
CollisionPrimitiveMsg newMsg = new CollisionPrimitiveMsg(collisionPrimitiveMsg.GetName(), collisionPrimitiveMsg.GetSetup(),
new SolidPrimitiveMsg(primitive_type.BOX, new List <float>()
{
primitive.transform.localScale.x, primitive.transform.localScale.y, primitive.transform.localScale.z
}),
new PoseStampedMsg(collisionPrimitiveMsg.GetPose().GetHeader(),
new PoseMsg(new PointMsg(primitive.transform.localPosition.x, -primitive.transform.localPosition.y, primitive.transform.localPosition.z),
new QuaternionMsg(-primitive.transform.localRotation.x, primitive.transform.localRotation.y, -primitive.transform.localRotation.z, primitive.transform.localRotation.w))));
collisionPrimitiveMsg = newMsg;
CollisionEnvironmentManager.Instance.UpdateCollisionPrimitiveMsg(collisionPrimitiveMsg);
//update transform of this object in ROS
ROSCommunicationManager.Instance.ros.CallService(ROSCommunicationManager.addCollisionPrimitiveService, "{\"primitive\": " + newMsg.ToYAMLString() + "}");
lastPosition = primitive.transform.localPosition;
lastScale = primitive.transform.localScale;
lastRotation = primitive.transform.localRotation;
coroutineStarted = false;
yield return(null);
}
public void CreateNewPrimitive(GameObject prefab, Vector3 position)
{
// TODO replace marker to world_frame
CollisionPrimitiveMsg newMsg = new CollisionPrimitiveMsg(GenerateUniqueName(), MainMenuManager.Instance.currentSetup.GetSetupID(),
new SolidPrimitiveMsg(primitive_type.BOX, new List <float>()
{
prefab.transform.localScale.x, prefab.transform.localScale.y, prefab.transform.localScale.z
}),
new PoseStampedMsg(new HeaderMsg(0, new TimeMsg(0, 0), "marker"),
new ROSBridgeLib.geometry_msgs.PoseMsg(new PointMsg(position.x, -position.y, position.z),
new QuaternionMsg(0, 0, 0, 1))));
UpdatePrimitive(newMsg);
ROSCommunicationManager.Instance.ros.CallService(ROSCommunicationManager.addCollisionPrimitiveService, "{\"primitive\": " + newMsg.ToYAMLString() + "}");
}