public void JointStateCallback(SIGVerse.RosBridge.sensor_msgs.JointState jointState)
{
for (int i = 0; i < jointState.name.Count; i++)
{
string name = jointState.name[i];
TurtleBot3Common.UpdateJoint(name, this.transformMap[name], (float)jointState.position[i]);
}
}
void Awake()
{
this.joint1Link = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.Joint1);
this.joint2Link = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.Joint2);
this.joint3Link = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.Joint3);
this.joint4Link = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.Joint4);
this.gripJointLink = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.GripJoint);
this.gripJointSubLink = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.GripJointSub);
}
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);
}
}
}
void Awake()
{
this.joint1Link = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.Joint1);
this.joint2Link = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.Joint2);
this.joint3Link = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.Joint3);
this.joint4Link = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.Joint4);
this.gripJointLink = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.GripJoint);
this.gripJointSubLink = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, JointType.GripJointSub);
this.trajectoryInfoMap = new Dictionary <string, TrajectoryInfo>();
this.trajectoryInfoMap.Add(TurtleBot3Common.jointNameMap[JointType.Joint1], null);
this.trajectoryInfoMap.Add(TurtleBot3Common.jointNameMap[JointType.Joint2], null);
this.trajectoryInfoMap.Add(TurtleBot3Common.jointNameMap[JointType.Joint3], null);
this.trajectoryInfoMap.Add(TurtleBot3Common.jointNameMap[JointType.Joint4], null);
this.trajectoryInfoMap.Add(TurtleBot3Common.jointNameMap[JointType.GripJoint], null);
this.trajectoryInfoMap.Add(TurtleBot3Common.jointNameMap[JointType.GripJointSub], null);
this.trajectoryKeyList = new List <string>(trajectoryInfoMap.Keys);
}
private Dictionary <string, Transform> transformMap; // key:joint name, value:link transform
// Use this for initialization
void Start()
{
if (this.rosbridgeIP.Equals(string.Empty))
{
this.rosbridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
}
if (this.rosbridgePort == 0)
{
this.rosbridgePort = ConfigManager.Instance.configInfo.rosbridgePort;
}
this.transformMap = TurtleBot3Common.GetJointNameToLinkMap(this.transform);
this.webSocketConnection = new SIGVerse.RosBridge.RosBridgeWebSocketConnection(this.rosbridgeIP, this.rosbridgePort);
this.webSocketConnection.Subscribe <SIGVerse.RosBridge.sensor_msgs.JointState>(topicName, this.JointStateCallback);
// Connect to ROSbridge server
this.webSocketConnection.Connect <SIGVerse.RosBridge.sensor_msgs.JointState>();
}
void Awake()
{
List <UnityEngine.Transform> localLinkList = TurtleBot3Common.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.synchronizer = this.GetComponent <TurtleBot3PubSynchronizer>();
this.publishSequenceNumber = this.synchronizer.GetAssignedSequenceNumber();
this.isUsingThread = this.synchronizer.useThread;
}
}
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 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();
}
void Awake()
{
this.baseFootprint = TurtleBot3Common.FindGameObjectFromChild(this.transform.root, TurtleBot3LinkInfo.LinkType.BaseFootprint);
this.baseRigidbody = this.baseFootprint.GetComponent <Rigidbody>();
}
