private void PublishResult(List <SemanticObject> detectedObjectsInside)
{
foreach (ROS client in clients)
{
HeaderMsg _head = new HeaderMsg(0, new TimeMsg(DateTime.Now.Second, 0), currentRoom.ToString());
List <SemanticRoomScoreMsg> probabilities = new List <SemanticRoomScoreMsg>();
foreach (KeyValuePair <string, float> result in semantic_rooms[currentRoom])
{
probabilities.Add(new SemanticRoomScoreMsg(result.Key, result.Value));
}
SemanticRoomMsg msg = new SemanticRoomMsg(_head, currentRoom, probabilities.ToArray());
client.Publish(RoomScores_pub.GetMessageTopic(), msg);
if (detectedObjectsInside.Count > 0)
{
List <SemanticObjectMsg> obj_msg = new List <SemanticObjectMsg>();
foreach (var obj in detectedObjectsInside)
{
var _scores = new ObjectHypothesisMsg[obj.Scores.Count];
int i = 0;
foreach (KeyValuePair <string, float> score in obj.Scores)
{
_scores[i] = new ObjectHypothesisMsg(score.Key, score.Value);
i++;
}
SemanticObjectMsg semanticObject = new SemanticObjectMsg(
obj.Id,
_scores,
new PoseMsg(obj.Position, obj.Rotation),
obj.NDetections,
obj.GetIdRoom(),
obj.Room.roomType,
new Vector3Msg(obj.Size));
obj_msg.Add(semanticObject);
}
SemanticObjectArrayMsg msg2 = new SemanticObjectArrayMsg(_head, obj_msg.ToArray());
client.Publish(ObjectsInRoom_pub.GetMessageTopic(), msg2);
}
}
}
public static string ToYAMLString(SemanticObjectArrayMsg msg)
{
return(msg.ToYAMLString());
}
