// Initilize the sensor
public void InitilizeSensor(int uniqueID, string sensorURL, int sensorPort, List <string> sensorSubscribers)
{
Debug.Log("Init PC Face Mesh Connection at " + sensorURL + ":" + sensorPort.ToString());
ros = new ROSBridgeWebSocketConnection(sensorURL, sensorPort);
client_id = uniqueID.ToString();
foreach (string subscriber in sensorSubscribers)
{
string subscriberTopic = "";
switch (subscriber)
{
default:
subscriberTopic = "/" + subscriber;
// Create PC Face Visualizer, initilize it as a child of this sensor gameobject and add it to the PCFaceVisualizer dictionary
PCFaceVisualizer pcFaceVisualizer = gameObject.AddComponent <PCFaceVisualizer>();
pcFaceVisualizer.CreateMeshGameobject(this.transform);
pcFaceVisualizers.Add(subscriberTopic, pcFaceVisualizer);
break;
}
Debug.Log(" PC Face Mesh Subscribing to : " + subscriberTopic);
sensorSubscriberTopicsDict.Add(subscriberTopic, true);
ros.AddSubscriber(subscriberTopic, this);
}
ros.Connect();
// Hardcode Parent transform
SetLocalPosition(new Vector3(1.98f, 0f, -6.65f));
SetLocalOrientation(Quaternion.Euler(0f, 124.654f, 0f));
SetLocalScale(new Vector3(0.505388f, 0.505388f, 0.505388f));
}
// ROS Topic Subscriber methods
public ROSBridgeMsg OnReceiveMessage(string topic, JSONNode raw_msg, ROSBridgeMsg parsed = null)
{
Debug.Log(" PC Face Mesh Recieved message");
ROSBridgeMsg result = null;
bool parsePCFaceMesh = false;
/// Color of the mesh
Color color = Color.white;
// Writing all code in here for now. May need to move out to separate handler functions when it gets too unwieldy.
switch (topic)
{
case "/colorized_points_faced_0":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(0, 0, 0.5f, alpha);
break;
case "/colorized_points_faced_1":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(0, 0, 1, alpha);
break;
case "/colorized_points_faced_2":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(0, 1, 1, alpha);
break;
case "/colorized_points_faced_3":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(0, 1, 0, alpha);
break;
case "/colorized_points_faced_4":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(1, 1, 0, alpha);
break;
case "/colorized_points_faced_5":
Debug.Log("PC Face Mesh Visualizer Callback: " + topic);
parsePCFaceMesh = true;
color = new Color(1, 0, 0, alpha);
break;
default:
Debug.LogError("Topic not implemented: " + topic);
parsePCFaceMesh = true;
break;
}
if (parsePCFaceMesh)
{
PCFaceMsg meshMsg = new PCFaceMsg(raw_msg);
// Obtain visualizer for this topic and update mesh & color
PCFaceVisualizer visualizer = pcFaceVisualizers[topic];
visualizer.SetMesh(meshMsg);
visualizer.SetColor(color);
}
return(result);
}
