public override string SimulatedResponse(Protocols.Protocol type)
{
if (type == Protocols.Protocol.J1850)
{
return
("59 A9 57 01" + Protocols.Elm327.EndOfLine +
"59 A9 58 01" + Protocols.Elm327.EndOfLine +
"59 B8 02 01" + Protocols.Elm327.EndOfLine +
"59 D0 16 11" + Protocols.Elm327.EndOfLine +
"59 A9 57 3F" + Protocols.Elm327.EndOfLine +
"59 A9 57 25" + Protocols.Elm327.EndOfLine +
"59 06 70 7F" + Protocols.Elm327.EndOfLine +
"59 04 01 3F" + Protocols.Elm327.EndOfLine +
"59 27 71 21" + Protocols.Elm327.EndOfLine +
"59 00 00 13" + Protocols.Elm327.EndOfLine + Protocols.Elm327.Prompt);
}
else
{
return
("0: 59 A9 57 01" + Protocols.Elm327.EndOfLine +
"1:A9 58 01" + Protocols.Elm327.EndOfLine +
"2: B8 02 01" + Protocols.Elm327.EndOfLine +
"3: D0 16 11" + Protocols.Elm327.EndOfLine +
"4: A9 57 3F" + Protocols.Elm327.EndOfLine +
"5: A9 57 25" + Protocols.Elm327.EndOfLine +
"6: 06 70 7F" + Protocols.Elm327.EndOfLine +
"7:04 01 3F" + Protocols.Elm327.EndOfLine +
"8: 27 71 21" + Protocols.Elm327.EndOfLine +
"9: 00 00 13" + Protocols.Elm327.EndOfLine + Protocols.Elm327.Prompt);
}
}
public void Transfer(Protocols.Protocol protocolType, string serverUrl, int timeout, string urdfPath, string rosPackage, RosSocket.SerializerEnum serializer)
{
if (Path.GetExtension(urdfPath)?.ToLowerInvariant() != ".urdf")
{
Debug.LogWarning("Please select a valid URDF file to publish.");
return;
}
Thread transferToRos = new Thread(() => TransferAsync(protocolType, serverUrl, timeout, urdfPath, rosPackage, serializer));
transferToRos.Start();
}
public override string SimulatedResponse(Protocols.Protocol type)
{
if (type == Protocols.Protocol.J1850)
{
// this simulation is for ISO based protocols only, CAN may or may not
// have an extra byte after the mode describing how many codes there are
return("47 02 76 96 45" + Protocols.Elm327.EndOfLine + Protocols.Elm327.Prompt);
}
else
{
return("47 02 02 76 96 45 AA AA" + Protocols.Elm327.EndOfLine + Protocols.Elm327.Prompt);
}
}
public void TransferUrdf(Protocols.Protocol protocolType, string serverUrl, int timeout, string assetPath, string urdfParameter, RosSocket.SerializerEnum serializer)
{
this.timeout = timeout;
this.assetPath = assetPath;
this.urdfParameter = urdfParameter;
// initialize
ResetStatusEvents();
rosSocket = RosConnector.ConnectToRos(protocolType, serverUrl, OnConnected, OnClosed, serializer);
if (!StatusEvents["connected"].WaitOne(timeout * 1000))
{
Debug.LogWarning("Failed to connect to ROS before timeout");
return;
}
ImportAssets();
}
private void TransferAsync(Protocols.Protocol protocolType, string serverUrl, int timeout, string urdfPath, string rosPackage, RosSocket.SerializerEnum serializer)
{
RosSocket = RosConnector.ConnectToRos(protocolType, serverUrl, OnConnected, OnClose, serializer);
if (!StatusEvents["connected"].WaitOne(timeout * 1000))
{
Debug.LogWarning("Failed to connect to " + serverUrl + " before timeout.");
RosSocket.Close();
return;
}
string robotName = Path.GetFileName(urdfPath);
UrdfTransferToRos urdfTransferToRos = new UrdfTransferToRos(RosSocket, robotName, urdfPath, rosPackage);
StatusEvents["robotNamePublished"] = urdfTransferToRos.Status["robotNamePublished"];
StatusEvents["robotDescriptionPublished"] = urdfTransferToRos.Status["robotDescriptionPublished"];
StatusEvents["resourceFilesSent"] = urdfTransferToRos.Status["resourceFilesSent"];
urdfTransferToRos.Transfer();
}
private void GetEditorPrefs()
{
protocolType = (Protocols.Protocol)(EditorPrefs.HasKey("UrdfPublisherProtocolNumber") ?
EditorPrefs.GetInt("UrdfPublisherProtocolNumber") : 1);
serverUrl = (EditorPrefs.HasKey("UrdfPublisherServerUrl") ?
EditorPrefs.GetString("UrdfPublisherServerUrl") :
"ws://192.168.0.1:9090");
urdfPath = (EditorPrefs.HasKey("UrdfPublisherUrdfPath") ?
EditorPrefs.GetString("UrdfPublisherUrdfPath") :
Path.Combine(Path.Combine(Path.GetFullPath("."), "Assets"), "Urdf"));
rosPackage = (EditorPrefs.HasKey("UrdfPublisherRosPackage") ?
EditorPrefs.GetString("UrdfPublisherRosPackage") :
"new_package");
timeout = (EditorPrefs.HasKey("UrdfPublisherTimeout") ?
EditorPrefs.GetInt("UrdfPublisherTimeout") :
10);
}
private void GetEditorPrefs()
{
protocolType = (Protocols.Protocol)(EditorPrefs.HasKey("UrdfImporterProtocolNumber") ?
EditorPrefs.GetInt("UrdfImporterProtocolNumber") : 0);
address = (EditorPrefs.HasKey("UrdfImporterAddress") ?
EditorPrefs.GetString("UrdfImporterAddress") :
"ws://192.168.0.1:9090");
assetPath = (EditorPrefs.HasKey("UrdfImporterAssetPath") ?
EditorPrefs.GetString("UrdfImporterAssetPath") :
Path.Combine(Path.Combine(Path.GetFullPath("."), "Assets"), "Urdf"));
timeout = (EditorPrefs.HasKey("UrdfImporterTimeout") ?
EditorPrefs.GetInt("UrdfImporterTimeout") :
10);
urdfParameter = (EditorPrefs.HasKey("UrdfImporterUrdfParameter") ?
EditorPrefs.GetString("UrdfImporterUrdfParameter") :
"robot_description");
}
public override string SimulatedResponse(Protocols.Protocol type)
{
if (type == Protocols.Protocol.J1850)
{
return
("49 02 01 00 00 00 31" + Protocols.Elm327.EndOfLine +
"49 02 02 47 43 48 4B" + Protocols.Elm327.EndOfLine +
"49 02 03 33 33 32 38" + Protocols.Elm327.EndOfLine +
"49 02 04 37 31 34 32" + Protocols.Elm327.EndOfLine +
"49 02 05 30 38 32 36" + Protocols.Elm327.EndOfLine +
Protocols.Elm327.Prompt);
}
else
{
return
("014" + Protocols.Elm327.EndOfLine +
"0: 49 02 01 31 47 43" + Protocols.Elm327.EndOfLine +
"1: 48 4B 33 33 32 38 37" + Protocols.Elm327.EndOfLine +
"2: 31 34 32 30 38 32 36" + Protocols.Elm327.EndOfLine +
Protocols.Elm327.Prompt);
}
}
public override string SimulatedResponse(Protocols.Protocol type)
{
if (type == Protocols.Protocol.J1850)
{
// this simulation is for ISO based protocols only, CAN may or may not
// have an extra byte after the mode describing how many codes there are
return("43 01 76 02 56 45 10" + Protocols.Elm327.EndOfLine + Protocols.Elm327.Prompt);
}
else
{
// simulation for CAN related vehicles with only one packets-worth of codes
// return "43 03 01 76 02 56 45 10";
// simulation for CAN related to vehicles with multiple packets-worth of codes
// where each line is marked with an identifier
return("43 00 AA AA AA AA AA " + Protocols.Elm327.EndOfLine + // purposeful bad line
"010 " + Protocols.Elm327.EndOfLine + // purposeful bad line
"0: 43 07 00 97 01 02 " + Protocols.Elm327.EndOfLine + // start of trouble codes
"1: 01 13 11 01 11 C2 22 " + Protocols.Elm327.EndOfLine +
" 2: 27 22 28 AA AA AA AA" + Protocols.Elm327.EndOfLine + Protocols.Elm327.Prompt);
}
}
public virtual string SimulatedResponse(Protocols.Protocol type)
{
// populate the string with valid info until the data packets, then fill with random data
string dataStr = (this.Mode + 0x40).ToString("X2") + " ";
// the pid needs to be split into two separate bytes if an extended pid
// with a space separating them just like the elm response would be
if (this.PacketSize == 3)
{
byte[] pidBytes = BitConverter.GetBytes(this.PID);
dataStr += pidBytes[1].ToString("X2") + " " + pidBytes[0].ToString("X2");
}
else
{
dataStr += this.PID.ToString("X2");
}
for (int i = 0; i < this.DataByteCount; i++)
{
dataStr += " " + ((byte)(DateTime.Now.Ticks + this.PID)).ToString("X2");
}
if (type == Protocols.Protocol.HighSpeedCAN11 ||
type == Protocols.Protocol.LowSpeedCAN11 ||
type == Protocols.Protocol.HighSpeedCAN29 ||
type == Protocols.Protocol.LowSpeedCAN29)
{
dataStr += " AA AA AA AA";
}
// purposely sleep to simulate the minimum cable transmission delay
System.Threading.Thread.Sleep(25);
return(dataStr);
}
private void OnGUI()
{
GUILayout.Label("URDF Transfer (From Unity to ROS)", EditorStyles.boldLabel);
EditorGUILayout.BeginHorizontal();
serverUrl = EditorGUILayout.TextField("Server URL", serverUrl);
EditorGUILayout.EndHorizontal();
showSettings = EditorGUILayout.Foldout(showSettings, "Settings");
if (showSettings)
{
EditorGUILayout.BeginHorizontal();
EditorGUIUtility.labelWidth = 100;
protocolType = (Protocols.Protocol)EditorGUILayout.EnumPopup("Protocol", protocolType);
EditorGUILayout.EndHorizontal();
EditorGUILayout.BeginHorizontal();
urdfPath = EditorGUILayout.TextField("URDF to export", urdfPath);
if (GUILayout.Button("Select", new GUIStyle(EditorStyles.miniButtonRight)
{
fixedWidth = 75
}))
{
urdfPath = EditorUtility.OpenFilePanel("Select a URDF file", urdfPath, "urdf");
}
EditorGUILayout.EndHorizontal();
EditorGUILayout.BeginHorizontal();
timeout = EditorGUILayout.IntField("Timeout [s]", timeout);
EditorGUILayout.EndHorizontal();
EditorGUILayout.BeginHorizontal();
rosPackage = EditorGUILayout.TextField(
new GUIContent("ROS package",
"The package where all meshes and resources files will be exported to in ROS."),
rosPackage);
EditorGUILayout.EndHorizontal();
}
EditorGUILayout.BeginHorizontal();
EditorGUILayout.Space();
if (GUILayout.Button("Reset to Default", GUILayout.Width(150)))
{
DeleteEditorPrefs();
GetEditorPrefs();
}
EditorGUILayout.EndHorizontal();
GUILayout.Space(10);
EditorGUILayout.BeginHorizontal();
if (GUILayout.Button("Publish Robot Description"))
{
SetEditorPrefs();
transferHandler.Transfer(protocolType, serverUrl, timeout, urdfPath, rosPackage);
}
EditorGUILayout.EndHorizontal();
GUILayout.Space(20);
EditorGUIUtility.labelWidth = 225;
DrawLabelField("Connected: ", "connected");
DrawLabelField("Robot name published: ", "robotNamePublished");
DrawLabelField("Robot description published: ", "robotDescriptionPublished");
DrawLabelField("All resources files published: ", "resourceFilesSent");
GUILayout.Space(10);
EditorGUI.BeginDisabledGroup(transferHandler.RosSocket == null || !transferHandler.RosSocket.protocol.IsAlive());
if (GUILayout.Button("Close Connection"))
{
transferHandler.RosSocket?.Close();
}
EditorGUI.EndDisabledGroup();
}
public override string SimulatedResponse(Protocols.Protocol type)
{
return(Protocols.Elm327.Prompt); // there is nothing to return
}
private void OnGUI()
{
GUILayout.Label("URDF Transfer (From ROS to Unity)", EditorStyles.boldLabel);
EditorGUILayout.BeginHorizontal();
address = EditorGUILayout.TextField("Address", address);
EditorGUILayout.EndHorizontal();
showSettings = EditorGUILayout.Foldout(showSettings, "Settings");
if (showSettings)
{
EditorGUILayout.BeginHorizontal();
EditorGUIUtility.labelWidth = 100;
protocolType = (Protocols.Protocol)EditorGUILayout.EnumPopup("Protocol", protocolType);
EditorGUILayout.EndHorizontal();
EditorGUILayout.BeginHorizontal();
EditorGUIUtility.labelWidth = 100;
serializerType = (RosSocket.SerializerEnum)EditorGUILayout.EnumPopup("Serializer", serializerType);
EditorGUILayout.EndHorizontal();
//TODO URDF Parameter
EditorGUILayout.BeginHorizontal();
urdfParameter = EditorGUILayout.TextField("URDF Parameter", urdfParameter);
EditorGUILayout.EndHorizontal();
EditorGUILayout.BeginHorizontal();
timeout = EditorGUILayout.IntField("Timeout [s]", timeout);
EditorGUILayout.EndHorizontal();
EditorGUILayout.BeginHorizontal();
assetPath = EditorGUILayout.TextField("Asset Path", assetPath);
EditorGUILayout.EndHorizontal();
}
EditorGUILayout.BeginHorizontal();
EditorGUILayout.Space();
if (GUILayout.Button("Reset to Default", GUILayout.Width(150)))
{
DeleteEditorPrefs();
GetEditorPrefs();
}
EditorGUILayout.EndHorizontal();
GUILayout.Space(20);
EditorGUILayout.BeginHorizontal();
if (GUILayout.Button("Read Robot Description"))
{
SetEditorPrefs();
Thread rosSocketConnectThread = new Thread(() => transferHandler.TransferUrdf(protocolType, address, timeout, assetPath, urdfParameter, serializerType));
rosSocketConnectThread.Start();
}
EditorGUILayout.EndHorizontal();
GUILayout.Space(20);
EditorGUIUtility.labelWidth = 300;
DrawLabelField("Connected:", "connected");
DrawLabelField("Robot Name Received:", "robotNameReceived");
DrawLabelField("Robot Description Received:", "robotDescriptionReceived");
DrawLabelField("Resource Files Received:", "resourceFilesReceived");
DrawLabelField("Disconnected:", "disconnected");
DrawLabelField("Import Complete:", "importComplete");
}
