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 UpdatePrimitive(CollisionPrimitiveMsg msg)
{
//don't update primitive if new msg is same as old one
if (!collisionPrimitiveMsg.ToYAMLString().Equals(msg.ToYAMLString()))
{
collisionPrimitiveMsg = msg;
UpdateExistingPrimitive();
}
}
public void UpdatePrimitiveMsg(CollisionPrimitiveMsg msg)
{
for (int i = 0; i < _primitives.Count; i++)
{
if (_primitives[i].GetName() == msg.GetName())
{
_primitives[i] = msg;
break;
}
}
}
private void SetPrimitiveTransform(CollisionPrimitiveMsg msg)
{
primitive.transform.localPosition = new Vector3(msg.GetPose().GetPose().GetPosition().GetX(), -msg.GetPose().GetPose().GetPosition().GetY(),
msg.GetPose().GetPose().GetPosition().GetZ());
primitive.transform.localRotation = new Quaternion(-msg.GetPose().GetPose().GetOrientation().GetX(), msg.GetPose().GetPose().GetOrientation().GetY(),
-msg.GetPose().GetPose().GetOrientation().GetZ(), msg.GetPose().GetPose().GetOrientation().GetW());
primitive.transform.localScale = new Vector3(msg.GetBBox().GetDimesions()[0], msg.GetBBox().GetDimesions()[1],
msg.GetBBox().GetDimesions()[2]);
lastPosition = primitive.transform.localPosition;
lastScale = primitive.transform.localScale;
lastRotation = primitive.transform.localRotation;
paramsSet = true;
//primitive.transform.hasChanged = false;
}
public CollisionPrimitiveMsg GetPrimitiveByName(string name)
{
CollisionPrimitiveMsg searchedPrimitive = null;
foreach (CollisionPrimitiveMsg primitive in _primitives)
{
if (primitive.GetName() == name)
{
searchedPrimitive = primitive;
break;
}
}
return(searchedPrimitive);
}
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() + "}");
}
//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>());
}
}
}
//creates new primitive based on given prefab and position in received CollisionPrimitiveMsg
public void InitNewPrimitive(CollisionPrimitiveMsg msg, GameObject prefab)
{
collisionPrimitiveMsg = msg;
primitivePrefab = prefab;
CreateNewPrimitive();
}
public void UpdateCollisionPrimitiveMsg(CollisionPrimitiveMsg msg)
{
newCollisionObjectsMsg.UpdatePrimitiveMsg(msg);
//SendCollisionObjectsMsgToROS(newCollisionObjectsMsg);
}
