/**************************************************
* ROSからの返答待ち
**************************************************/
void WaitResponce(float timeout)
{
time_access += Time.deltaTime;
if (time_access > timeout)
{
time_access = 0.0f;
abort_access = true;
access_db = false;
}
if (wsc.IsReceiveSrvRes() && wsc.GetSrvResValue("service") == "sp5_control")
{
srvRes = wsc.GetSrvResMsg();
Debug.Log("ROS: " + srvRes);
responce = JsonUtility.FromJson <Res_sp5_control>(srvRes);
Debug.Log("result: " + responce.values.result.ToString());
string val_string = "[";
foreach (float val in responce.values.val)
{
val_string += val.ToString() + ", ";
}
if (val_string.Length > 1)
{
val_string = val_string.Substring(0, val_string.Length - 2);
}
val_string += "]";
Debug.Log("val: " + val_string);
success_access = true;
access_db = false;
}
}
// Update is called once per frame
void Update()
{
time_leftgripper_state += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_leftgripper_state > 0.5f)
{
time_leftgripper_state = 0.0f;
IEnumerator coroutine = cm.ReadLeftGripperState();
StartCoroutine(coroutine);
}
if (cm.CheckWaitLeftGripperState())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
switch (responce.values.result)
{
case 0:
LeftGripper_State_Text.text = "State: Not Moving";
break;
case 1:
LeftGripper_State_Text.text = "State: Moving";
break;
default:
LeftGripper_State_Text.text = "State: Unknown State";
break;
}
}
}
time_leftgripper_pos += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_leftgripper_pos > 0.5f)
{
time_leftgripper_pos = 0.0f;
IEnumerator coroutine = cm.ReadLeftGripperPos();
StartCoroutine(coroutine);
}
if (cm.CheckWaitLeftGripperPos())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
LeftGripper_Pos_Text.text = "Joint: (" + (responce.values.val[0] * Mathf.Rad2Deg).ToString("f0") + ")";
}
}
}
// Update is called once per frame
void Update()
{
time_leftarm_state += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_leftarm_state > 0.5f)
{
time_leftarm_state = 0.0f;
IEnumerator coroutine = cm.ReadLeftArmState();
StartCoroutine(coroutine);
}
if (cm.CheckWaitLeftArmState())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
switch (responce.values.result)
{
case 16:
LeftArm_State_Text.text = "State: Unpowered";
break;
case 17:
LeftArm_State_Text.text = "State: Powered";
break;
case 18:
LeftArm_State_Text.text = "State: Ready";
break;
case 19:
LeftArm_State_Text.text = "State: Busy";
break;
case 20:
LeftArm_State_Text.text = "State: Paused";
break;
case 21:
LeftArm_State_Text.text = "State: Alarm";
break;
case 22:
LeftArm_State_Text.text = "State: Jog Busy";
break;
case 23:
LeftArm_State_Text.text = "State: Direct Busy";
break;
default:
LeftArm_State_Text.text = "State: Unknown State";
break;
}
}
}
time_leftarm_pos += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_leftarm_pos > 1.0f)
{
time_leftarm_pos = 0.0f;
IEnumerator coroutine = cm.ReadLeftArmPos();
StartCoroutine(coroutine);
}
if (cm.CheckWaitLeftArmPos())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
string val_string = "(";
foreach (float val in responce.values.val)
{
float val_tmp = val * Mathf.Rad2Deg;
val_string += val_tmp.ToString("f0") + ", ";
}
if (val_string.Length > 1)
{
val_string = val_string.Substring(0, val_string.Length - 2);
}
val_string += ")";
LeftArm_Pos_Text.text = "Pos: " + val_string;
}
}
}
// Update is called once per frame
void Update()
{
switch (cm.wscConnectionState())
{
case wscCONST.STATE_CONNECTED:
Information_Text.text = "System: Connected";
break;
case wscCONST.STATE_DISCONNECTED:
Information_Text.text = "System: Disconnected";
break;
case wscCONST.STATE_ERROR:
Information_Text.text = "System: Error";
break;
}
//Vehicle周りの情報取得
time_vehicle_state += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_vehicle_state > 1.0f)
{
time_vehicle_state = 0.0f;
IEnumerator coroutine = cm.ReadVehicleState();
StartCoroutine(coroutine);
}
if (cm.CheckWaitVehicleState())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
switch (responce.values.result)
{
case 16:
Vehicle_State_Text.text = "State: Ready";
break;
case 17:
Vehicle_State_Text.text = "State: Busy";
break;
case 19:
Vehicle_State_Text.text = "State: Alarm";
break;
case 20:
Vehicle_State_Text.text = "State: Stuck";
break;
case 21:
Vehicle_State_Text.text = "State: Paused";
break;
case 23:
Vehicle_State_Text.text = "State: Locked";
break;
case 24:
Vehicle_State_Text.text = "State: Powered";
break;
case 25:
Vehicle_State_Text.text = "State: Unpowered";
break;
case 26:
Vehicle_State_Text.text = "State: Caution";
break;
default:
Vehicle_State_Text.text = "State: Unknown State";
break;
}
}
}
time_vehicle_pos += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_vehicle_pos > 1.0f)
{
time_vehicle_pos = 0.0f;
IEnumerator coroutine = cm.ReadVehiclePos();
StartCoroutine(coroutine);
}
if (cm.CheckWaitVehiclePos())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
Vector3 pos = new Vector3(responce.values.val[0], responce.values.val[1], responce.values.val[2]);
Vehicle_Pos_Text.text = "Pos: " + pos.ToString("f2");
}
}
//Right Arm周りの情報取得
time_rightarm_power += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_rightarm_power > 3.0f)
{
time_rightarm_power = 0.0f;
IEnumerator coroutine = cm.ReadRightArmPower();
StartCoroutine(coroutine);
}
if (cm.CheckWaitRightArmPower())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
if (responce.values.result == 1)
{
RightArm_Power_Text.text = "Power: OK!!";
}
else
{
RightArm_Power_Text.text = "Power: NG...";
}
}
}
time_rightarm_servo += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_rightarm_servo > 3.0f)
{
time_rightarm_servo = 0.0f;
IEnumerator coroutine = cm.ReadRightArmServo();
StartCoroutine(coroutine);
}
if (cm.CheckWaitRightArmServo())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
if (responce.values.result == 1)
{
RightArm_Servo_Text.text = "Servo: OK!!";
}
else
{
RightArm_Servo_Text.text = "Servo: NG...";
}
}
}
time_rightarm_state += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_rightarm_state > 1.0f)
{
time_rightarm_state = 0.0f;
IEnumerator coroutine = cm.ReadRightArmState();
StartCoroutine(coroutine);
}
if (cm.CheckWaitRightArmState())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
switch (responce.values.result)
{
case 16:
RightArm_State_Text.text = "State: Unpowered";
break;
case 17:
RightArm_State_Text.text = "State: Powered";
break;
case 18:
RightArm_State_Text.text = "State: Ready";
break;
case 19:
RightArm_State_Text.text = "State: Busy";
break;
case 20:
RightArm_State_Text.text = "State: Paused";
break;
case 21:
RightArm_State_Text.text = "State: Alarm";
break;
case 22:
RightArm_State_Text.text = "State: Jog Busy";
break;
case 23:
RightArm_State_Text.text = "State: Direct Busy";
break;
default:
RightArm_State_Text.text = "State: Unknown State";
break;
}
if (responce.values.result == 21)
{
RightArm_ClearAlarm_Button.gameObject.SetActive(true);
}
else
{
RightArm_ClearAlarm_Button.gameObject.SetActive(false);
}
}
}
time_rightarm_pos += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_rightarm_pos > 1.0f)
{
time_rightarm_pos = 0.0f;
IEnumerator coroutine = cm.ReadRightArmPos();
StartCoroutine(coroutine);
}
if (cm.CheckWaitRightArmPos())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
string val_string = "(";
foreach (float val in responce.values.val)
{
float val_tmp = val * Mathf.Rad2Deg;
val_string += val_tmp.ToString("f0") + ", ";
}
if (val_string.Length > 1)
{
val_string = val_string.Substring(0, val_string.Length - 2);
}
val_string += ")";
RightArm_Pos_Text.text = "Joint: " + val_string;
}
}
//Left Arm周りの情報取得
time_leftarm_power += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_leftarm_power > 3.0f)
{
time_leftarm_power = 0.0f;
IEnumerator coroutine = cm.ReadLeftArmPower();
StartCoroutine(coroutine);
}
if (cm.CheckWaitLeftArmPower())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
if (responce.values.result == 1)
{
LeftArm_Power_Text.text = "Power: OK!!";
}
else
{
LeftArm_Power_Text.text = "Power: NG...";
}
}
}
time_leftarm_servo += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_leftarm_servo > 3.0f)
{
time_leftarm_servo = 0.0f;
IEnumerator coroutine = cm.ReadLeftArmServo();
StartCoroutine(coroutine);
}
if (cm.CheckWaitLeftArmServo())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
if (responce.values.result == 1)
{
LeftArm_Servo_Text.text = "Servo: OK!!";
}
else
{
LeftArm_Servo_Text.text = "Servo: NG...";
}
}
}
time_leftarm_state += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_leftarm_state > 1.0f)
{
time_leftarm_state = 0.0f;
IEnumerator coroutine = cm.ReadLeftArmState();
StartCoroutine(coroutine);
}
if (cm.CheckWaitLeftArmState())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
switch (responce.values.result)
{
case 16:
LeftArm_State_Text.text = "State: Unpowered";
break;
case 17:
LeftArm_State_Text.text = "State: Powered";
break;
case 18:
LeftArm_State_Text.text = "State: Ready";
break;
case 19:
LeftArm_State_Text.text = "State: Busy";
break;
case 20:
LeftArm_State_Text.text = "State: Paused";
break;
case 21:
LeftArm_State_Text.text = "State: Alarm";
break;
case 22:
LeftArm_State_Text.text = "State: Jog Busy";
break;
case 23:
LeftArm_State_Text.text = "State: Direct Busy";
break;
default:
LeftArm_State_Text.text = "State: Unknown State";
break;
}
if (responce.values.result == 21)
{
LeftArm_ClearAlarm_Button.gameObject.SetActive(true);
}
else
{
LeftArm_ClearAlarm_Button.gameObject.SetActive(false);
}
}
}
time_leftarm_pos += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_leftarm_pos > 1.0f)
{
time_leftarm_pos = 0.0f;
IEnumerator coroutine = cm.ReadLeftArmPos();
StartCoroutine(coroutine);
}
if (cm.CheckWaitLeftArmPos())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
string val_string = "(";
foreach (float val in responce.values.val)
{
float val_tmp = val * Mathf.Rad2Deg;
val_string += val_tmp.ToString("f0") + ", ";
}
if (val_string.Length > 1)
{
val_string = val_string.Substring(0, val_string.Length - 2);
}
val_string += ")";
LeftArm_Pos_Text.text = "Joint: " + val_string;
}
}
//Right Gripper周りの情報取得
time_rightgripper_state += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_rightgripper_state > 1.0f)
{
time_rightgripper_state = 0.0f;
IEnumerator coroutine = cm.ReadRightGripperState();
StartCoroutine(coroutine);
}
if (cm.CheckWaitRightGripperState())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
switch (responce.values.result)
{
case 0:
RightGripper_State_Text.text = "State: Not Moving";
break;
case 1:
RightGripper_State_Text.text = "State: Moving";
break;
default:
RightGripper_State_Text.text = "State: Unknown State";
break;
}
}
}
time_rightgripper_pos += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_rightgripper_pos > 1.0f)
{
time_rightgripper_pos = 0.0f;
IEnumerator coroutine = cm.ReadRightGripperPos();
StartCoroutine(coroutine);
}
if (cm.CheckWaitRightGripperPos())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
RightGripper_Pos_Text.text = "Joint: (" + (responce.values.val[0] * Mathf.Rad2Deg).ToString("f0") + ")";
}
}
//Left Gripper周りの情報取得
time_leftgripper_state += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_leftgripper_state > 1.0f)
{
time_leftgripper_state = 0.0f;
IEnumerator coroutine = cm.ReadLeftGripperState();
StartCoroutine(coroutine);
}
if (cm.CheckWaitLeftGripperState())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
switch (responce.values.result)
{
case 0:
LeftGripper_State_Text.text = "State: Not Moving";
break;
case 1:
LeftGripper_State_Text.text = "State: Moving";
break;
default:
LeftGripper_State_Text.text = "State: Unknown State";
break;
}
}
}
time_leftgripper_pos += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_leftgripper_pos > 1.0f)
{
time_leftgripper_pos = 0.0f;
IEnumerator coroutine = cm.ReadLeftGripperPos();
StartCoroutine(coroutine);
}
if (cm.CheckWaitLeftGripperPos())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
LeftGripper_Pos_Text.text = "Joint: (" + (responce.values.val[0] * Mathf.Rad2Deg).ToString("f0") + ")";
}
}
}
// Update is called once per frame
void Update()
{
//Vehicle周りの情報取得
time_vehicle_state += Time.deltaTime;
if (cm.wscConnectionState() == wscCONST.STATE_CONNECTED && !cm.CheckWaitAnything() && time_vehicle_state > 0.5f)
{
time_vehicle_state = 0.0f;
IEnumerator coroutine = cm.ReadVehicleState();
StartCoroutine(coroutine);
}
if (cm.CheckWaitVehicleState())
{
if (cm.CheckAbort())
{
cm.FinishAccess();
}
if (cm.CheckSuccess())
{
Res_sp5_control responce = cm.GetResponce();
cm.FinishAccess();
switch (responce.values.result)
{
case 16:
Vehicle_State_Text.text = "State: Ready";
break;
case 17:
Vehicle_State_Text.text = "State: Busy";
break;
case 19:
Vehicle_State_Text.text = "State: Alarm";
break;
case 20:
Vehicle_State_Text.text = "State: Stuck";
break;
case 21:
Vehicle_State_Text.text = "State: Paused";
break;
case 23:
Vehicle_State_Text.text = "State: Locked";
break;
case 24:
Vehicle_State_Text.text = "State: Powered";
break;
case 25:
Vehicle_State_Text.text = "State: Unpowered";
break;
case 26:
Vehicle_State_Text.text = "State: Caution";
break;
default:
Vehicle_State_Text.text = "State: Unknown State";
break;
}
if (responce.values.result == 16)
{
vehicle_ready = true;
}
else
{
vehicle_ready = false;
}
}
}
}
