public void ReceiveTF_FrameWithParent_ReturnsSameTranslation() { ROSConnection ros = ROSConnection.GetOrCreateInstance(); ros.ConnectOnStart = false; TFSystem system = TFSystem.GetOrCreateInstance(); TFSystem.TFTopicState topic = system.GetOrCreateTFTopic(); TFStream stream = system.GetOrCreateFrame(composite_frame_id); Assert.AreEqual(stream.Name, simple_frame_id); Assert.AreEqual(stream.Parent.Name, parent_frame_id); TimeMsg time = MakeTimeMsg(4567, 890); Vector3 unityPosition = new Vector3(1, 2, 3); topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( MakeHeaderMsg(time, parent_frame_id), composite_frame_id, new TransformMsg(unityPosition.To <FLU>(), new QuaternionMsg() )) })); TFFrame frame = stream.GetWorldTF(time.ToLongTime()); Assert.AreEqual(frame.translation.x, unityPosition.x); Assert.AreEqual(frame.translation.y, unityPosition.y); Assert.AreEqual(frame.translation.z, unityPosition.z); Vector3 gameObjectPos = stream.GameObject.transform.position; Assert.AreEqual(gameObjectPos.x, unityPosition.x); Assert.AreEqual(gameObjectPos.y, unityPosition.y); Assert.AreEqual(gameObjectPos.z, unityPosition.z); }
void parseXYZ(ROSConnection rs, string filePath) { string[] lines = File.ReadAllLines(filePath); double[] X = new double[lines.Length]; double[] Y = new double[lines.Length]; double[] Z = new double[lines.Length]; int i = 0; foreach (string line in lines) { var words = line.Split(' '); X[i] = (double.Parse(words[0]) / 1000); Y[i] = (double.Parse(words[1]) / 1000); Z[i] = (double.Parse(words[2]) / 1000); i++; } RosMessageTypes.ROS.XYZcloud Cloud = new RosMessageTypes.ROS.XYZcloud(); Cloud.X = X; Cloud.Y = Y; Cloud.Z = Z; if (DisplayPointCloud == true) { ApplyToParticleSystem(X, Y, Z); } else { var ps = GetComponent <ParticleSystem>(); ps.Stop(); } rs.Send("/unity/XYZCloud", Cloud); }
public void GetOrCreateInstance_CallOnce_ReturnsValidInstance() { ROSConnection ros = ROSConnection.GetOrCreateInstance(); ros.ConnectOnStart = false; Assert.NotNull(ros); }
// Start is called before the first frame update void Start() { // start the ROS connection ros = ROSConnection.instance; // Set up the arm variables this.gameObject.AddComponent <FKRobot>(); articulationChain = this.GetComponentsInChildren <ArticulationBody>(); // https://docs.unity3d.com/2020.1/Documentation/ScriptReference/ArticulationBody.html?_ga=2.54684075.1087433992.1613790814-228562203.1613145667 int defDyanmicVal = 10; foreach (ArticulationBody joint in articulationChain) { // Set up each of the joints joint.gameObject.AddComponent <JointControl>(); joint.jointFriction = defDyanmicVal; joint.angularDamping = defDyanmicVal; ArticulationDrive currentDrive = joint.xDrive; currentDrive.forceLimit = forceLimit; joint.xDrive = currentDrive; print(joint); } num_joints = articulationChain.Length; print(num_joints); print("------------"); }
protected override void OnCreate() { ros = Object.FindObjectOfType <ROSConnection>(); _stopWatch = new Stopwatch(); _stopWatch.Start(); }
public OccupancyGridVisual(string topic, OccupancyGridDefaultVisualizer settings) { m_Topic = topic; m_Settings = settings; ROSConnection.GetOrCreateInstance().Subscribe <OccupancyGridMsg>(m_Topic, AddMessage); }
void Start() { // Get ROS connection static instance ros = ROSConnection.instance; // Assign UI elements InitializeButton = GameObject.Find("ROSObjects/Canvas/ButtonPanel/DefaultButton").GetComponent <Button>(); RandomizeButton = GameObject.Find("ROSObjects/Canvas/ButtonPanel/RandomButton").GetComponent <Button>(); ServiceButton = GameObject.Find("ROSObjects/Canvas/ButtonPanel/ServiceButton").GetComponent <Button>(); ActualPos = GameObject.Find("ROSObjects/Canvas/PositionPanel/ActualPosField").GetComponent <Text>(); ActualRot = GameObject.Find("ROSObjects/Canvas/PositionPanel/ActualRotField").GetComponent <Text>(); EstimatedPos = GameObject.Find("ROSObjects/Canvas/PositionPanel/EstPosField").GetComponent <Text>(); EstimatedRot = GameObject.Find("ROSObjects/Canvas/PositionPanel/EstRotField").GetComponent <Text>(); // Initialize UI element values ActualPos.text = target.transform.position.ToString(); ActualRot.text = target.transform.eulerAngles.ToString(); EstimatedPos.text = "-"; EstimatedRot.text = "-"; // Render texture renderTexture = new RenderTexture(Camera.main.pixelWidth, Camera.main.pixelHeight, 24, UnityEngine.Experimental.Rendering.GraphicsFormat.R8G8B8A8_SRGB); renderTexture.Create(); }
void Start() { MapMesh = new GameObject("MapMesh"); MapMesh.transform.parent = GameObject.Find("RobotontNavigationFrame").transform; MapMesh.AddComponent <MeshFilter>(); meshRenderer = MapMesh.AddComponent <MeshRenderer>(); mesh = MapMesh.GetComponent <MeshFilter>().mesh; meshVerticies = new Vector3[4]; //meshVerticies[0] = new Vector3(0f, 0f, 0f); //meshVerticies[1] = new Vector3(0f, 0f, 0f); //meshVerticies[2] = new Vector3(0f, 0f, 0f); //meshVerticies[3] = new Vector3(0f, 0f, 0f); meshTriangles = new int[6] { 0, 1, 2, 2, 1, 3 }; //order of verticies in triangle mesh.vertices = meshVerticies; mesh.triangles = meshTriangles; _recievedMap = new Texture2D(1, 1, TextureFormat.R8, false); _recievedMap.filterMode = FilterMode.Point; meshRenderer.material = new Material(Shader.Find("Standard")); //meshRenderer.material.SetTexture("_mainTex", _recievedMap); _isCreated = true; ROSConnection.GetOrCreateInstance().Subscribe <RosMap>("/map", MapChange); }
public void ReceiveTF_MultipleMessages_InterpolatesTimes() { ROSConnection ros = ROSConnection.GetOrCreateInstance(); ros.ConnectOnStart = false; TFSystem.TFTopicState topic = TFSystem.GetOrCreateInstance().GetOrCreateTFTopic(); TFStream stream = topic.GetOrCreateFrame(simple_frame_id); int time1_secs = 1000; int time2_secs = 2000; TimeMsg time1 = MakeTimeMsg(time1_secs, 0); Vector3 unityPosition1 = new Vector3(1, 2, 3); topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( MakeHeaderMsg(time1, parent_frame_id), simple_frame_id, new TransformMsg(unityPosition1.To <FLU>(), new QuaternionMsg() )) })); TimeMsg time2 = MakeTimeMsg(time2_secs, 0); Vector3 unityPosition2 = new Vector3(2, 3, 4); topic.ReceiveTF(new TFMessageMsg(new TransformStampedMsg[] { new TransformStampedMsg( MakeHeaderMsg(time2, parent_frame_id), simple_frame_id, new TransformMsg(unityPosition2.To <FLU>(), new QuaternionMsg() )) })); TimeMsg time1_5 = MakeTimeMsg((time1_secs + time2_secs) / 2, 0); Vector3 pointAtTime1 = stream.GetWorldTF(time1).translation; Vector3 pointAtTime1_5 = stream.GetWorldTF(time1_5).translation; Vector3 pointAtTime2 = stream.GetWorldTF(time2).translation; Vector3 unityPosition1_5 = (unityPosition1 + unityPosition2) / 2; Assert.AreEqual(pointAtTime1, unityPosition1); Assert.AreEqual(pointAtTime1_5, unityPosition1_5); Assert.AreEqual(pointAtTime2, unityPosition2); }
protected virtual void OnGUI() { if (prefab == null) { prefabObj = Resources.Load <GameObject>("ROSConnectionPrefab"); if (prefabObj != null) { prefab = prefabObj.GetComponent <ROSConnection>(); } if (prefab == null) { GameObject sceneObj = new GameObject("ROSConnection"); sceneObj.AddComponent <ROSConnection>(); if (!Directory.Exists("Assets/Resources")) { Directory.CreateDirectory("Assets/Resources"); } prefabObj = PrefabUtility.SaveAsPrefabAsset(sceneObj, "Assets/Resources/ROSConnectionPrefab.prefab"); if (prefabObj != null) { prefab = prefabObj.GetComponent <ROSConnection>(); } DestroyImmediate(sceneObj); } } EditorGUILayout.LabelField("Settings for a new ROSConnection.instance", EditorStyles.boldLabel); prefab.rosIPAddress = EditorGUILayout.TextField("ROS IP Address", prefab.rosIPAddress); prefab.rosPort = EditorGUILayout.IntField("ROS Port", prefab.rosPort); EditorGUILayout.Space(); prefab.overrideUnityIP = EditorGUILayout.TextField( new GUIContent("Override Unity IP Address", "If blank, determine IP automatically."), prefab.overrideUnityIP); prefab.unityPort = EditorGUILayout.IntField("Unity Port", prefab.unityPort); EditorGUILayout.Space(); EditorGUILayout.LabelField("If awaiting a service response:", EditorStyles.boldLabel); prefab.awaitDataMaxRetries = EditorGUILayout.IntField( new GUIContent("Max Service Retries", "While waiting for a service to respond, check this many times before giving up."), prefab.awaitDataMaxRetries); prefab.awaitDataSleepSeconds = EditorGUILayout.FloatField( new GUIContent("Sleep (seconds)", "While waiting for a service to respond, wait this many seconds between checks."), prefab.awaitDataSleepSeconds); prefab.readChunkSize = EditorGUILayout.IntField( new GUIContent("Read chunk size", "While reading received messages, read this many bytes at a time."), prefab.readChunkSize); prefab.awaitDataReadRetry = EditorGUILayout.IntField( new GUIContent("Max Read retries", "While waiting to read a full message, check this many times before giving up."), prefab.awaitDataReadRetry); if (GUI.changed) { EditorUtility.SetDirty(prefabObj); } }
/// <summary> /// /// </summary> void Start() { // Get ROS connection static instance ros = ROSConnection.instance; jointArticulationBodies = new ArticulationBody[numRobotJoints]; string shoulder_link = "world/base_link/shoulder_link"; jointArticulationBodies[0] = niryoOne.transform.Find(shoulder_link).GetComponent <ArticulationBody>(); string arm_link = shoulder_link + "/arm_link"; jointArticulationBodies[1] = niryoOne.transform.Find(arm_link).GetComponent <ArticulationBody>(); string elbow_link = arm_link + "/elbow_link"; jointArticulationBodies[2] = niryoOne.transform.Find(elbow_link).GetComponent <ArticulationBody>(); string forearm_link = elbow_link + "/forearm_link"; jointArticulationBodies[3] = niryoOne.transform.Find(forearm_link).GetComponent <ArticulationBody>(); string wrist_link = forearm_link + "/wrist_link"; jointArticulationBodies[4] = niryoOne.transform.Find(wrist_link).GetComponent <ArticulationBody>(); string hand_link = wrist_link + "/hand_link"; jointArticulationBodies[5] = niryoOne.transform.Find(hand_link).GetComponent <ArticulationBody>(); }
public override void Stop() { if (ROSConnection == null) { return; } ROSConnection.RemoveSubcriber(this); }
/// <summary> /// /// </summary> void Start() { RosMessageTypes.Geometry.Pose[] frame_message = new RosMessageTypes.Geometry.Pose[Arms.Length]; RosMessageTypes.Geometry.Pose[] position_message = new RosMessageTypes.Geometry.Pose[Controllers.Length]; // Get ROS connection static instance ros = ROSConnection.instance; parseXYZ(ros, Path + fileName); int i = 0; joint_topics = new string[Arms.Length]; frame_topics = new string[Arms.Length]; controller_topics = new string[Controllers.Length]; foreach (GameObject _controller in Controllers) { controller_topics[i] = "/unity/" + _controller.name + "/pose/"; position_message[i] = new RosMessageTypes.Geometry.Pose(); i++; } i = 0; foreach (GameObject arm in Arms) { frame_message[i] = new RosMessageTypes.Geometry.Pose(); joint_topics[i] = "/unity/" + arm.name + "/joint_state/"; frame_topics[i] = "/unity/" + arm.name + "/base_frame/"; if (arm.name == "ECM") { i_ECM = i; Base_Frames[i] = GameObject.Find("ecm_setup_base_link"); } else if (arm.name == "PSM1") { i_PSM1 = i; Base_Frames[i] = GameObject.Find(arm.name + "_psm_base_link"); } else if (arm.name == "PSM2") { i_PSM2 = i; Base_Frames[i] = GameObject.Find(arm.name + "_psm_base_link"); } i++; } for (i = 0; i < Base_Frames.Length; i++) { frame_message[i].position = Base_Frames[i].transform.position.To <FLU>(); frame_message[i].orientation = Base_Frames[i].transform.rotation.To <FLU>(); } i = 0; for (i = 0; i < frame_message.Length; i++) { ros.Send(frame_topics[i], frame_message[i]); } }
public ImageVisual(string topic, ImageDefaultVisualizer factory) { m_Topic = topic; m_Factory = factory; m_Debayer = m_Factory.m_Debayer; m_CheapTextureMaterial = new Material(Shader.Find("Unlit/ImageMsg")); ROSConnection.GetOrCreateInstance().Subscribe <ImageMsg>(m_Topic, AddMessage); }
// Start is called before the first frame update void Start() { ros = ROSConnection.GetOrCreateInstance(); ros.RegisterPublisher <PoseStampedMsg>(topicName); this._goal.header.frame_id = "map"; this._goal.header.stamp.sec = 0; this._goal.header.stamp.nanosec = 0; }
public override void Stop() { base.Stop(); if (ROSConnection == null) { return; } ROSConnection.RemovePublisher(this); }
void CreateRosConnection() { // Create RosConnect var rosConnect = new GameObject(k_RosConnectName); m_RosConnection = rosConnect.AddComponent <ROSConnection>(); m_RosConnection.RosIPAddress = m_HostIP; m_RosConnection.RosPort = k_HostPort; }
public void Start() { m_FillTexture = VisualizationUtils.MakeTexture(16, 16, new Color(0.125f, 0.19f, 0.25f)); m_Connection = ROSConnection.GetOrCreateInstance(); HudPanel.RegisterTab(this, (int)HudTabOrdering.Topics); HudPanel.RegisterTab(new VisualizationLayoutTab(this), (int)HudTabOrdering.Layout); LoadLayout(); m_Connection.ListenForTopics(OnNewTopic, notifyAllExistingTopics: true); }
void CreateRosConnection() { m_RosConnection = new GameObject(k_NameRosConnect).AddComponent <ROSConnection>(); m_RosConnection.RosIPAddress = k_IpAddressLoopback; m_RosConnection.RosPort = k_HostPort; //m_RosConnection.overrideUnityIP = k_IpAddressLoopback; //m_RosConnection.unityPort = k_UnityPort; //m_RosConnection.awaitDataMaxRetries = k_NumAwaitDataRetries; //m_RosConnection.awaitDataSleepSeconds = k_NumAwaitDataSleepSeconds; }
public void GetOrCreateInstance_CallTwice_ReturnsSameInstance() { ROSConnection ros = ROSConnection.GetOrCreateInstance(); Assert.NotNull(ros); ros.ConnectOnStart = false; ROSConnection ros2 = ROSConnection.GetOrCreateInstance(); Assert.AreEqual(ros, ros2); }
public ToStringVisual(string topic, MessageSubtopic subtopic) { ROSConnection ros = ROSConnection.GetOrCreateInstance(); RosTopicState state = ros.GetTopic(topic); if (subtopic == MessageSubtopic.Response) { state = state.ServiceResponseTopic; } state.AddSubscriber(AddMessage); }
void Awake() { statusText = gameObject.GetComponent <Text>(); unityIsRunning = true; // TODO Check if the following are running vrepIsRunning = false; rosIsRunning = false; ros = GameObject.Find("ROSConnection").GetComponent <ROSConnection>(); rosSocket = ros.getRosSocket(); }
private void CreateRosConnection() { // Create RosConnect GameObject rosConnect = new GameObject(rosConnectName); rosConnection = rosConnect.AddComponent <ROSConnection>(); rosConnection.rosIPAddress = hostIP; rosConnection.rosPort = hostPort; rosConnection.overrideUnityIP = overrideUnityIP; rosConnection.unityPort = unityPort; rosConnection.awaitDataMaxRetries = awaitDataMaxRetries; rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds; }
public MarkersVisual(string topic) { ROSConnection ros = ROSConnection.GetOrCreateInstance(); RosTopicState state = ros.GetTopic(topic); if (state.RosMessageName == MarkerArrayMsg.k_RosMessageName) { ros.Subscribe <MarkerArrayMsg>(topic, OnMarkerArray); } else if (state.RosMessageName == MarkerMsg.k_RosMessageName) { ros.Subscribe <MarkerMsg>(topic, OnMarker); } }
public static TFSystem GetOrCreateInstance() { if (instance != null) { return(instance); } ROSConnection ros = ROSConnection.GetOrCreateInstance(); instance = new TFSystem(); foreach (string s in ros.TFTopics) { instance.GetOrCreateTFTopic(s); } return(instance); }
void Start() { // Get ROS connection static instance m_Ros = ROSConnection.GetOrCreateInstance(); m_Ros.RegisterPublisher <NiryoMoveitJointsMsg>(m_TopicName); m_JointArticulationBodies = new UrdfJointRevolute[k_NumRobotJoints]; var linkName = string.Empty; for (var i = 0; i < k_NumRobotJoints; i++) { linkName += LinkNames[i]; m_JointArticulationBodies[i] = m_NiryoOne.transform.Find(linkName).GetComponent <UrdfJointRevolute>(); } }
public void GetOrCreateFrame_Hierarchy_BuildsValidHierarchy() { ROSConnection ros = ROSConnection.GetOrCreateInstance(); ros.ConnectOnStart = false; TFSystem.TFTopicState topic = TFSystem.GetOrCreateInstance().GetOrCreateTFTopic(); TFStream stream = topic.GetOrCreateFrame(composite_frame_id); GameObject gameObject = stream.GameObject; Assert.IsNotNull(stream.GameObject); Assert.AreEqual(stream.GameObject.name, simple_frame_id); Assert.AreEqual(stream.GameObject.transform.parent.name, parent_frame_id); TFStream stream2 = topic.GetOrCreateFrame(composite_frame_id2); Assert.IsNotNull(stream2.GameObject); Assert.AreEqual(stream2.GameObject.name, simple_frame_id2); Assert.AreNotEqual(stream.GameObject, stream2.GameObject); Assert.AreEqual(stream.GameObject.transform.parent, stream2.GameObject.transform.parent); }
/// <summary> /// Find all robot joints in Awake() and add them to the jointArticulationBodies array. /// Find left and right finger joints and assign them to their respective articulation body objects. /// </summary> void Start() { // Get ROS connection static instance ros = ROSConnection.instance; jointArticulationBodies = new ArticulationBody[numRobotJoints]; string shoulder_link = "world/base_link/shoulder_link"; jointArticulationBodies[0] = niryoOne.transform.Find(shoulder_link).GetComponent <ArticulationBody>(); string arm_link = shoulder_link + "/arm_link"; jointArticulationBodies[1] = niryoOne.transform.Find(arm_link).GetComponent <ArticulationBody>(); string elbow_link = arm_link + "/elbow_link"; jointArticulationBodies[2] = niryoOne.transform.Find(elbow_link).GetComponent <ArticulationBody>(); string forearm_link = elbow_link + "/forearm_link"; jointArticulationBodies[3] = niryoOne.transform.Find(forearm_link).GetComponent <ArticulationBody>(); string wrist_link = forearm_link + "/wrist_link"; jointArticulationBodies[4] = niryoOne.transform.Find(wrist_link).GetComponent <ArticulationBody>(); string hand_link = wrist_link + "/hand_link"; jointArticulationBodies[5] = niryoOne.transform.Find(hand_link).GetComponent <ArticulationBody>(); // Find left and right fingers string right_gripper = hand_link + "/tool_link/gripper_base/servo_head/control_rod_right/right_gripper"; string left_gripper = hand_link + "/tool_link/gripper_base/servo_head/control_rod_left/left_gripper"; string gripper_base = hand_link + "/tool_link/gripper_base/Collisions/unnamed"; gripperBase = niryoOne.transform.Find(gripper_base); leftGripperGameObject = niryoOne.transform.Find(left_gripper); rightGripperGameObject = niryoOne.transform.Find(right_gripper); rightGripper = rightGripperGameObject.GetComponent <ArticulationBody>(); leftGripper = leftGripperGameObject.GetComponent <ArticulationBody>(); }
public static IVisual GetVisual(string topic, MessageSubtopic subtopic = MessageSubtopic.Default) { RosTopicState topicState = ROSConnection.GetOrCreateInstance().GetTopic(topic); if (topicState != null && subtopic == MessageSubtopic.Response) { topicState = topicState.ServiceResponseTopic; } if (topicState == null) { return(null); } IVisualFactory factory = VisualFactoryRegistry.GetVisualFactory(topic, topicState.RosMessageName, subtopic); if (factory == null) { return(null); } return(factory.GetOrCreateVisual(topic)); }
// Tutorial Part 2 without the Publisher object private void SetupRos() { if (generateRosMessages) { // Generate ROS messages MessageAutoGen.GenerateSingleMessage( Path.Combine(rosSrcDirectory, moveitMsgPackageName, msgDirectory, robotTrajectoryMessageFileName), rosMessagesDirectory, moveitMsgPackageName); MessageAutoGen.GenerateDirectoryMessages( Path.Combine(rosSrcDirectory, niryoMoveitPackageName, msgDirectory), rosMessagesDirectory); // Generate ROS services ServiceAutoGen.GenerateSingleService( Path.Combine(rosSrcDirectory, niryoMoveitPackageName, srvDirectory, moverServiceFileName), rosMessagesDirectory, niryoMoveitPackageName); } // Recompile ROS message scripts and external scripts List <string> scripts = new List <string>(); scripts.AddRange(Directory.GetFiles(rosMessagesDirectory, scriptPattern, SearchOption.AllDirectories)); scripts.AddRange(Directory.GetFiles(externalScriptsDirectory)); RecompileScripts(scripts.ToArray()); // Create RosConnect GameObject rosConnect = new GameObject(rosConnectName); rosConnection = rosConnect.AddComponent <ROSConnection>(); rosConnection.rosIPAddress = hostIP; rosConnection.rosPort = hostPort; rosConnection.overrideUnityIP = overrideUnityIP; rosConnection.unityPort = unityPort; rosConnection.awaitDataMaxRetries = awaitDataMaxRetries; rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds; }