void Update()
{
for (int i = 0; i < numCars; i++)
{
GameObject car = cars[i];
GameObject rosInterface = rosInterfaces[i];
RosConnector rosConnector = rosInterface.GetComponent <RosConnector>();
bool connected = rosConnector.connectionEstablished ? true : false;
PoseStampedSubscriber poseStampedSubscriber = rosInterface.GetComponent <PoseStampedSubscriber>();
float x = poseStampedSubscriber.position.x * waypointScaling + 24;
float z = poseStampedSubscriber.position.z * waypointScaling;
float y = wheelOffset;
Vector3 carPos = new Vector3(x, y, z);
car.transform.position = carPos;
car.transform.rotation = poseStampedSubscriber.rotation;
Debug.Log(string.Format("Car {0} pos: {1}", i, carPos));
numSamePos[i] = prevPos[i] == poseStampedSubscriber.position ? numSamePos[i] + 1 : 0;
prevPos[i] = poseStampedSubscriber.position;
bool isCarActive = numSamePos[i] > 50 ? false : true;
car.SetActive(isCarActive);
Debug.Log(string.Format("Car {0} numSamePos: {1}", i, numSamePos[i]));
Debug.Log(string.Format("Car {0} pos: {1}", i, poseStampedSubscriber.position));
Debug.Log(string.Format("Car {0} prevPos: {1}", i, prevPos[i]));
}
}
void Start()
{
// Create car objects and subscriber/rosconnector for each car
cars = new List <GameObject>();
rosInterfaces = new List <GameObject>();
numSamePos = new List <int>();
prevPos = new List <Vector3>();
for (int i = 0; i < numCars; i++)
{
GameObject car = Instantiate(Resources.Load("Prefabs/Car_" + i)) as GameObject;
car.name = "Car_" + i;
car.transform.localScale = new Vector3(2.5f, 2.5f, 2.5f);
car.transform.position = new Vector3(0f, wheelOffset, 0f);
car.SetActive(false);
BoxCollider boxCollider = car.AddComponent(typeof(BoxCollider)) as BoxCollider;
if (egoCarNum == i)
{
egoCarGoal = new GameObject();
egoCarGoal.transform.position = initPosition;
PoseStampedPublisher poseStampedPublisher = egoCarGoal.AddComponent(typeof(PoseStampedPublisher)) as PoseStampedPublisher;
poseStampedPublisher.Topic = "/car/" + i + "/nav/goal";
poseStampedPublisher.PublishedTransform = egoCarGoal.transform;
EgoCarEvaluationInterface evaluationInterface = car.AddComponent(typeof(EgoCarEvaluationInterface)) as EgoCarEvaluationInterface;
LineRenderer lineRenderer = car.AddComponent(typeof(LineRenderer)) as LineRenderer;
Rigidbody rigidBody = car.AddComponent(typeof(Rigidbody)) as Rigidbody;
car.GetComponent <Rigidbody>().isKinematic = true;
}
car.SetActive(true);
cars.Add(car);
prevPos.Add(car.transform.position);
numSamePos.Add(0);
GameObject rosInterface = new GameObject();
rosInterface.name = "RosInterface" + i;
rosInterface.transform.position = new Vector3(0f, 0f, 0f);
rosInterface.SetActive(false);
RosConnector rosConnector = rosInterface.AddComponent(typeof(RosConnector)) as RosConnector;
rosConnector.Protocol = RosConnector.Protocols.WebSocketSharp;
rosConnector.Timeout = 10;
rosConnector.RosBridgeServerUrl = url;
rosConnector.Awake();
PoseStampedSubscriber poseStampedSubscriber = rosInterface.AddComponent(typeof(PoseStampedSubscriber)) as PoseStampedSubscriber;
poseStampedSubscriber.TimeStep = 0.1f;
poseStampedSubscriber.Topic = "/car/" + i + "/pose";
poseStampedSubscriber.PublishedTransform = rosInterface.transform;
poseStampedSubscriber.Awake();
rosInterface.SetActive(true);
rosInterfaces.Add(rosInterface);
}
}
// Start is called before the first frame update.
// Starting coroutine and declerating variables.
void Start()
{
changed = 0;
robot_pose_sub = GameObject.Find("RosBridge").GetComponent <PoseStampedSubscriber>();
Debug.Log("Starting to update position");
StartCoroutine("timer");
}
// Start is called before the first frame update.
// Starting coroutine and declerating variables.
void Start()
{
changed = 0;
robot_pose_sub = GameObject.Find("RosBridge").GetComponent <PoseStampedSubscriber>();
input = GameObject.Find("Input").GetComponent <VRInput>();
info = this.GetComponent <RobotInformation>();
StartCoroutine("timer");
Debug.Log("Roboter Control: Starting to update position of robot");
}
// Start is called before the first frame update.
// Sets robot and player into simulation scene and activates all scripts that are attached to the rosbridge.
void Start()
{
rosBridge = GameObject.Find("RosBridge");
foreach (MonoBehaviour script in rosBridge.GetComponents <MonoBehaviour>())
{
script.enabled = true;
}
robot_pose_sub = rosBridge.GetComponent <PoseStampedSubscriber>();
manager = GameObject.Find("SceneManagement").GetComponent <SceneManagement>();
GameObject robotToLoad = manager.simRobot != null ? manager.simRobot : null;
// Sets robot into scene.
if (robotToLoad != null)
{
robotToLoad = Instantiate(robotToLoad, robot_pose_sub.position, robot_pose_sub.rotation);
}
SetupSimulation(robotToLoad, robotToLoad.GetComponent <RobotInformation>().robotType);
// Sets player into scene.
Vector3 offset = -(Player.instance.feetPositionGuess - Player.instance.trackingOriginTransform.position);
Player.instance.trackingOriginTransform.position = robotToLoad.transform.position + offset + offsetFromRobot;
Player.instance.transform.GetChild(0).RotateAround(Player.instance.hmdTransform.position, Vector3.up, angle);
// Sets Objects used for teleporting.
GameObject.Find("Teleporting").transform.position = Player.instance.trackingOriginTransform.position;
GameObject.Find("TeleportingGround").transform.position = Player.instance.trackingOriginTransform.position + offset;
// Used for updating joints (works for telemax only until now).
rosBridge.AddComponent <JointStateSubscriberMod>();
Debug.Log("Jointsub added");
}
// Start is called before the first frame update.
void Start()
{
poseStampedSubscriber = GameObject.Find("RosBridge").GetComponent <PoseStampedSubscriber>();
player = Player.instance;
}
