/// <summary>
/// Main function to receive messages from ROSBridge. Adjusts the roboy pose and the motors values (future).
/// </summary>
/// <param name="msg">JSON msg containing roboy pose.</param>
public void ReceiveMessage(RoboyPoseMsg msg)
{
//Debug.Log("Received message");
adjustPose(msg.Name, msg);
//Use additional data to adjust motor values
}
/// <summary>
/// Adjusts roboy pose for all parts with the values from the simulation.
/// </summary>
/// <param name="msg">JSON msg containing the roboy pose.</param>
void adjustPose(string name, RoboyPoseMsg msg)
{
m_RoboyPoseMessage = msg;
Dictionary <string, float> xPositionsDictionary = m_RoboyPoseMessage.XDic;
Dictionary <string, float> yPositionsDictionary = m_RoboyPoseMessage.YDic;
Dictionary <string, float> zPositionsDictionary = m_RoboyPoseMessage.ZDic;
Dictionary <string, float> qxRotationsDictionary = m_RoboyPoseMessage.QxDic;
Dictionary <string, float> qyRotationsDictionary = m_RoboyPoseMessage.QyDic;
Dictionary <string, float> qzRotationsDictionary = m_RoboyPoseMessage.QzDic;
Dictionary <string, float> qwRotationsDictionary = m_RoboyPoseMessage.QwDic;
Debug.Log(name);
foreach (KeyValuePair <string, RoboyPart> roboyPart in m_RoboyPartsList[name])
{
Debug.Log(roboyPart.Key);
Debug.Log(roboyPart.Value);
string index = roboyPart.Key;
Vector3 originPosition = new Vector3(xPositionsDictionary[index], yPositionsDictionary[index], zPositionsDictionary[index]);
Quaternion originRotation = new Quaternion(qxRotationsDictionary[index], qyRotationsDictionary[index], qzRotationsDictionary[index], qwRotationsDictionary[index]);
roboyPart.Value.transform.localPosition = GazeboUtility.GazeboPositionToUnity(originPosition);
roboyPart.Value.transform.localRotation = GazeboUtility.GazeboRotationToUnity(originRotation);
}
}
public new static void CallBack(ROSBridgeMsg msg)
{
RoboyPoseMsg pose = (RoboyPoseMsg)msg;
RoboyManager.Instance.ReceiveMessage(pose);
}
public static string ToYAMLString(RoboyPoseMsg msg)
{
return(msg.ToYAMLString());
}
