private void Update()
{
string linkName = (m_IsLeft) ? "left_hand" : "right_hand";
List <string> linkNames = new List <string>();
linkNames.Add(linkName);
var xDic = new Dictionary <string, float>();
var yDic = new Dictionary <string, float>();
var zDic = new Dictionary <string, float>();
var qxDic = new Dictionary <string, float>();
var qyDic = new Dictionary <string, float>();
var qzDic = new Dictionary <string, float>();
var qwDic = new Dictionary <string, float>();
Vector3 gazeboPosition = GazeboUtility.UnityPositionToGazebo(transform.position);
Quaternion gazeboRotation = GazeboUtility.UnityRotationToGazebo(transform.rotation);
//Vector3 gazeboPosition = transform.position;
//Quaternion gazeboRotation = transform.rotation;
xDic.Add(linkName, gazeboPosition.x);
yDic.Add(linkName, gazeboPosition.y);
zDic.Add(linkName, gazeboPosition.z);
qxDic.Add(linkName, gazeboRotation.x);
qyDic.Add(linkName, gazeboRotation.y);
qzDic.Add(linkName, gazeboRotation.z);
qwDic.Add(linkName, gazeboRotation.w);
ROSBridgeLib.custom_msgs.RoboyPoseMsg msg = new ROSBridgeLib.custom_msgs.RoboyPoseMsg("hands", linkNames, xDic, yDic, zDic, qxDic, qyDic, qzDic, qwDic);
ROSBridge.Instance.Publish(RoboyHandsPublisher.GetMessageTopic(), msg);
}
/// <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 void CreateSimulationModel()
{
if (m_Colliding)
{
return;
}
GameObject simModel = Instantiate(SimulationModel, transform.position, transform.rotation);
simModel.name = SimulationModel.name;
InputManager.Instance.ModelSpawn_Controller.Operating = false;
Destroy(InputManager.Instance.Selector_Tool.CurrentPreviewModel.gameObject);
InputManager.Instance.Selector_Tool.CurrentPreviewModel = null;
if (SimulationModel.GetComponent <ROSObject>() == null)
{
return;
}
// publish a message to add the object to ros node
List <string> objects = new List <string>();
objects.Add(SimulationModel.name);
List <Vector3> positions = new List <Vector3>();
positions.Add(GazeboUtility.UnityPositionToGazebo(transform.position));
ModelMsg msg = new ModelMsg(1, 1, objects, positions);
ROSBridge.Instance.Publish(RoboyModelPublisher.GetMessageTopic(), msg);
RoboyManager.Instance.AddRoboy(simModel.transform);
}
/// <summary>
/// Sends a message to the simulation to apply an external force at a certain position.
/// </summary>
/// <param name="roboyPart">The roboypart where the force should be applied.</param>
/// <param name="position">The relative position of the force to the roboypart.</param>
/// <param name="force">The direction and the amount of force relative to roboypart.</param>
/// <param name="duration">The duration for which the force should be applied.</param>
public void ReceiveExternalForce(RoboyPart roboyPart, Vector3 position, Vector3 force, int duration)
{
ROSBridgeLib.custom_msgs.ExternalForceMsg msg =
new ROSBridgeLib.custom_msgs.ExternalForceMsg(roboyPart.gameObject.name, GazeboUtility.UnityPositionToGazebo(position), GazeboUtility.UnityPositionToGazebo(force), duration);
ROSBridge.Instance.Publish(RoboyForcePublisher.GetMessageTopic(), msg);
//Debug.Log(msg.ToYAMLString());
}
/// <summary>
/// List of all points for this tendon
/// </summary>
/// <returns></returns>
public Vector3[] GetWirepoints()
{
Vector3[] result = new Vector3[_wirepoints.Count];
for (int i = 0; i < _wirepoints.Count; i++)
{
result[i] = GazeboUtility.GazeboPositionToUnity(_wirepoints[i]);
}
return(_wirepoints.ToArray());
}
/// <summary>
/// constructor
/// </summary>
/// <param name="msg"></param>
public DebugMsg(JSONNode msg)
{
//tendonID
//These need to match names of externally defined msgs
//-> https://github.com/CapChrisCap/roboy_communication/tree/feature/error-detection-msgs/roboy_communication_control/msg
_code = int.Parse(msg["code"]);
_object = GazeboUtility.RemoveQuotationMarks(msg["object"].ToString()); //parse string and remove ""
_msg = GazeboUtility.RemoveQuotationMarks(msg["msg"].ToString());
_extra = GazeboUtility.RemoveQuotationMarks(msg["extra"].ToString());
_timeFrame = ((float)int.Parse(msg["validityDuration"])) / 1000;
//_timeFrame = 4f;
}
// Update is called once per frame
void Update()
{
if (Input.GetKeyDown(KeyCode.Space))
{
List <string> objects = new List <string>();
objects.Add("Roboy_simplified");
List <Vector3> positions = new List <Vector3>();
positions.Add(GazeboUtility.UnityPositionToGazebo(new Vector3(2.3123f, 2.2311f, 0.5f)));
ModelMsg msg = new ModelMsg(1, 1, objects, positions);
Debug.Log(msg.ToYAMLString());
ROSBridge.Instance.Publish(RoboyModelPublisher.GetMessageTopic(), msg);
}
else if (Input.GetKeyDown(KeyCode.Escape))
{
List <string> objects = new List <string>();
objects.Add("Roboy_simplified");
List <Vector3> positions = new List <Vector3>();
positions.Add(Vector3.zero);
ModelMsg msg = new ModelMsg(0, 1, objects, positions);
ROSBridge.Instance.Publish(RoboyModelPublisher.GetMessageTopic(), msg);
}
}
/// <summary>
/// Creates prefabs for every model which were downloaded.
/// </summary>
public void CreatePrefab()
{
foreach (string modelName in m_ModelNames)
{
string pathToSDFFile = UpdaterUtility.ProjectFolder + @"/temp" + modelName + "SDFs.txt";
if (!File.Exists(pathToSDFFile))
{
Debug.LogWarning("Scan file not found! Check whether it exists or if python script is working!");
return;
}
// get file content of format title:url
string[] sdfContent = File.ReadAllLines(pathToSDFFile);
File.Delete(pathToSDFFile);
List <string[]> linkList = new List <string[]>();
foreach (string line in sdfContent)
{
string[] SDFline = line.Split(';');
linkList.Add(SDFline);
}
GameObject modelParent = null;
string absoluteModelPath = UpdaterUtility.ProjectFolder + @"/SimulationModels/" + modelName + "/OriginModels";
for (int i = 0; i < linkList.Count; i++)
//foreach (string[] line in linkList)
{
if (linkList[i][0] == "model_name")
{
//Create GameObject where everything will be attached
modelParent = new GameObject(linkList[i][1]);
linkList.Remove(linkList[i]);
//continue;
}
}
//List for all downloaded visuals
List <string> visMeshList = new List <string>();
if (absoluteModelPath != "")
{
visMeshList = UpdaterUtility.getFilePathsFBX(absoluteModelPath + @"/visual");
}
//List for all downloaded colliders
List <string> colMeshList = new List <string>();
if (absoluteModelPath != "")
{
colMeshList = UpdaterUtility.getFilePathsFBX(absoluteModelPath + @"/collision");
}
foreach (string name in visMeshList)
{
string[] currentLine = null;
foreach (string[] line in linkList)
{
for (int j = 0; j < line.Length; j++)
{
string name1 = name.Replace(".fbx", "");
Debug.Log(name1 + " : " + name);
if (line[j].Contains(name1))
{
currentLine = line;
Debug.Log(currentLine.Length);
}
}
}
string[] pose = null;
string[] VIS_scale = null;
//string[] COL_scale = null;
for (int i = 0; i < currentLine.Length; i++)
{
if (currentLine[i].Contains("link_pose"))
{
pose = currentLine[i + 1].Split(' ');
}
if (currentLine[i].Contains("VIS_mesh_scale"))
{
VIS_scale = currentLine[i + 1].Split(' ');
}
//if (currentLine[i].Contains("COL_mesh_scale"))
//{
// COL_scale = currentLine[i + 1].Split(' ');
//}
}
GameObject meshPrefab = null;
string relativeModelPath = "Assets/SimulationModels/" + modelName + "/OriginModels/";
// import Mesh
meshPrefab = (GameObject)AssetDatabase.LoadAssetAtPath(relativeModelPath + @"visual/" + name, typeof(UnityEngine.Object));
//StartCoroutine(importModelCoroutine(path, (result) => { meshPrefab = result; }));
if (meshPrefab == null)
{
Debug.Log("Could not import model!");
continue;
}
GameObject meshCopy = Instantiate(meshPrefab);
Debug.Log(pose.Length);
meshCopy.tag = "RoboyPart";
meshCopy.transform.position = GazeboUtility.GazeboPositionToUnity(new Vector3(float.Parse(pose[0], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(pose[1], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(pose[2], CultureInfo.InvariantCulture.NumberFormat)));
meshCopy.transform.eulerAngles = GazeboUtility.GazeboPositionToUnity(new Vector3(Mathf.Rad2Deg * float.Parse(pose[3], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(pose[4], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(pose[5], CultureInfo.InvariantCulture.NumberFormat)));
if (VIS_scale != null)
{
meshCopy.transform.localScale = GazeboUtility.GazeboPositionToUnity(new Vector3(100 * float.Parse(VIS_scale[0], CultureInfo.InvariantCulture.NumberFormat),
100 * float.Parse(VIS_scale[1], CultureInfo.InvariantCulture.NumberFormat),
100 * float.Parse(VIS_scale[2], CultureInfo.InvariantCulture.NumberFormat)));
}
UpdaterUtility.attachCollider(meshCopy, relativeModelPath, name);
var regex1 = new Regex(Regex.Escape("(Clone)"));
meshCopy.name = regex1.Replace(meshCopy.name, "", 1);
var regex2 = new Regex(Regex.Escape("VIS_"));
meshCopy.name = regex2.Replace(meshCopy.name, "", 1);
SelectableObject selectableObjectComponent = meshCopy.AddComponent <SelectableObject>();
selectableObjectComponent.TargetedMaterial = Resources.Load("RoboyMaterials/TargetedMaterial") as Material;
selectableObjectComponent.SelectedMaterial = Resources.Load("RoboyMaterials/SelectedMaterial") as Material;
meshCopy.AddComponent <RoboyPart>();
// Attach Model with mesh to parent GO
meshCopy.transform.parent = modelParent.transform;
}
modelParent.tag = "Roboy";
//Create Prefab of existing GO
UnityEngine.Object prefab = PrefabUtility.CreateEmptyPrefab("Assets/SimulationModels/" + modelName + "/" + modelName + ".prefab");
PrefabUtility.ReplacePrefab(modelParent, prefab, ReplacePrefabOptions.ConnectToPrefab);
//Destroy GO after prefab is created
DestroyImmediate(modelParent);
}
m_ModelNames.Clear();
}
/// <summary>
/// Turn Roboy with the movement of the HMD.
/// </summary>
private void translateRoboy()
{
// References to roboy parts we need for rotation/ translation
Transform head_parent = transform.GetChild(0).Find("head");
Transform head_pivot = head_parent.GetChild(0);
Transform torso_pivot = transform.GetChild(0).Find("torso_pivot");
// Check whether the user has rotated the headset in x direction or not
if (m_CurrentAngleX != m_Cam.transform.eulerAngles.x)
{
// If the headset was rotated, rotate roboy
head_parent.RotateAround(head_pivot.position, Vector3.right, m_Cam.transform.eulerAngles.x - m_CurrentAngleX);
}
m_CurrentAngleX = m_Cam.transform.eulerAngles.x;
// Check whether the user has rotated the headset in y direction or not
if (m_CurrentAngleY != m_Cam.transform.eulerAngles.y)
{
// If the headset was rotated, rotate roboy
head_parent.RotateAround(head_pivot.position, Vector3.up, m_Cam.transform.eulerAngles.y - m_CurrentAngleY);
}
m_CurrentAngleY = m_Cam.transform.eulerAngles.y;
// Move roboy accordingly to headset movement
Quaternion headRotation = InputTracking.GetLocalRotation(VRNode.Head);
transform.position = m_Cam.transform.position + (headRotation * Vector3.forward) * (-0.3f);
// Publish position to gazebo
ReceivePosition(GazeboUtility.UnityPositionToGazebo(transform.position));
// The torso of roboy will be rotated towards the (right)controller position
torso_pivot.transform.LookAt(new Vector3(m_Controller.transform.position.x, torso_pivot.transform.position.y, m_Controller.transform.position.z));
// Send rotation data via ROS
// Convert the headset rotation from unity coordinate spaze to gazebo coordinates
Quaternion rot = GazeboUtility.UnityRotationToGazebo(headRotation);
float x_angle = 0.0f;
float y_angle = 0.0f;
// Angle for torso rotation
float t_angle = 0.0f;
// Convert head rotation
if (rot.eulerAngles.x > 180)
{
y_angle = (rot.eulerAngles.x - 360) * Mathf.Deg2Rad;
}
else
{
y_angle = rot.eulerAngles.x * Mathf.Deg2Rad;
}
x_angle = rot.eulerAngles.z * Mathf.Deg2Rad;
// Now convert torso rotation
if (torso_pivot.eulerAngles.y > 180)
{
t_angle = (torso_pivot.eulerAngles.y - 360) * Mathf.Deg2Rad;
}
else
{
t_angle = torso_pivot.eulerAngles.y * Mathf.Deg2Rad;
}
// Determine which joints should me modified
List <string> joints = new List <string>();
// X rotation
joints.Add("neck_3");
// Y rotation
joints.Add("neck_4");
// Body rotation
joints.Add("spine_1");
// Determine the angle for the joints
List <float> angles = new List <float>();
// Add the x-angle of the headset after conversion from unity to ros
angles.Add(x_angle);
// Add the y-angle of the headset after conversion from unity to ros
angles.Add(y_angle);
// Add the torso rotation angle after conversion from unity to ros
angles.Add(t_angle * (-1.0f));
// Start sending the actual message
ReceiveExternalJoint(joints, angles);
}
/// <summary>
/// Downloads models given in the .world files and saves their pose in a struct.
/// </summary>
public void Magic()
{
List <KeyValuePair <string, bool> > tempURLList = WorldChoiceDictionary.Where(entry => entry.Value == true).ToList();
foreach (var urlEntry in tempURLList)
{
if (File.Exists(UpdaterUtility.ProjectFolder + @"/SimulationWorlds/" + urlEntry.Key + @"/" + urlEntry.Key + ".world"))
{
Debug.Log(".world file found!");
// read .world file
string[] argumentsSDFreader = { "python \"" + UpdaterUtility.PathToWorldReader + "\" \"" + UpdaterUtility.ProjectFolder + @"/SimulationWorlds/" + urlEntry.Key + @"/" + urlEntry.Key + ".world\"" };
CommandlineUtility.RunCommandLine(argumentsSDFreader);
}
else
{
Debug.LogWarning(".world file not found!");
}
string pathToWorldFile = UpdaterUtility.ProjectFolder + @"/temp" + urlEntry.Key + @"World.txt";
if (!File.Exists(pathToWorldFile))
{
Debug.LogWarning("Scan file not found! Check whether it exists or if python script is working!");
return;
}
//save content of temp file
string[] WorldContent = File.ReadAllLines(pathToWorldFile);
//delete temp file
File.Delete(pathToWorldFile);
List <string[]> modelList = new List <string[]>();
foreach (string line in WorldContent)
{
//each line contains of a model link with pose, scale, model_link
string[] SDFline = line.Split(';');
modelList.Add(SDFline);
}
string worldName = null;
for (int i = 0; i < modelList.Count; i++)
{
if (modelList[i][0] == "world_name")
{
//get world name
worldName = modelList[i][1];
modelList.Remove(modelList[i]);
}
}
foreach (string[] line in modelList)
{
//each model will be saved in a struct, so we can apply pose and scale later on
ModelTransformation testModel = new ModelTransformation();
testModel.worldname = worldName;
//check which attributes are set by the .world file and set them in the struct
for (int j = 0; j < line.Length; j++)
{
testModel.link = new LinkTransformation();
string[] pose = null;
if (line[j] == "model_pose")
{ //saving model_pose
pose = line[j + 1].Split(' ');
testModel.position = GazeboUtility.GazeboPositionToUnity(new Vector3(float.Parse(pose[0], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(pose[1], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(pose[2], CultureInfo.InvariantCulture.NumberFormat)));
testModel.rotation = GazeboUtility.GazeboPositionToUnity(new Vector3(Mathf.Rad2Deg * float.Parse(pose[3], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(pose[4], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(pose[5], CultureInfo.InvariantCulture.NumberFormat)));
}
if (line[j] == "model_link")
{ //saving model_link name
testModel.link.name = line[j + 1];
}
if (line[j] == "VIS_mesh_uri")
{ // saving VIS_mesh_link and model.name
testModel.link.VIS_meshName = line[j + 1];
string name1 = Regex.Replace(line[j + 1], ".*?//", "");
string name2 = Regex.Replace(name1, "/.*", "");
testModel.name = name2;
Debug.Log(testModel.link.VIS_meshName);
}
//saving COL_mesh_link
if (line[j] == "COL_mesh_uri")
{
testModel.link.COL_meshName = line[j + 1];
}
string[] VIS_Scale = null;
if (line[j] == "VIS_mesh_scale")
{
//saving model scale
VIS_Scale = line[j + 1].Split(' ');
}
if (VIS_Scale != null)
{ //converting model scale
testModel.link.VIS_scale = GazeboUtility.GazeboPositionToUnity(new Vector3(100 * float.Parse(VIS_Scale[0], CultureInfo.InvariantCulture.NumberFormat),
100 * float.Parse(VIS_Scale[1], CultureInfo.InvariantCulture.NumberFormat),
100 * float.Parse(VIS_Scale[2], CultureInfo.InvariantCulture.NumberFormat)));
}
string[] linkpose = null;
if (line[j] == "link_pose")
{
pose = line[j + 1].Split(' ');
}
if (linkpose != null)
{
testModel.position = GazeboUtility.GazeboPositionToUnity(new Vector3(float.Parse(linkpose[0], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(linkpose[1], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(linkpose[2], CultureInfo.InvariantCulture.NumberFormat)));
testModel.rotation = GazeboUtility.GazeboPositionToUnity(new Vector3(Mathf.Rad2Deg * float.Parse(linkpose[3], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(linkpose[4], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(linkpose[5], CultureInfo.InvariantCulture.NumberFormat)));
}
}
modellist.Add(testModel);
//}
}
WorldsCurrentState = UpdaterUtility.State.Downloaded;
}
List <string> names = new List <string>();
foreach (ModelTransformation model in modellist)
{
if (!names.Contains(model.name))
{
names.Add(model.name);
//Debug.Log(model.name);
}
}
foreach (string name in names)
{
//replace "tree" with "raw" in URL
var regex = new Regex(Regex.Escape("tree"));
m_PathToModelsFolder = regex.Replace(m_PathToModelsFolder, "raw", 1);
//start modeldownloader.py for visual
string[] updateArgumentsWorld = { "start \"\" \"" + UpdaterUtility.PathToBlender + "\" -P", UpdaterUtility.PathToDownloadScript, m_PathToModelsFolder + @"/" + name + @"/meshes/", UpdaterUtility.ProjectFolder + @"/SimulationWorlds/Models/" + name + "/meshes", "" };
CommandlineUtility.RunCommandLine(updateArgumentsWorld);
}
}
