private void CheckOverLimitSpeed()
{
foreach (HSRCommon.Joint joint in this.trajectoryKeyList)
{
if (this.trajectoryInfoMap[joint] == null)
{
continue;
}
List <float> trajectoryInfoDurations = new List <float>(this.trajectoryInfoMap[joint].Durations);
List <float> trajectoryInfoGoalPositions = new List <float>(this.trajectoryInfoMap[joint].GoalPositions);
trajectoryInfoDurations.Insert(0, 0.0f);
trajectoryInfoGoalPositions.Insert(0, this.trajectoryInfoMap[joint].CurrentPosition);
for (int i = 1; i < trajectoryInfoGoalPositions.Count; i++)
{
double tempDistance = Math.Abs(trajectoryInfoGoalPositions[i] - trajectoryInfoGoalPositions[i - 1]);
double tempDurations = Math.Abs(trajectoryInfoDurations [i] - trajectoryInfoDurations[i - 1]);
double tempSpeed = tempDistance / tempDurations;
if (IsOverLimitSpeed(joint, tempSpeed))
{
SIGVerseLogger.Warn("Trajectry speed error. (" + this.topicName + ", Joint Name=" + joint.ToString() + ", Speed=" + tempSpeed + ", Max Speed=" + HSRCommon.GetMaxJointSpeed(joint) + ")");
return;
}
}
}
}
public void OnReceiveRosStringMsg(SIGVerse.RosBridge.std_msgs.String rosMsg)
{
if (this.isTaskFinished)
{
this.SendRosMessage("Task was finished");
return;
}
if (rosMsg.data == MsgTellMe)
{
this.receivedMessageMap[rosMsg.data] = true;
}
else if (rosMsg.data == MsgPointIt)
{
if (this.receivedMessageMap[MsgPointIt])
{
SIGVerseLogger.Warn("Now pointing");
return;
}
this.receivedMessageMap[rosMsg.data] = true;
}
else
{
SIGVerseLogger.Warn("Received Illegal message : " + rosMsg.data);
return;
}
}
public virtual void OnPlayButtonClick()
{
if (this.step == Step.Waiting && this.isInitialized)
{
switch (this.speedDropdown.value)
{
case 0: { this.playingSpeed = 1.0f; break; }
case 1: { this.playingSpeed = 2.0f; break; }
case 2: { this.playingSpeed = 4.0f; break; }
case 3: { this.playingSpeed = 8.0f; break; }
}
this.isRepeating = this.repeatToggle.isOn;
if (!this.Play(this.GetElapsedTimeUsingSlider()))
{
SIGVerseLogger.Warn("Cannot start the world playing");
}
}
else if (this.step == Step.Playing)
{
if (!this.Stop())
{
SIGVerseLogger.Warn("Cannot stop the world playing");
}
}
}
public void PressAorX(ModeratorStep step)
{
switch (step)
{
case ModeratorStep.WaitForIamReady:
{
this.hasPressedButtonToStartRecordingAvatarMotion = true;
break;
}
case ModeratorStep.WaitForObjectGrasped:
case ModeratorStep.WaitForTaskFinished:
case ModeratorStep.Judgement:
case ModeratorStep.WaitForNextTask:
{
this.hasPressedButtonToStopRecordingAvatarMotion = true;
break;
}
default:
{
SIGVerseLogger.Warn("This pressing A or X by the avatar is an invalid timing. step=" + step);
break;
}
}
}
public void SendMsg(NetworkStream networkStream)
{
MemoryStream memoryStream = new MemoryStream();
BsonWriter writer = new BsonWriter(memoryStream);
JsonSerializer serializer = new JsonSerializer();
serializer.ReferenceLoopHandling = ReferenceLoopHandling.Ignore;
serializer.Serialize(writer, this); // high load
byte[] msgBinary = memoryStream.ToArray();
// System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
// sw.Start();
try
{
if (networkStream.CanWrite)
{
networkStream.Write(msgBinary, 0, msgBinary.Length);
}
}
catch (ObjectDisposedException exception)
{
SIGVerseLogger.Warn(exception.Message);
}
// sw.Stop();
// UnityEngine.Debug.Log("time=" + sw.Elapsed + ", size=" + msgBinary.Length);
}
void OnTriggerEnter(Collider other)
{
if (other.isTrigger)
{
return;
}
if (other.attachedRigidbody == null)
{
return;
}
if (this.rigidbodyMap.ContainsKey(other))
{
SIGVerseLogger.Warn("This Collider has already been added. (" + this.GetType().FullName + ") name=" + SIGVerseUtils.GetHierarchyPath(other.transform));
return;
}
if (this.triggerType == TriggerType.Entrance && !this.rigidbodyMap.ContainsValue(other.attachedRigidbody))
{
ExecuteEvents.Execute <IFingerTriggerHandler>
(
target: this.transform.root.gameObject,
eventData: null,
functor: (reciever, eventData) => reciever.OnTransferredTriggerEnter(other.attachedRigidbody, this.fingerType)
);
}
this.rigidbodyMap.Add(other, other.attachedRigidbody);
}
protected override void SubscribeMessageCallback(SIGVerse.RosBridge.geometry_msgs.Twist twist)
{
if (this.jointTrajectoryOmni != null && this.jointTrajectoryOmni.IsRunning())
{
SIGVerseLogger.Warn("Rejected twist message because the robot is moving by JointTrajectory.");
return;
}
float linearVel = Mathf.Sqrt(Mathf.Pow(twist.linear.x, 2) + Mathf.Pow(twist.linear.y, 2));
float linearVelClamped = Mathf.Clamp(linearVel, 0.0f, HSRCommon.MaxSpeedBase);
if (linearVel >= 0.001)
{
this.linearVelX = twist.linear.x * linearVelClamped / linearVel;
this.linearVelY = twist.linear.y * linearVelClamped / linearVel;
}
else
{
this.linearVelX = 0.0f;
this.linearVelY = 0.0f;
}
this.angularVelZ = Mathf.Sign(twist.angular.z) * Mathf.Clamp(Mathf.Abs(twist.angular.z), 0.0f, HSRCommon.MaxSpeedBaseRad);
// Debug.Log("linearVel=" + linearVel + ", angularVel=" + angularVel);
this.isMoving = Mathf.Abs(this.linearVelX) >= 0.001f || Mathf.Abs(this.linearVelY) >= 0.001f || Mathf.Abs(this.angularVelZ) >= 0.001f;
}
void FixedUpdate()
{
try
{
// Receive the time gap between Unity and ROS
if (this.networkStream.DataAvailable)
{
byte[] byteArray = new byte[256];
if (networkStream.CanRead)
{
networkStream.Read(byteArray, 0, byteArray.Length);
}
string message = System.Text.Encoding.UTF8.GetString(byteArray);
string[] messageArray = message.Split(',');
if (messageArray.Length == 3)
{
SIGVerseLogger.Info("Time gap sec=" + messageArray[1] + ", msec=" + messageArray[2]);
SIGVerse.RosBridge.std_msgs.Header.SetTimeGap(Int32.Parse(messageArray[1]), Int32.Parse(messageArray[2]));
}
else
{
SIGVerseLogger.Error("Illegal message. Time gap message=" + message);
}
}
}
catch (ObjectDisposedException exception)
{
SIGVerseLogger.Warn(exception.Message);
}
}
void OnTriggerExit(Collider other)
{
if (other.isTrigger)
{
return;
}
if (other.attachedRigidbody == null)
{
return;
}
if (!this.rigidbodyMap.ContainsKey(other))
{
SIGVerseLogger.Warn("This Collider does not exist in the Dictionary. (" + this.GetType().FullName + ") name=" + SIGVerseUtils.GetHierarchyPath(other.transform));
return;
}
this.rigidbodyMap.Remove(other);
if (this.triggerType == TriggerType.Exit && !this.rigidbodyMap.ContainsValue(other.attachedRigidbody))
{
ExecuteEvents.Execute <IFingerTriggerHandler>
(
target: this.transform.root.gameObject,
eventData: null,
functor: (reciever, eventData) => reciever.OnTransferredTriggerExit(other.attachedRigidbody, this.fingerType)
);
}
}
public void ControlSpeech(bool isTaskFinished)
{
if (!this.isSpeechUsed)
{
return;
}
// Cancel current speech that can be canceled when task finished
try
{
if (isTaskFinished && this.latestSpeechInfo != null && this.latestSpeechInfo.canCancel && !this.speechProcess.HasExited)
{
this.speechProcess.Kill();
}
}
catch (Exception)
{
SIGVerseLogger.Warn("Couldn't terminate the speech process, but do nothing.");
// Do nothing even if an error occurs
}
if (this.speechInfoQue.Count <= 0)
{
return;
}
// Return if the current speech is not over
if (this.latestSpeechInfo != null && !this.speechProcess.HasExited)
{
return;
}
SpeechInfo speechInfo = this.speechInfoQue.Dequeue();
if (isTaskFinished && speechInfo.canCancel)
{
return;
}
this.latestSpeechInfo = speechInfo;
string message = this.latestSpeechInfo.message.Replace("_", " "); // Remove "_"
this.speechProcess.StartInfo.Arguments = "\"" + message + "\" \"Language=" + SpeechLanguage + "; Gender=" + this.latestSpeechInfo.gender + "\"";
try
{
this.speechProcess.Start();
SIGVerseLogger.Info("Spoke :" + message);
}
catch (Exception)
{
SIGVerseLogger.Warn("Could not speak :" + message);
}
}
private bool IsTrajectryMsgCorrect(ref SIGVerse.RosBridge.trajectory_msgs.JointTrajectory msg)
{
for (int i = 0; i < msg.points.Count; i++)
{
if (msg.joint_names.Count != msg.points[i].positions.Count)
{
SIGVerseLogger.Warn("Trajectry count error. (joint_names.Count = " + msg.joint_names.Count + ", msg.points[" + i + "].positions.Count = " + msg.points[i].positions.Count);
return(false);
}
}
if (msg.joint_names.Count == 1) // Torso
{
if (msg.joint_names.Contains(PR2Common.Joint.torso_lift_joint.ToString()))
{
return(true);
}
}
else if (msg.joint_names.Count == 2) // Head
{
if (msg.joint_names.Contains(PR2Common.Joint.head_pan_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.head_tilt_joint.ToString())
)
{
return(true);
}
}
else if (msg.joint_names.Count == 7) // Arm
{
// Left Arm
if (msg.joint_names.Contains(PR2Common.Joint.l_shoulder_pan_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.l_shoulder_lift_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.l_upper_arm_roll_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.l_elbow_flex_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.l_forearm_roll_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.l_wrist_flex_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.l_wrist_roll_joint.ToString())
)
{
return(true);
}
// Right Arm
if (msg.joint_names.Contains(PR2Common.Joint.r_shoulder_pan_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.r_shoulder_lift_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.r_upper_arm_roll_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.r_elbow_flex_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.r_forearm_roll_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.r_wrist_flex_joint.ToString()) &&
msg.joint_names.Contains(PR2Common.Joint.r_wrist_roll_joint.ToString())
)
{
return(true);
}
}
SIGVerseLogger.Warn("Wrong joint name or points. (" + this.topicName + ")");
return(false);
}
private EnvironmentInfo EnableEnvironment(List <GameObject> environments)
{
if (environments.Count != (from environment in environments select environment.name).Distinct().Count())
{
throw new Exception("There is the name conflict of environments.");
}
EnvironmentInfo environmentInfo = new EnvironmentInfo();
switch (this.executionMode)
{
// For the competition. Read generated data.
case ExecutionMode.Competition:
{
environmentInfo = this.GetEnvironmentInfo();
break;
}
// For data generation.
case ExecutionMode.DataGeneration:
{
GameObject activeEnvironment = (from environment in environments where environment.activeSelf == true select environment).SingleOrDefault();
if (activeEnvironment != null)
{
environmentInfo.environmentName = activeEnvironment.name;
SIGVerseLogger.Warn("Selected an active environment. name=" + activeEnvironment.name);
}
else
{
environmentInfo.environmentName = environments[UnityEngine.Random.Range(0, environments.Count)].name;
}
break;
}
default:
{
throw new Exception("Illegal Execution mode. mode=" + CleanupConfig.Instance.configFileInfo.executionMode);
}
}
foreach (GameObject environment in environments)
{
if (environment.name == environmentInfo.environmentName)
{
environment.SetActive(true);
}
else
{
environment.SetActive(false);
}
}
return(environmentInfo);
}
public void OnReceiveRosMessage(RosBridge.handyman.HandymanMsg handymanMsg)
{
if (this.receivedMessageMap.ContainsKey(handymanMsg.message))
{
// Check message order
if (handymanMsg.message == MsgIamReady)
{
if (this.step != ModeratorStep.WaitForIamReady)
{
SIGVerseLogger.Warn("Illegal timing. message : " + handymanMsg.message + ", step=" + this.step); return;
}
}
if (handymanMsg.message == MsgRoomReached)
{
if (this.step != ModeratorStep.WaitForRoomReached)
{
SIGVerseLogger.Warn("Illegal timing. message : " + handymanMsg.message + ", step=" + this.step); return;
}
}
if (handymanMsg.message == MsgDoesNotExist)
{
if (this.step != ModeratorStep.WaitForDoesNotExist && this.step != ModeratorStep.WaitForObjectGrasped)
{
SIGVerseLogger.Warn("Illegal timing. message : " + handymanMsg.message + ", step=" + this.step); return;
}
}
if (handymanMsg.message == MsgObjectGrasped)
{
if (this.step != ModeratorStep.WaitForDoesNotExist && this.step != ModeratorStep.WaitForObjectGrasped)
{
SIGVerseLogger.Warn("Illegal timing. message : " + handymanMsg.message + ", step=" + this.step); return;
}
}
if (handymanMsg.message == MsgTaskFinished)
{
if (this.step != ModeratorStep.WaitForTaskFinished)
{
SIGVerseLogger.Warn("Illegal timing. message : " + handymanMsg.message + ", step=" + this.step); return;
}
}
if (handymanMsg.message == MsgGiveUp)
{
this.OnGiveUp();
}
this.receivedMessageMap[handymanMsg.message] = true;
}
else
{
SIGVerseLogger.Warn("Received Illegal message : " + handymanMsg.message);
}
}
void Awake()
{
this.mainMenu = GameObject.Find(ChatManager.MainMenuName);
if (this.mainMenu == null)
{
SIGVerseLogger.Warn("Could not find MainMenu.");
}
}
public virtual void AddDataLine(string dataLine)
{
if (this.step != Step.Recording)
{
SIGVerseLogger.Warn("Illegal timing to add dataLine. data=" + dataLine);
return;
}
this.dataLines.Add(dataLine);
}
public void OnGiveUp()
{
if (this.step > ModeratorStep.TaskStart && this.step < ModeratorStep.WaitForNextTask)
{
this.interruptedReason = HandymanModerator.ReasonGiveUp;
}
else
{
SIGVerseLogger.Warn("It is a timing not allowed to give up.");
}
}
public void StopPlaybackRecord()
{
if (HumanNaviConfig.Instance.configInfo.playbackType == WorldPlaybackCommon.PlaybackTypeRecord)
{
bool isStopped = this.playbackRecorder.Stop();
if (!isStopped)
{
SIGVerseLogger.Warn("Cannot stop the world playback recording");
}
}
}
public void StartPlayback()
{
if (HandymanConfig.Instance.configFileInfo.playbackType == HandymanPlaybackCommon.PlaybackTypeRecord)
{
bool isStarted = this.playbackRecorder.Record();
if (!isStarted)
{
SIGVerseLogger.Warn("Cannot start the world playback recording");
}
}
}
public void StopAvatarMotionPlaybackRecorder()
{
// For data generation.
if (this.executionMode == ExecutionMode.DataGeneration)
{
bool isStopped = this.avatarMotionRecorder.Stop();
if (!isStopped)
{
SIGVerseLogger.Warn("Cannot stop the avatar motion recording");
}
}
}
public void StopAvatarMotionPlaybackPlayer()
{
// For the competition. Read generated data.
if (this.executionMode == ExecutionMode.Competition)
{
bool isStopped = this.avatarMotionPlayer.Stop();
if (!isStopped)
{
SIGVerseLogger.Warn("Cannot stop the avatar motion playing");
}
}
}
public void StopPlayback()
{
if (HandymanConfig.Instance.configFileInfo.playbackType == HandymanPlaybackCommon.PlaybackTypeRecord)
{
bool isStopped = this.playbackRecorder.Stop();
if (!isStopped)
{
SIGVerseLogger.Warn("Cannot stop the world playback recording");
}
}
this.StopAvatarMotionPlayback();
}
private bool IsTrajectryMsgCorrect(ref SIGVerse.RosBridge.trajectory_msgs.JointTrajectory msg)
{
for (int i = 0; i < msg.points.Count; i++)
{
if (msg.joint_names.Count != msg.points[i].positions.Count)
{
SIGVerseLogger.Warn("Trajectry count error. (joint_names.Count = " + msg.joint_names.Count + ", msg.points[" + i + "].positions.Count = " + msg.points[i].positions.Count);
return(false);
}
}
if (msg.joint_names.Count == 1) // Torso
{
if (msg.joint_names.Contains(TIAGoCommon.Joint.torso_lift_joint.ToString()))
{
return(true);
}
}
else if (msg.joint_names.Count == 2) // Head or Gripper
{
if (msg.joint_names.Contains(TIAGoCommon.Joint.head_1_joint.ToString()) &&
msg.joint_names.Contains(TIAGoCommon.Joint.head_2_joint.ToString()))
{
return(true);
}
if (msg.joint_names.Contains(TIAGoCommon.Joint.gripper_left_finger_joint.ToString()) &&
msg.joint_names.Contains(TIAGoCommon.Joint.gripper_right_finger_joint.ToString()))
{
return(true);
}
}
else if (msg.joint_names.Count == 7) // Arm
{
if (msg.joint_names.Contains(TIAGoCommon.Joint.arm_1_joint.ToString()) &&
msg.joint_names.Contains(TIAGoCommon.Joint.arm_2_joint.ToString()) &&
msg.joint_names.Contains(TIAGoCommon.Joint.arm_3_joint.ToString()) &&
msg.joint_names.Contains(TIAGoCommon.Joint.arm_4_joint.ToString()) &&
msg.joint_names.Contains(TIAGoCommon.Joint.arm_5_joint.ToString()) &&
msg.joint_names.Contains(TIAGoCommon.Joint.arm_6_joint.ToString()) &&
msg.joint_names.Contains(TIAGoCommon.Joint.arm_7_joint.ToString()))
{
return(true);
}
}
SIGVerseLogger.Warn("Wrong joint name or points. (" + this.topicName + ")");
return(false);
}
protected virtual void Awake()
{
this.chatManager = GameObject.Find(ChatManager.ChatManagerName);
if (this.chatManager == null)
{
SIGVerseLogger.Error("Could not find ChatManager!");
}
this.mainMenu = GameObject.Find(ChatManager.MainMenuName);
if (this.mainMenu == null)
{
SIGVerseLogger.Warn("Could not find MainMenu.");
}
}
protected override void SubscribeMessageCallback(SIGVerse.RosBridge.trajectory_msgs.JointTrajectory jointTrajectory)
{
if (jointTrajectory.joint_names.Count != jointTrajectory.points[0].positions.Count)
{
SIGVerseLogger.Warn("joint_names.Count != points.positions.Count topicName = " + this.topicName);
return;
}
const int Zero = 0;
for (int i = 0; i < jointTrajectory.joint_names.Count; i++)
{
string name = jointTrajectory.joint_names[i];
float position = TurtleBot3Common.GetClampedPosition(name, (float)jointTrajectory.points[Zero].positions[i]);
float duration = (float)jointTrajectory.points[Zero].time_from_start.secs + (float)jointTrajectory.points[Zero].time_from_start.nsecs * 1.0e-9f;
if (name == TurtleBot3Common.jointNameMap[JointType.Joint1])
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, -position, Time.time, TurtleBot3Common.GetCorrectedJointsEulerAngle(name, this.joint1Link.localEulerAngles.z) * Mathf.Deg2Rad);
}
if (name == TurtleBot3Common.jointNameMap[JointType.Joint2])
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, -position, Time.time, TurtleBot3Common.GetCorrectedJointsEulerAngle(name, this.joint2Link.localEulerAngles.y) * Mathf.Deg2Rad);
}
if (name == TurtleBot3Common.jointNameMap[JointType.Joint3])
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, -position, Time.time, TurtleBot3Common.GetCorrectedJointsEulerAngle(name, this.joint3Link.localEulerAngles.y) * Mathf.Deg2Rad);
}
if (name == TurtleBot3Common.jointNameMap[JointType.Joint4])
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, -position, Time.time, TurtleBot3Common.GetCorrectedJointsEulerAngle(name, this.joint4Link.localEulerAngles.y) * Mathf.Deg2Rad);
}
if (name == TurtleBot3Common.jointNameMap[JointType.GripJoint])
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, -position, Time.time, this.gripJointLink.localPosition.y);
}
if (name == TurtleBot3Common.jointNameMap[JointType.GripJointSub])
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, +position, Time.time, -this.gripJointLink.localPosition.y);
}
}
}
private EnvironmentInfo EnableEnvironment(List <GameObject> environments)
{
if (environments.Count != (from environment in environments select environment.name).Distinct().Count())
{
throw new Exception("There is the name conflict of environments.");
}
EnvironmentInfo environmentInfo = null;
if (HandymanConfig.Instance.configFileInfo.isGraspableObjectsPositionRandom)
{
GameObject activeEnvironment = (from environment in environments where environment.activeSelf == true select environment).FirstOrDefault();
if (activeEnvironment != null)
{
this.environmentName = activeEnvironment.name;
SIGVerseLogger.Warn("Selected an active environment. name=" + activeEnvironment.name);
}
else
{
this.environmentName = environments[UnityEngine.Random.Range(0, environments.Count)].name;
}
}
else
{
environmentInfo = this.GetEnvironmentInfo();
this.environmentName = environmentInfo.environmentName;
}
foreach (GameObject environment in environments)
{
if (environment.name == this.environmentName)
{
environment.SetActive(true);
}
else
{
environment.SetActive(false);
}
}
return(environmentInfo);
}
public void OnReceiveRosMessage(RosBridge.human_navigation.HumanNaviMsg humanNaviMsg)
{
if (!this.isDuringSession)
{
SIGVerseLogger.Warn("Illegal timing [session is not started]");
return;
}
if (this.receivedMessageMap.ContainsKey(humanNaviMsg.message))
{
if (humanNaviMsg.message == MsgIamReady)
{
if (this.step != Step.WaitForIamReady)
{
SIGVerseLogger.Warn("Illegal timing [message : " + humanNaviMsg.message + ", step:" + this.step + "]");
return;
}
}
if (humanNaviMsg.message == MsgGetAvatarStatus)
{
this.SendRosAvatarStatusMessage();
}
if (humanNaviMsg.message == MsgGetObjectStatus)
{
this.SendRosObjectStatusMessage();
}
if (humanNaviMsg.message == MsgGetSpeechState)
{
this.SendRosHumanNaviMessage(MsgSpeechState, this.sessionManager.GetSeechRunStateMsgString());
}
if (humanNaviMsg.message == MsgGiveUp)
{
this.OnGiveUp();
}
this.receivedMessageMap[humanNaviMsg.message] = true;
}
else
{
SIGVerseLogger.Warn("Received Illegal message [message: " + humanNaviMsg.message + "]");
}
}
private bool StopOmniIfOverLimitSpeed()
{
List <float> trajectoryInfoDurations = new List <float>(this.trajectoryInfoMap[HSRCommon.Joint.odom_x].Durations);
List <float> trajectoryInfoXGoalPositions = new List <float>(this.trajectoryInfoMap[HSRCommon.Joint.odom_x].GoalPositions);
List <float> trajectoryInfoYGoalPositions = new List <float>(this.trajectoryInfoMap[HSRCommon.Joint.odom_y].GoalPositions);
List <float> trajectoryInfoTGoalPositions = new List <float>(this.trajectoryInfoMap[HSRCommon.Joint.odom_t].GoalPositions);
trajectoryInfoDurations.Insert(0, 0.0f);
trajectoryInfoXGoalPositions.Insert(0, this.startPosition.x);
trajectoryInfoYGoalPositions.Insert(0, this.startPosition.y);
trajectoryInfoTGoalPositions.Insert(0, this.startRotation);
for (int i = 1; i < trajectoryInfoDurations.Count; i++)
{
double deltaTime = trajectoryInfoDurations[i] - trajectoryInfoDurations[i - 1];
double deltaDistanceX = trajectoryInfoXGoalPositions[i] - trajectoryInfoXGoalPositions[i - 1];
double deltaDistanceY = trajectoryInfoYGoalPositions[i] - trajectoryInfoYGoalPositions[i - 1];
double deltaDistanceXY = Math.Sqrt(Math.Pow(deltaDistanceX, 2) + Math.Pow(deltaDistanceY, 2));
double deltaAngle = Math.Abs(trajectoryInfoTGoalPositions[i] - trajectoryInfoTGoalPositions[i - 1]);
if (deltaAngle < -Math.PI)
{
deltaAngle += (2 * Math.PI);
}
if (deltaAngle > Math.PI)
{
deltaAngle -= (2 * Math.PI);
}
double linearSpeed = deltaDistanceXY / deltaTime;
double angularSpeed = deltaAngle / deltaTime;
if (linearSpeed > HSRCommon.MaxSpeedBase || angularSpeed > HSRCommon.MaxSpeedBaseRad)
{
this.trajectoryInfoMap[HSRCommon.Joint.odom_x] = null;
this.trajectoryInfoMap[HSRCommon.Joint.odom_y] = null;
this.trajectoryInfoMap[HSRCommon.Joint.odom_t] = null;
SIGVerseLogger.Warn("Omni speed error. (linear_speed = " + linearSpeed + " [m/s], angular_speed = " + angularSpeed + "[rad/s].");
return(true);
}
}
return(false);
}
public void OnGiveUp()
{
if (this.isDuringSession)
{
this.interruptedReason = HumanNaviModerator.ReasonGiveUp;
this.SendRosHumanNaviMessage(MsgTaskFailed, ReasonGiveUp);
this.SendPanelNotice("Give up", 100, PanelNoticeStatus.Red);
base.StartCoroutine(this.ShowNoticeMessagePanelForAvatar("Give up", 3.0f));
this.panelMainController.giveUpPanel.SetActive(false);
this.TaskFinished();
}
else
{
SIGVerseLogger.Warn("It is a timing not allowed to give up.");
}
}
public bool SpeakMessage(string message)
{
if (this.isSpeaking)
{
SIGVerseLogger.Info("Text-To-Speech: isSpeaking");
try
{
if (this.speechProcess.HasExited)
{
this.speechProcess.Kill();
}
}
catch (Exception)
{
SIGVerseLogger.Warn("Do nothing even if an error occurs");
// Do nothing even if an error occurs
}
}
string truncatedMessage;
if (message.Length > maxCharcters)
{
truncatedMessage = message.Substring(0, maxCharcters);
SIGVerseLogger.Info("Length of guidance message is over " + this.maxCharcters.ToString() + " charcters.");
}
else
{
truncatedMessage = message;
}
// speak
string settings = "Language=" + this.language + "; Gender=" + this.gender;
this.speechProcess.StartInfo.Arguments = "\"" + truncatedMessage + "\" \"" + settings + "\"";
this.speechProcess.Start();
this.isSpeaking = true;
return(true);
}
private bool IsTrajectryMsgCorrect(ref SIGVerse.RosBridge.trajectory_msgs.JointTrajectory msg)
{
for (int i = 0; i < msg.points.Count; i++)
{
if (msg.joint_names.Count != msg.points[i].positions.Count)
{
SIGVerseLogger.Warn("Trajectry count error. (joint_names.Count = " + msg.joint_names.Count + ", msg.points[" + i + "].positions.Count = " + msg.points[i].positions.Count);
return(false);
}
}
if (msg.joint_names.Count == 2) // Head
{
if (msg.joint_names.Contains(HSRCommon.Joint.head_pan_joint.ToString()) &&
msg.joint_names.Contains(HSRCommon.Joint.head_tilt_joint.ToString()))
{
return(true);
}
}
else if (msg.joint_names.Count == 5) // Arm
{
if (msg.joint_names.Contains(HSRCommon.Joint.wrist_flex_joint.ToString()) &&
msg.joint_names.Contains(HSRCommon.Joint.wrist_roll_joint.ToString()) &&
msg.joint_names.Contains(HSRCommon.Joint.arm_lift_joint.ToString()) &&
msg.joint_names.Contains(HSRCommon.Joint.arm_flex_joint.ToString()) &&
msg.joint_names.Contains(HSRCommon.Joint.arm_roll_joint.ToString()))
{
return(true);
}
}
else if (msg.joint_names.Count == 1) // Hand
{
if (msg.joint_names.Contains(HSRCommon.Joint.hand_motor_joint.ToString()))
{
return(true);
}
}
SIGVerseLogger.Warn("Wrong joint name or points. (" + this.topicName + ")");
return(false);
}
