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