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 UpdatePrimitiveMsg(CollisionPrimitiveMsg msg)
{
for (int i = 0; i < _primitives.Count; i++)
{
if (_primitives[i].GetName() == msg.GetName())
{
_primitives[i] = msg;
break;
}
}
}
//updates existing primitive or creates new one
private void UpdatePrimitive(CollisionPrimitiveMsg primitiveMsg)
{
bool updated = false;
foreach (GameObject primitive in collisionPrimitives)
{
CollisionPrimitive colPrimitive = primitive.GetComponent <CollisionPrimitive>();
//if primitive exists, then just update it
if (colPrimitive.collisionPrimitiveMsg.GetName().Equals(primitiveMsg.GetName()))
{
colPrimitive.UpdatePrimitive(primitiveMsg);
updated = true;
}
}
//if primitive wasn't updated, meaning it doesn't exists.. so create new one
if (!updated)
{
GameObject new_primitive = new GameObject("CollisionObj-" + primitiveMsg.GetName());
new_primitive.transform.parent = worldAnchor.transform;
new_primitive.transform.localPosition = Vector3.zero;
new_primitive.transform.localEulerAngles = Vector3.zero;
CollisionPrimitive collisionPrimitive = new_primitive.AddComponent <CollisionPrimitive>();
// TODO pick correct prefab due to collision primitive (cube, sphere..)
collisionPrimitive.InitNewPrimitive(primitiveMsg, collisionPrefabs[0]);
new_primitive.SetActive(!environment_hidden);
collisionPrimitives.Add(new_primitive);
// remove AppBarDisplay to not to show the app bar in order to disable manipulation with specified objects
if (nonManipulatableObjects.Contains(primitiveMsg.GetName()))
{
Destroy(new_primitive.GetComponentInChildren <AppBarDisplay>());
}
}
}
