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 joint1
positions.Add(-TurtleBot3Common.GetCorrectedJointsEulerAngle(TurtleBot3Common.jointNameMap[JointType.Joint1], this.joint1Link.localEulerAngles.z) * Mathf.Deg2Rad);
//2 joint2
positions.Add(-TurtleBot3Common.GetCorrectedJointsEulerAngle(TurtleBot3Common.jointNameMap[JointType.Joint2], this.joint2Link.localEulerAngles.y) * Mathf.Deg2Rad);
//3 joint3
positions.Add(-TurtleBot3Common.GetCorrectedJointsEulerAngle(TurtleBot3Common.jointNameMap[JointType.Joint3], this.joint3Link.localEulerAngles.y) * Mathf.Deg2Rad);
//4 joint4
positions.Add(-TurtleBot3Common.GetCorrectedJointsEulerAngle(TurtleBot3Common.jointNameMap[JointType.Joint4], this.joint4Link.localEulerAngles.y) * Mathf.Deg2Rad);
// TODO Have to check sensor data of a real turtlebot3
//5 grip_joint
positions.Add(-this.gripJointLink.localPosition.y);
//6 grip_joint_sub
positions.Add(+this.gripJointSubLink.localPosition.y);
// Debug.Log("Pub JointState joint1="+positions[0]);
this.jointState.header.Update();
this.jointState.position = positions;
// float position = TurtleBot3Common.GetClampedPosition(value, name);
this.publisher.Publish(this.jointState);
}
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);
}
}
}
protected override void Update()
{
base.Update();
foreach (string jointName in this.trajectoryKeyList)
{
if (this.trajectoryInfoMap[jointName] != null)
{
if (jointName == TurtleBot3Common.jointNameMap[JointType.Joint1])
{
float newPos = TurtleBot3Common.GetCorrectedJointsEulerAngle(jointName, GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, TurtleBot3Common.MinSpeedRad, TurtleBot3Common.MaxSpeedArm) * Mathf.Rad2Deg);
this.joint1Link.localEulerAngles = new Vector3(this.joint1Link.localEulerAngles.x, this.joint1Link.localEulerAngles.y, newPos);
}
if (jointName == TurtleBot3Common.jointNameMap[JointType.Joint2])
{
float newPos = TurtleBot3Common.GetCorrectedJointsEulerAngle(jointName, GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, TurtleBot3Common.MinSpeedRad, TurtleBot3Common.MaxSpeedArm) * Mathf.Rad2Deg);
this.joint2Link.localEulerAngles = new Vector3(this.joint2Link.localEulerAngles.x, newPos, this.joint2Link.localEulerAngles.z);
}
if (jointName == TurtleBot3Common.jointNameMap[JointType.Joint3])
{
float newPos = TurtleBot3Common.GetCorrectedJointsEulerAngle(jointName, GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, TurtleBot3Common.MinSpeedRad, TurtleBot3Common.MaxSpeedArm) * Mathf.Rad2Deg);
this.joint3Link.localEulerAngles = new Vector3(this.joint3Link.localEulerAngles.x, newPos, this.joint3Link.localEulerAngles.z);
}
if (jointName == TurtleBot3Common.jointNameMap[JointType.Joint4])
{
float newPos = TurtleBot3Common.GetCorrectedJointsEulerAngle(jointName, GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, TurtleBot3Common.MinSpeedRad, TurtleBot3Common.MaxSpeedArm) * Mathf.Rad2Deg);
this.joint4Link.localEulerAngles = new Vector3(this.joint4Link.localEulerAngles.x, newPos, this.joint4Link.localEulerAngles.z);
}
if (jointName == TurtleBot3Common.jointNameMap[JointType.GripJoint])
{
float newPos = GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, TurtleBot3Common.MinSpeed, TurtleBot3Common.MaxSpeedHand);
// Grasping and hand closing
if (this.graspedObject != null && newPos < this.gripJointLink.localPosition.y)
{
// Have to stop
this.trajectoryInfoMap[jointName] = null;
}
// Otherwise
else
{
this.gripJointLink.localPosition = new Vector3(this.gripJointLink.localPosition.x, newPos, this.gripJointLink.localPosition.z);
}
}
if (jointName == TurtleBot3Common.jointNameMap[JointType.GripJointSub])
{
float newPos = GetPositionAndUpdateTrajectory(this.trajectoryInfoMap, jointName, TurtleBot3Common.MinSpeed, TurtleBot3Common.MaxSpeedHand);
// Grasping and hand closing
if (this.graspedObject != null && newPos > this.gripJointSubLink.localPosition.y)
{
// Have to stop
this.trajectoryInfoMap[jointName] = null;
}
// Otherwise
else
{
this.gripJointSubLink.localPosition = new Vector3(this.gripJointSubLink.localPosition.x, newPos, this.gripJointSubLink.localPosition.z);
}
}
}
}
this.webSocketConnection.Render();
}