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);
}
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());
}
// 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>
/// 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);
}