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);
}
}
void Start()
{
string urlPath = Application.persistentDataPath + "/simunity.txt";
Debug.Log($"Reading websocket URL from {urlPath}");
if (!File.Exists(urlPath))
{
using (StreamWriter writer = File.CreateText(urlPath))
{
writer.Write("ws://localhost:9090");
}
}
string url = File.ReadAllText(urlPath);
m_RosConnector = new RosConnector
{
Protocol = RosConnector.Protocols.WebSocketNET,
RosBridgeServerUrl = url
};
m_RosConnector.Awake();
while (m_RosConnector.RosSocket == null)
{
}
m_Socket = m_RosConnector.RosSocket;
m_Socket.Subscribe <ArmMotorCommand>("/arm_control_data", msg =>
{
for (int i = 0; i < 6; i++)
{
Debug.Log($"Velocity output of motor #{i} is {msg.MotorVel[i]}");
}
});
lidarSensor = GameObject.Find("Lidar").GetComponent <LidarSensor>();
m_Socket.Advertise <LidarData>("/LidarData");
//m_Socket.Advertise<ProcessedControllerInput>("/processed_arm_controller_input");
//m_Socket.Advertise<WheelSpeed>("/WheelSpeed");
//m_Socket.Advertise<WheelSpeed>("/TestTopic");
//m_Socket.Subscribe<WheelSpeed>("/WheelSpeed", speed =>
//{
// Debug.Log(speed.Wheel_Speed[0]);
// Debug.Log(speed.Wheel_Speed[1]);
//});
//m_Socket.Subscribe<Int32>("/TestTopic", num =>
//{
// //Debug.Log(num.Wheel_Speed);
//});
}
void Awake()
{
m_RosConnector?.Awake();
}
