private void SetTrajectoryInfoMap(ref SIGVerse.RosBridge.trajectory_msgs.JointTrajectory msg)
{
for (int i = 0; i < msg.joint_names.Count; i++)
{
HSRCommon.Joint joint = (HSRCommon.Joint)Enum.Parse(typeof(HSRCommon.Joint), msg.joint_names[i]);
List <float> positions = new List <float>();
List <float> durations = new List <float>();
for (int pointIndex = 0; pointIndex < msg.points.Count; pointIndex++)
{
positions.Add(HSRCommon.GetClampedPosition((float)msg.points[pointIndex].positions[i], joint));
durations.Add((float)msg.points[pointIndex].time_from_start.secs + (float)msg.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
}
switch (joint)
{
case HSRCommon.Joint.arm_lift_joint: { this.SetJointTrajectoryPosition(joint, durations, positions, +this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ); break; }
case HSRCommon.Joint.arm_flex_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.armFlexLink.localEulerAngles.y); break; }
case HSRCommon.Joint.arm_roll_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -this.armRollLink.localEulerAngles.z); break; }
case HSRCommon.Joint.wrist_flex_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.wristFlexLink.localEulerAngles.y); break; }
case HSRCommon.Joint.wrist_roll_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -this.wristRollLink.localEulerAngles.z); break; }
case HSRCommon.Joint.head_pan_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, -this.headPanLink.localEulerAngles.z); break; }
case HSRCommon.Joint.head_tilt_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.headTiltLink.localEulerAngles.y); break; }
case HSRCommon.Joint.hand_motor_joint: { this.SetJointTrajectoryRotation(joint, durations, positions, +this.handMotorDummyLink.localEulerAngles.x); break; }
}
}
}
private void SetTrajectoryInfoMap(ref SIGVerse.RosBridge.trajectory_msgs.JointTrajectory msg)
{
for (int i = 0; i < msg.joint_names.Count; i++)
{
HSRCommon.Joint joint = (HSRCommon.Joint)Enum.Parse(typeof(HSRCommon.Joint), msg.joint_names[i]);
List <float> positions = new List <float>();
List <float> durations = new List <float>();
for (int pointIndex = 0; pointIndex < msg.points.Count; pointIndex++)
{
positions.Add(HSRCommon.GetClampedPosition((float)msg.points[pointIndex].positions[i], joint));
durations.Add((float)msg.points[pointIndex].time_from_start.secs + (float)msg.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
}
switch (joint)
{
case HSRCommon.Joint.odom_x:
case HSRCommon.Joint.odom_y:
case HSRCommon.Joint.odom_t: { this.trajectoryInfoMap[joint] = new TrajectoryData(Time.time, durations, positions); break; }
default: { throw new Exception("NOT odom joint"); }
}
}
}
protected override void Update()
{
base.Update();
this.elapsedTime += UnityEngine.Time.deltaTime;
if (this.elapsedTime < this.sendingInterval * 0.001)
{
return;
}
this.elapsedTime = 0.0f;
List <double> positions = new List <double>()
{
{ this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ }, //1 arm_lift_joint
{ HSRCommon.GetNormalizedJointEulerAngle(+this.armFlexLink.localEulerAngles.y, HSRCommon.Joint.arm_flex_joint) * Mathf.Deg2Rad }, // 2
{ HSRCommon.GetNormalizedJointEulerAngle(-this.armRollLink.localEulerAngles.z, HSRCommon.Joint.arm_roll_joint) * Mathf.Deg2Rad }, // 3
{ HSRCommon.GetNormalizedJointEulerAngle(+this.wristFlexLink.localEulerAngles.y, HSRCommon.Joint.wrist_flex_joint) * Mathf.Deg2Rad }, // 4
{ HSRCommon.GetNormalizedJointEulerAngle(-this.wristRollLink.localEulerAngles.z, HSRCommon.Joint.wrist_roll_joint) * Mathf.Deg2Rad }, // 5
{ HSRCommon.GetNormalizedJointEulerAngle(-this.headPanLink.localEulerAngles.z, HSRCommon.Joint.head_pan_joint) * Mathf.Deg2Rad }, // 6
{ HSRCommon.GetNormalizedJointEulerAngle(+this.headTiltLink.localEulerAngles.y, HSRCommon.Joint.head_tilt_joint) * Mathf.Deg2Rad }, // 7
{ HSRCommon.GetNormalizedJointEulerAngle(+this.handLProximalLink.localEulerAngles.x, HSRCommon.Joint.hand_l_proximal_joint) * Mathf.Deg2Rad }, // 8 hand_l_spring_proximal_joint
{ HSRCommon.GetNormalizedJointEulerAngle(+this.handRProximalLink.localEulerAngles.x, HSRCommon.Joint.hand_r_proximal_joint) * Mathf.Deg2Rad }, // 9 hand_r_spring_proximal_joint
};
this.jointState.header.Update();
this.jointState.position = positions;
this.publisher.Publish(this.jointState);
}
void Awake()
{
this.armLiftLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmLiftLinkName);
this.armFlexLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmFlexLinkName);
this.armRollLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmRollLinkName);
this.wristFlexLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.WristFlexLinkName);
this.wristRollLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.WristRollLinkName);
this.headPanLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HeadPanLinkName);
this.headTiltLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HeadTiltLinkName);
this.torsoLiftLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.TorsoLiftLinkName);
this.handLProximalLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HandLProximalLinkName);
this.handRProximalLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HandRProximalLinkName);
this.armLiftLinkIniPosZ = this.armLiftLink.localPosition.z;
this.torsoLiftLinkIniPosZ = this.torsoLiftLink.localPosition.z;
this.trajectoryInfoMap = new Dictionary <string, TrajectoryInfo>();
this.trajectoryInfoMap.Add(HSRCommon.ArmLiftJointName, null);
this.trajectoryInfoMap.Add(HSRCommon.ArmFlexJointName, null);
this.trajectoryInfoMap.Add(HSRCommon.ArmRollJointName, null);
this.trajectoryInfoMap.Add(HSRCommon.WristFlexJointName, null);
this.trajectoryInfoMap.Add(HSRCommon.WristRollJointName, null);
this.trajectoryInfoMap.Add(HSRCommon.HeadPanJointName, null);
this.trajectoryInfoMap.Add(HSRCommon.HeadTiltJointName, null);
this.trajectoryInfoMap.Add(HSRCommon.HandLProximalJointName, null);
this.trajectoryInfoMap.Add(HSRCommon.HandRProximalJointName, null);
}
protected void FixedUpdate()
{
foreach (HSRCommon.Joint joint in this.trajectoryKeyList)
{
if (this.trajectoryInfoMap[joint] == null)
{
continue;
}
switch (joint)
{
case HSRCommon.Joint.arm_lift_joint:
{
float newPos = GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, joint);
this.armLiftLink.localPosition = new Vector3(this.armLiftLink.localPosition.x, this.armLiftLink.localPosition.y, this.armLiftLinkIniPosZ + newPos);
this.torsoLiftLink.localPosition = new Vector3(this.torsoLiftLink.localPosition.x, this.torsoLiftLink.localPosition.y, this.torsoLiftLinkIniPosZ + newPos / 2.0f);
break;
}
case HSRCommon.Joint.arm_flex_joint: { this.UpdateLinkAngle(this.armFlexLink, joint, Vector3.up); break; }
case HSRCommon.Joint.arm_roll_joint: { this.UpdateLinkAngle(this.armRollLink, joint, Vector3.back); break; }
case HSRCommon.Joint.wrist_flex_joint: { this.UpdateLinkAngle(this.wristFlexLink, joint, Vector3.up); break; }
case HSRCommon.Joint.wrist_roll_joint: { this.UpdateLinkAngle(this.wristRollLink, joint, Vector3.back); break; }
case HSRCommon.Joint.head_pan_joint: { this.UpdateLinkAngle(this.headPanLink, joint, Vector3.back); break; }
case HSRCommon.Joint.head_tilt_joint: { this.UpdateLinkAngle(this.headTiltLink, joint, Vector3.up); break; }
case HSRCommon.Joint.hand_motor_joint:
{
float newPos = HSRCommon.GetNormalizedJointEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, joint) * Mathf.Rad2Deg, joint);
// Grasping and hand closing
if (this.graspedObject != null && this.IsAngleDecreasing(newPos, this.handMotorDummyLink.localEulerAngles.x))
{
// Have to stop
this.trajectoryInfoMap[joint] = null;
}
// Otherwise
else
{
this.handMotorDummyLink.localEulerAngles = new Vector3(+newPos, this.handMotorDummyLink.localEulerAngles.y, this.handMotorDummyLink.localEulerAngles.z);
this.handLProximalLink.localEulerAngles = new Vector3(+newPos, this.handLProximalLink.localEulerAngles.y, this.handLProximalLink.localEulerAngles.z);
this.handRProximalLink.localEulerAngles = new Vector3(-newPos, this.handRProximalLink.localEulerAngles.y, this.handRProximalLink.localEulerAngles.z);
this.handLDistalLink.localEulerAngles = new Vector3(-newPos, this.handLDistalLink.localEulerAngles.y, this.handLDistalLink.localEulerAngles.z);
this.handRDistalLink.localEulerAngles = new Vector3(+newPos, this.handRDistalLink.localEulerAngles.y, this.handRDistalLink.localEulerAngles.z);
}
break;
}
}
}
}
void Awake()
{
this.armLiftLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmLiftLinkName);
this.armFlexLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmFlexLinkName);
this.armRollLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.ArmRollLinkName);
this.wristFlexLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.WristFlexLinkName);
this.wristRollLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.WristRollLinkName);
this.headPanLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HeadPanLinkName);
this.headTiltLink = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.HeadTiltLinkName);
this.armLiftLinkIniPosZ = this.armLiftLink.localPosition.z;
}
private static float GetPositionAndUpdateTrajectory(Dictionary <HSRCommon.Joint, TrajectoryInfo> trajectoryInfoMap, HSRCommon.Joint joint)
{
float minSpeed = HSRCommon.GetMinJointSpeed(joint);
float maxSpeed = HSRCommon.GetMaxJointSpeed(joint);
TrajectoryInfo trajectoryInfo = trajectoryInfoMap[joint];
int targetPointIndex = GetTargetPointIndex(trajectoryInfo);
float speed = 0.0f;
if (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime >= trajectoryInfo.Durations[targetPointIndex])
{
speed = maxSpeed;
}
else
{
speed = Mathf.Abs((trajectoryInfo.GoalPositions[targetPointIndex] - trajectoryInfo.CurrentPosition) / (trajectoryInfo.Durations[targetPointIndex] - (trajectoryInfo.CurrentTime - trajectoryInfo.StartTime)));
speed = Mathf.Clamp(speed, minSpeed, maxSpeed);
}
// Calculate position
float newPosition;
float movingDistance = speed * (Time.time - trajectoryInfo.CurrentTime);
if (movingDistance > Mathf.Abs(trajectoryInfo.GoalPositions[targetPointIndex] - trajectoryInfo.CurrentPosition))
{
newPosition = trajectoryInfo.GoalPositions[targetPointIndex];
trajectoryInfoMap[joint] = null;
}
else
{
trajectoryInfo.CurrentTime = Time.time;
if (trajectoryInfo.GoalPositions[targetPointIndex] > trajectoryInfo.CurrentPosition)
{
trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition + movingDistance;
newPosition = trajectoryInfo.CurrentPosition;
}
else
{
trajectoryInfo.CurrentPosition = trajectoryInfo.CurrentPosition - movingDistance;
newPosition = trajectoryInfo.CurrentPosition;
}
}
return(newPosition);
}
private void UpdateLinkAngle(Transform link, HSRCommon.Joint joint, Vector3 axis)
{
float newPos = HSRCommon.GetNormalizedJointEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, joint) * Mathf.Rad2Deg, joint);
if (Mathf.Abs(axis.x) == 1)
{
link.localEulerAngles = new Vector3(0.0f, link.localEulerAngles.y, link.localEulerAngles.z) + newPos * axis;
}
else if (Mathf.Abs(axis.y) == 1)
{
link.localEulerAngles = new Vector3(link.localEulerAngles.x, 0.0f, link.localEulerAngles.z) + newPos * axis;
}
else if (Mathf.Abs(axis.z) == 1)
{
link.localEulerAngles = new Vector3(link.localEulerAngles.x, link.localEulerAngles.y, 0.0f) + newPos * axis;
}
}
void Awake()
{
List <UnityEngine.Transform> localLinkList = HSRCommon.GetLinksInChildren(this.transform.root);
foreach (UnityEngine.Transform localLink in localLinkList)
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = localLink.parent.name;
localTransformStamped.child_frame_id = localLink.name;
TfInfo localTfInfo = new TfInfo(localLink, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
this.publishSequenceNumber = HSRPubSynchronizer.GetAssignedSequenceNumber();
}
void Start()
{
if (this.rosBridgeIP.Equals(string.Empty))
{
this.rosBridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
}
if (this.sigverseBridgePort == 0)
{
this.sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort;
}
this.tcpClient = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);
this.networkStream = this.tcpClient.GetStream();
this.networkStream.ReadTimeout = 100000;
this.networkStream.WriteTimeout = 100000;
this.transformStampedMsg = new SIGVerseROSBridgeMessage <TransformStamped[]>("publish", this.topicName, "sigverse/TfList", null);
}
protected override void Update()
{
base.Update();
this.elapsedTime += UnityEngine.Time.deltaTime;
if (this.elapsedTime < this.sendingInterval * 0.001)
{
return;
}
this.elapsedTime = 0.0f;
List <double> positions = new List <double>();
//1 ArmLiftJoint
positions.Add(this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ);
//2 ArmFlexJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.armFlexLink.localEulerAngles.y, HSRCommon.ArmFlexJointName) * Mathf.Deg2Rad);
//3 ArmRollJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(-this.armRollLink.localEulerAngles.z, HSRCommon.ArmRollJointName) * Mathf.Deg2Rad);
//4 WristFlexJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.wristFlexLink.localEulerAngles.y, HSRCommon.WristFlexJointName) * Mathf.Deg2Rad);
//5 WristRollJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(-this.wristRollLink.localEulerAngles.z, HSRCommon.WristRollJointName) * Mathf.Deg2Rad);
//6 HeadPanJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(-this.headPanLink.localEulerAngles.z, HSRCommon.HeadPanJointName) * Mathf.Deg2Rad);
//7 HeadTiltJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.headTiltLink.localEulerAngles.y, HSRCommon.HeadTiltJointName) * Mathf.Deg2Rad);
//8 HandLSpringProximalJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.handLProximalLink.localEulerAngles.x, HSRCommon.HandLProximalJointName) * Mathf.Deg2Rad);
//9 HandRSpringProximalJoint
positions.Add(HSRCommon.GetCorrectedJointsEulerAngle(this.handRProximalLink.localEulerAngles.x, HSRCommon.HandRProximalJointName) * Mathf.Deg2Rad);
this.jointState.header.Update();
this.jointState.position = positions;
// float position = HSRCommon.GetClampedPosition(value, name);
this.publisher.Publish(this.jointState);
}
void Start()
{
if (this.rosBridgeIP.Equals(string.Empty))
{
this.rosBridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
}
if (this.sigverseBridgePort == 0)
{
this.sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort;
}
this.tcpClient = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);
this.networkStream = this.tcpClient.GetStream();
this.networkStream.ReadTimeout = 100000;
this.networkStream.WriteTimeout = 100000;
this.header = new Header(0, new SIGVerse.ROSBridge.msg_helpers.Time(0, 0), this.sensorLink.name);
this.laserScan = new LaserScanForSIGVerseBridge();
this.numLines = (int)Mathf.Round(LaserAngle / LaserAngularResolution) + 1;
this.laserScan.header = this.header;
this.laserScan.angle_min = -HalfOfLaserAngle * Mathf.Deg2Rad;
this.laserScan.angle_max = +HalfOfLaserAngle * Mathf.Deg2Rad;
this.laserScan.angle_increment = LaserAngularResolution * Mathf.Deg2Rad;
this.laserScan.time_increment = 0.0;
this.laserScan.scan_time = 0.1; // tentative
this.laserScan.range_min = 0.05;
this.laserScan.range_max = 20.0;
this.laserScan.ranges = new double[this.numLines];
this.laserScan.intensities = new double[this.numLines];
this.laserScanMsg = new SIGVerseROSBridgeMessage <LaserScanForSIGVerseBridge>("publish", this.topicName, LaserScanForSIGVerseBridge.GetMessageType(), this.laserScan);
// Debug.Log("this.layerMask.value = "+this.layerMask.value);
}
void Awake()
{
List <UnityEngine.Transform> localLinkList = HSRCommon.GetLinksInChildren(this.transform.root);
foreach (UnityEngine.Transform localLink in localLinkList)
{
if (localLink.name == HSRCommon.Link.base_footprint.ToString())
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = HSRCommon.OdomName;
localTransformStamped.child_frame_id = localLink.name;
UnityEngine.Transform baseFootprintRigidbody = SIGVerseUtils.FindTransformFromChild(this.transform.root, HSRCommon.BaseFootPrintRigidbodyName);
TfInfo localTfInfo = new TfInfo(baseFootprintRigidbody, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
else
{
TransformStamped localTransformStamped = new TransformStamped();
localTransformStamped.header.frame_id = localLink.parent.name;
localTransformStamped.child_frame_id = localLink.name;
TfInfo localTfInfo = new TfInfo(localLink, localTransformStamped);
this.localTfInfoList.Add(localTfInfo);
}
}
this.synchronizer = this.GetComponent <HSRPubSynchronizer>();
this.publishSequenceNumber = this.synchronizer.GetAssignedSequenceNumber();
this.isUsingThread = this.synchronizer.useThread;
}
public void Initialize(string rosBridgeIP, int sigverseBridgePort, string topicNameCameraInfo, string topicNameImage, bool isRight)
{
if (rosBridgeIP.Equals(string.Empty))
{
rosBridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
}
if (sigverseBridgePort == 0)
{
sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort;
}
this.tcpClientCameraInfo = HSRCommon.GetSIGVerseRosbridgeConnection(rosBridgeIP, sigverseBridgePort);
this.tcpClientImage = HSRCommon.GetSIGVerseRosbridgeConnection(rosBridgeIP, sigverseBridgePort);
this.networkStreamCameraInfo = this.tcpClientCameraInfo.GetStream();
this.networkStreamCameraInfo.ReadTimeout = 100000;
this.networkStreamCameraInfo.WriteTimeout = 100000;
this.networkStreamImage = this.tcpClientImage.GetStream();
this.networkStreamImage.ReadTimeout = 100000;
this.networkStreamImage.WriteTimeout = 100000;
// RGB Camera
this.rgbCamera = this.cameraFrameObj.GetComponentInChildren <Camera>();
int imageWidth = this.rgbCamera.targetTexture.width;
int imageHeight = this.rgbCamera.targetTexture.height;
this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false);
// [camera/rgb/CameraInfo]
string distortionModel = "plumb_bob";
double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 };
double[] K = { 968.765, 0.0, 640, 0.0, 968.77, 480, 0.0, 0.0, 1.0 };
double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
double[] P = { 968.765, 0.0, 640, 0.0, 0.0, 968.77, 480, 0.0, 0.0, 0.0, 1.0, 0.0 };
if (isRight)
{
P[3] = -135.627; // -135.627 = - 968.765 * 0.14(baseline=distance between both eyes)
}
//double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 };
//double[] K = { 968.7653251755174, 0.0, 640.5, 0.0, 968.7653251755174, 480.5, 0.0, 0.0, 1.0 };
//double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
// Left Camera
//double[] P = { 968.7653251755174, 0.0, 640.5, -0.0, 0.0, 968.7653251755174, 480.5, 0.0, 0.0, 0.0, 1.0, 0.0 };
// Right Camera
//double[] P = { 968.7653251755174, 0.0, 640.5, -135.62714552457246, 0.0, 968.7653251755174, 480.5, 0.0, 0.0, 0.0, 1.0, 0.0 };
RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false);
this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi);
// [camera/rgb/Image_raw]
string encoding = "rgb8";
byte isBigendian = 0;
uint step = (uint)imageWidth * 3;
this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null);
this.header = new Header(0, new SIGVerse.ROSBridge.msg_helpers.Time(0, 0), this.cameraFrameObj.name);
this.cameraInfoMsg = new SIGVerseROSBridgeMessage <CameraInfoForSIGVerseBridge>("publish", topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData);
this.imageMsg = new SIGVerseROSBridgeMessage <ImageForSIGVerseBridge> ("publish", topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData);
}
void Awake()
{
this.baseFootPrint = HSRCommon.FindGameObjectFromChild(this.transform.root, HSRCommon.BaseFootPrintName);
}
private static bool IsOverLimitSpeed(HSRCommon.Joint joint, double speed)
{
return(speed > HSRCommon.GetMaxJointSpeed(joint));
}
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;
}
}
}
}
private void SetJointTrajectoryRotation(HSRCommon.Joint joint, List <float> durations, List <float> goalPositions, float currentPosition)
{
this.trajectoryInfoMap[joint] = new TrajectoryInfo(durations, goalPositions, HSRCommon.GetNormalizedJointEulerAngle(currentPosition, joint) * Mathf.Deg2Rad);
}
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;
}
for (int i = 0; i < jointTrajectory.joint_names.Count; i++)
{
string name = jointTrajectory.joint_names[i];
List <float> positions = new List <float>();
List <float> durations = new List <float>();
for (int pointIndex = 0; pointIndex < jointTrajectory.points.Count; pointIndex++)
{
positions.Add(HSRCommon.GetClampedPosition((float)jointTrajectory.points[pointIndex].positions[i], name));
durations.Add((float)jointTrajectory.points[pointIndex].time_from_start.secs + (float)jointTrajectory.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
}
if (name == HSRCommon.ArmLiftJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ);
}
if (name == HSRCommon.ArmFlexJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.armFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.ArmRollJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.armRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.WristFlexJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.wristFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.WristRollJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.wristRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HeadPanJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.headPanLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HeadTiltJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.headTiltLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HandLProximalJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handLProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HandRProximalJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handRProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
}
}
}
public void JointTrajectoryCallback(SIGVerse.ROSBridge.trajectory_msgs.JointTrajectory jointTrajectory)
{
//List<string> jointNames = jointTrajectory.joint_names;
//List<SIGVerse.ROSBridge.trajectory_msgs.JointTrajectoryPoint> points = jointTrajectory.points;
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 = HSRCommon.GetClampedPosition((float)jointTrajectory.points[Zero].positions[i], name);
float duration = (float)jointTrajectory.points[Zero].time_from_start.secs + (float)jointTrajectory.points[Zero].time_from_start.nsecs * 1.0e-9f;
// Debug.Log("Duration="+ duration);
if (name == HSRCommon.ArmLiftJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, this.armLiftLink.localPosition.z - this.armLiftLinkIniPosZ);
}
if (name == HSRCommon.ArmFlexJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.armFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.ArmRollJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.armRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.WristFlexJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.wristFlexLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.WristRollJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.wristRollLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HeadPanJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(-this.headPanLink.localEulerAngles.z, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HeadTiltJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.headTiltLink.localEulerAngles.y, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HandLProximalJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handLProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
}
if (name == HSRCommon.HandRProximalJointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, duration, position, Time.time, HSRCommon.GetCorrectedJointsEulerAngle(this.handRProximalLink.localEulerAngles.x, name) * Mathf.Deg2Rad);
}
}
}
void Update()
{
if (this.webSocketConnection == null || !this.webSocketConnection.IsConnected)
{
return;
}
foreach (string jointName in this.trajectoryKeyList)
{
if (this.trajectoryInfoMap[jointName] != null)
{
if (jointName == HSRCommon.ArmLiftJointName)
{
float newPos = GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeed, HSRCommon.MaxSpeedTorso);
this.armLiftLink.localPosition = new Vector3(this.armLiftLink.localPosition.x, this.armLiftLink.localPosition.y, this.armLiftLinkIniPosZ + newPos);
this.torsoLiftLink.localPosition = new Vector3(this.torsoLiftLink.localPosition.x, this.torsoLiftLink.localPosition.y, this.torsoLiftLinkIniPosZ + newPos / 2.0f);
}
if (jointName == HSRCommon.ArmFlexJointName)
{
float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
this.armFlexLink.localEulerAngles = new Vector3(this.armFlexLink.localEulerAngles.x, newPos, this.armFlexLink.localEulerAngles.z);
}
if (jointName == HSRCommon.ArmRollJointName)
{
float newPos = -HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
this.armRollLink.localEulerAngles = new Vector3(this.armRollLink.localEulerAngles.x, this.armRollLink.localEulerAngles.y, newPos);
}
if (jointName == HSRCommon.WristFlexJointName)
{
float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
this.wristFlexLink.localEulerAngles = new Vector3(this.wristFlexLink.localEulerAngles.x, newPos, this.wristFlexLink.localEulerAngles.z);
}
if (jointName == HSRCommon.WristRollJointName)
{
float newPos = -HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
this.wristRollLink.localEulerAngles = new Vector3(this.wristRollLink.localEulerAngles.x, this.wristRollLink.localEulerAngles.y, newPos);
}
if (jointName == HSRCommon.HeadPanJointName)
{
float newPos = -HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedHead) * Mathf.Rad2Deg, jointName);
this.headPanLink.localEulerAngles = new Vector3(this.headPanLink.localEulerAngles.x, this.headPanLink.localEulerAngles.y, newPos);
}
if (jointName == HSRCommon.HeadTiltJointName)
{
float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedHead) * Mathf.Rad2Deg, jointName);
this.headTiltLink.localEulerAngles = new Vector3(this.headTiltLink.localEulerAngles.x, newPos, this.headTiltLink.localEulerAngles.z);
}
if (jointName == HSRCommon.HandLProximalJointName)
{
float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
// Grasping and hand closing
if (this.graspedObject != null && this.IsAngleDecreasing(newPos, this.handLProximalLink.localEulerAngles.x))
{
// Have to stop
this.trajectoryInfoMap[jointName] = null;
}
// Otherwise
else
{
this.handLProximalLink.localEulerAngles = new Vector3(newPos, this.handLProximalLink.localEulerAngles.y, this.handLProximalLink.localEulerAngles.z);
}
}
if (jointName == HSRCommon.HandRProximalJointName)
{
float newPos = HSRCommon.GetCorrectedJointsEulerAngle(GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, HSRCommon.MinSpeedRad, HSRCommon.MaxSpeedArm) * Mathf.Rad2Deg, jointName);
// Grasping and hand closing
if (this.graspedObject != null && this.IsAngleIncreasing(newPos, this.handRProximalLink.localEulerAngles.x))
{
// Have to stop
this.trajectoryInfoMap[jointName] = null;
}
// Otherwise
else
{
this.handRProximalLink.localEulerAngles = new Vector3(newPos, this.handRProximalLink.localEulerAngles.y, this.handRProximalLink.localEulerAngles.z);
}
}
}
}
this.webSocketConnection.Render();
}
void Start()
{
if (this.rosBridgeIP.Equals(string.Empty))
{
this.rosBridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
}
if (this.sigverseBridgePort == 0)
{
this.sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort;
}
this.tcpClientCameraInfo = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);
this.tcpClientImage = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);
this.networkStreamCameraInfo = this.tcpClientCameraInfo.GetStream();
this.networkStreamCameraInfo.ReadTimeout = 100000;
this.networkStreamCameraInfo.WriteTimeout = 100000;
this.networkStreamImage = this.tcpClientImage.GetStream();
this.networkStreamImage.ReadTimeout = 100000;
this.networkStreamImage.WriteTimeout = 100000;
// RGB Camera
this.xtionRGBCamera = this.rgbCamera.GetComponentInChildren <Camera>();
int imageWidth = this.xtionRGBCamera.targetTexture.width;
int imageHeight = this.xtionRGBCamera.targetTexture.height;
this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false);
// [camera/rgb/CameraInfo]
string distortionModel = "plumb_bob";
double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 };
double[] K = { 554, 0.0, 320, 0.0, 554, 240, 0.0, 0.0, 1.0 };
double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
double[] P = { 554, 0.0, 320, 0.0, 0.0, 554, 240, 0.0, 0.0, 0.0, 1.0, 0.0 };
//double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 };
//double[] K = { 554.3827128226441, 0.0, 320.5, 0.0, 554.3827128226441, 240.5, 0.0, 0.0, 1.0 };
//double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
//double[] P = { 554.3827128226441, 0.0, 320.5, 0.0, 0.0, 554.3827128226441, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0 };
RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false);
this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi);
// [camera/rgb/Image_raw]
string encoding = "rgb8";
byte isBigendian = 0;
uint step = (uint)imageWidth * 3;
this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null);
this.header = new Header(0, new SIGVerse.ROSBridge.msg_helpers.Time(0, 0), this.rgbCamera.name);
this.cameraInfoMsg = new SIGVerseROSBridgeMessage <CameraInfoForSIGVerseBridge>("publish", this.topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData);
this.imageMsg = new SIGVerseROSBridgeMessage <ImageForSIGVerseBridge> ("publish", this.topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData);
}
protected override void SubscribeMessageCallback(SIGVerse.RosBridge.trajectory_msgs.JointTrajectory jointTrajectory)
{
this.startPosition = this.baseFootprintRigidbody.position;//save start position.
this.startRotation = this.baseFootprintRigidbody.rotation;
if (jointTrajectory.joint_names.Count != jointTrajectory.points[0].positions.Count)
{
SIGVerseLogger.Warn("joint_names.Count != points.positions.Count topicName = " + this.topicName);
return;
}
else if (jointTrajectory.joint_names.Count != 3)
{
SIGVerseLogger.Warn("joint_names.Count != 3");
return;
}
for (int i = 0; i < jointTrajectory.joint_names.Count; i++)
{
string name = jointTrajectory.joint_names[i];
List <float> positions = new List <float>();
List <float> durations = new List <float>();
for (int pointIndex = 0; pointIndex < jointTrajectory.points.Count; pointIndex++)
{
positions.Add(HSRCommon.GetClampedPosition((float)jointTrajectory.points[pointIndex].positions[i], name));
durations.Add((float)jointTrajectory.points[pointIndex].time_from_start.secs + (float)jointTrajectory.points[pointIndex].time_from_start.nsecs * 1.0e-9f);
}
if (name == HSRCommon.OmniOdomX_JointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, this.baseFootprintRigidbody.position.z);
}
if (name == HSRCommon.OmniOdomY_JointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, this.baseFootprintRigidbody.position.x);
}
if (name == HSRCommon.OmniOdomT_JointName)
{
this.trajectoryInfoMap[name] = new TrajectoryInfo(Time.time, durations, positions, Time.time, 0.0f);
}
}
if (trajectoryInfoMap[HSRCommon.OmniOdomX_JointName] == null || trajectoryInfoMap[HSRCommon.OmniOdomY_JointName] == null || trajectoryInfoMap[HSRCommon.OmniOdomT_JointName] == null)
{
trajectoryInfoMap[HSRCommon.OmniOdomX_JointName] = null;
trajectoryInfoMap[HSRCommon.OmniOdomY_JointName] = null;
trajectoryInfoMap[HSRCommon.OmniOdomT_JointName] = null;
SIGVerseLogger.Warn("Omni trajectry args error.");
}
//check limit speed
for (int i = 0; i < trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions.Count; i++)
{
double linear_speed = 0;
if (i == 0)
{
double temp_distance = Math.Sqrt(Math.Pow(((trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions[0] + this.initialPosition.z) - this.startPosition.z) * 100, 2) + Math.Pow(((trajectoryInfoMap[HSRCommon.OmniOdomY_JointName].GoalPositions[0] + this.initialPosition.x) - this.startPosition.x) * 100, 2));
linear_speed = (temp_distance / 100) / trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].Durations[0];
}
else
{
double temp_distance = Math.Sqrt(Math.Pow((trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions[i] - trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].GoalPositions[i - 1]) * 100, 2) + Math.Pow((trajectoryInfoMap[HSRCommon.OmniOdomY_JointName].GoalPositions[i] - trajectoryInfoMap[HSRCommon.OmniOdomY_JointName].GoalPositions[i - 1]) * 100, 2));
linear_speed = (temp_distance / 100) / (trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].Durations[i] - trajectoryInfoMap[HSRCommon.OmniOdomX_JointName].Durations[i - 1]);
}
double angular_speed = 0;
if (i == 0)
{
double temp_angle = Math.Abs((this.ToAngle(trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].GoalPositions[0]) + this.initialRotation.eulerAngles.y) - this.startRotation.eulerAngles.y);
if (temp_angle > Math.PI)
{
temp_angle -= Math.PI;
}
angular_speed = temp_angle / trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].Durations[0];
}
else
{
double temp_angle = Math.Abs(this.ToAngle(trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].GoalPositions[i]) - trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].GoalPositions[i - 1]);
if (temp_angle > Math.PI)
{
temp_angle -= Math.PI;
}
angular_speed = temp_angle / (trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].Durations[i] - trajectoryInfoMap[HSRCommon.OmniOdomT_JointName].Durations[i - 1]);
}
if (linear_speed > HSRCommon.MaxSpeedBase || angular_speed > this.ToAngle(HSRCommon.MaxSpeedBaseRad))//exceed limit
{
trajectoryInfoMap[HSRCommon.OmniOdomX_JointName] = null;
trajectoryInfoMap[HSRCommon.OmniOdomY_JointName] = null;
trajectoryInfoMap[HSRCommon.OmniOdomT_JointName] = null;
SIGVerseLogger.Warn("Omni trajectry args error. (exceed limit)");
return;
}
} //for
} //SubscribeMessageCallback