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>(); }