void SendControls()
{
scale = TFListener.scale;
//Convert the Unity position of the hand controller to a ROS position (scaled)
Vector3 outPos = UnityToRosPositionAxisConversion(GetComponent <Transform>().position) / scale;
//Convert the Unity rotation of the hand controller to a ROS rotation (scaled, quaternions)
Quaternion outQuat = UnityToRosRotationAxisConversion(GetComponent <Transform>().rotation);
//construct the Ein message to be published
string message = "";
//Allows movement control with controllers if menu is disabled
//if deadman switch held in, move to new pose
if (Input.GetAxis(grip_label) > 0.5f)
{
//construct message to move to new pose for the robot end effector
message = outPos.x + " " + outPos.y + " " + outPos.z + " " +
outQuat.x + " " + outQuat.y + " " + outQuat.z + " " + outQuat.w + " moveToEEPose";
//if touchpad is pressed (Crane game), incrementally move in new direction
}
//If trigger pressed, open the gripper. Else, close gripper
if (Input.GetAxis(trigger_label) > 0.5f)
{
message += " openGripper ";
}
else
{
message += " closeGripper ";
}
//Send the message to the websocket client (i.e: publish message onto ROS network)
wsc.SendEinMessage(message, arm);
}
void SendControls()
{
scale = TFListener.scale;
//Convert the Unity position of the hand controller to a ROS position (scaled)
Vector3 outPos = UnityToRosPositionAxisConversion(GetComponent <Transform>().position) / scale;
//Convert the Unity rotation of the hand controller to a ROS rotation (scaled, quaternions)
Quaternion outQuat = UnityToRosRotationAxisConversion(GetComponent <Transform>().rotation);
//construct the Ein message to be published
string message = "";
//Allows movement control with controllers if menu is disabled
//if deadman switch held in, move to new pose
if (controller.gripPressed)
{
//construct message to move to new pose for the robot end effector
message = outPos.x + " " + outPos.y + " " + outPos.z + " " +
outQuat.x + " " + outQuat.y + " " + outQuat.z + " " + outQuat.w + " moveToEEPose";
//if touchpad is pressed (Crane game), incrementally move in new direction
}
else if (controller.touchpadPressed)
{
//get the angle contact point on touch pad
float angle = controller.GetTouchpadAxisAngle();
//Con
if (angle >= 45 && angle < 135) // touching right
{
message += " yDown ";
}
else if (angle >= 135 && angle < 225) // touching bottom
{
message += " xDown ";
}
else if (angle >= 225 && angle < 315) // touching left
{
message += " yUp ";
}
else //touching top
{
message += " xUp ";
}
}
//If trigger pressed, open the gripper. Else, close gripper
if (controller.triggerPressed)
{
message += " openGripper ";
}
else
{
message += " closeGripper ";
}
//Send the message to the websocket client (i.e: publish message onto ROS network)
wsc.SendEinMessage(message, arm);
//Debug.Log(arm+":"+message);
}
void sendControls()
{
scale = TFListener.scale;
Vector3 outPos = UnityToRosPositionAxisConversion(tf.position) / scale;
Quaternion outQuat = UnityToRosRotationAxisConversion(tf.rotation);
string message = "";
//Allows movement control with controllers if menu is disabled
if (!menu.activeSelf)
{
if (controller.gripPressed)
{
message = outPos.x + " " + outPos.y + " " + outPos.z + " " + outQuat.x + " " + outQuat.y + " " + outQuat.z + " " + outQuat.w + " moveToEEPose";
}
else if (controller.touchpadPressed)
{
float angle = controller.GetTouchpadAxisAngle();
if (angle >= 45 && angle < 135) // touching right
{
message += " yDown ";
}
else if (angle >= 135 && angle < 225) // touching bottom
{
message += " xDown ";
}
else if (angle >= 225 && angle < 315) // touching left
{
message += " yUp ";
}
else //touching top
{
message += " xUp ";
}
}
if (controller.triggerPressed)
{
message += " openGripper ";
}
else
{
message += " closeGripper ";
}
wsc.SendEinMessage(message, arm);
}
//Debug.Log(arm+":"+message);
}
void goHome_left()
{
Debug.Log("cheecker");
wsc.SendEinMessage("goHome", "left");
}
void sendMessage() //send an ein message to arm
{
wsc.SendEinMessage(message, arm);
}