diff --git a/Libraries/RosBridgeClient/Messages/Geometry/Transform.cs b/Libraries/RosBridgeClient/Messages/Geometry/Transform.cs new file mode 100644 index 000000000..58a1f3be0 --- /dev/null +++ b/Libraries/RosBridgeClient/Messages/Geometry/Transform.cs @@ -0,0 +1,32 @@ +/* +© HoloLab Inc., 2019 +Author: Yusuke Furuta (furuta@hololab.co.jp) + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at +. +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ + +using Newtonsoft.Json; + +namespace RosSharp.RosBridgeClient.Messages.Geometry +{ + public class Transform : Message + { + [JsonIgnore] + public const string RosMessageName = "geometry_msgs/Transform"; + public Vector3 translation; + public Quaternion rotation; + public Transform() + { + translation = new Vector3(); + rotation = new Quaternion(); + } + } +} \ No newline at end of file diff --git a/Libraries/RosBridgeClient/Messages/Geometry/TransformStamped.cs b/Libraries/RosBridgeClient/Messages/Geometry/TransformStamped.cs new file mode 100644 index 000000000..e3e7bbe9a --- /dev/null +++ b/Libraries/RosBridgeClient/Messages/Geometry/TransformStamped.cs @@ -0,0 +1,34 @@ +/* +© HoloLab Inc., 2019 +Author: Yusuke Furuta (furuta@hololab.co.jp) + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at +. +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ + +using Newtonsoft.Json; + +namespace RosSharp.RosBridgeClient.Messages.Geometry +{ + public class TransformStamped : Message + { + [JsonIgnore] + public const string RosMessageName = "geometry_msgs/TransformStamped"; + public string child_frame_id; + public Standard.Header header; + public Transform transform; + public TransformStamped() + { + header = new Standard.Header(); + child_frame_id = ""; + transform = new Transform(); + } + } +} \ No newline at end of file diff --git a/Libraries/RosBridgeClient/Messages/Navigation/Path.cs b/Libraries/RosBridgeClient/Messages/Navigation/Path.cs new file mode 100644 index 000000000..8987230ca --- /dev/null +++ b/Libraries/RosBridgeClient/Messages/Navigation/Path.cs @@ -0,0 +1,32 @@ +/* +© HoloLab Inc., 2019 +Author: Yusuke Furuta (furuta@hololab.co.jp) + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at +. +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ + +using Newtonsoft.Json; + +namespace RosSharp.RosBridgeClient.Messages.Navigation +{ + public class Path : Message + { + [JsonIgnore] + public const string RosMessageName = "nav_msgs/Path"; + public Standard.Header header; + public Geometry.PoseStamped[] poses; + public Path() + { + header = new Standard.Header(); + poses = new Geometry.PoseStamped[0]; + } + } +} \ No newline at end of file diff --git a/Libraries/RosBridgeClient/Messages/TF2/TFMessage.cs b/Libraries/RosBridgeClient/Messages/TF2/TFMessage.cs new file mode 100644 index 000000000..70d8bc755 --- /dev/null +++ b/Libraries/RosBridgeClient/Messages/TF2/TFMessage.cs @@ -0,0 +1,31 @@ +/* +© HoloLab Inc., 2019 +Author: Yusuke Furuta (furuta@hololab.co.jp) + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at +. +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ + +using Newtonsoft.Json; + +namespace RosSharp.RosBridgeClient.Messages.TF2 +{ + public class TFMessage : Message + { + [JsonIgnore] + public const string RosMessageName = "tf2_msgs/TFMessage"; + public Geometry.TransformStamped[] transforms; + + public TFMessage() + { + transforms = new Geometry.TransformStamped[0]; + } + } +} \ No newline at end of file diff --git a/Libraries/RosBridgeClient/RosBridgeClient.csproj b/Libraries/RosBridgeClient/RosBridgeClient.csproj index 498b8b170..ed9769b71 100644 --- a/Libraries/RosBridgeClient/RosBridgeClient.csproj +++ b/Libraries/RosBridgeClient/RosBridgeClient.csproj @@ -1,119 +1,122 @@ - - - - - Debug - AnyCPU - {27CD898A-8840-4CDD-A475-4A672FD2EF50} - Library - Properties - RosBridgeClient - RosBridgeClient - v4.6 - 512 - - - - - true - full - false - bin\Debug\ - DEBUG;TRACE - prompt - 4 - false - - - pdbonly - true - bin\Release\ - TRACE - prompt - 4 - false - - - - ..\packages\Newtonsoft.Json.12.0.1\lib\net45\Newtonsoft.Json.dll - - - - - - - - - ..\packages\WebSocketSharp.1.0.3-rc11\lib\websocket-sharp.dll - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + Debug + AnyCPU + {27CD898A-8840-4CDD-A475-4A672FD2EF50} + Library + Properties + RosBridgeClient + RosBridgeClient + v4.6 + 512 + + + + + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + false + + + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + false + + + + ..\packages\Newtonsoft.Json.12.0.1\lib\net45\Newtonsoft.Json.dll + + + ..\packages\Newtonsoft.Json.Bson.1.0.2\lib\net45\Newtonsoft.Json.Bson.dll + + + + + + + + + ..\packages\WebSocketSharp.1.0.3-rc11\lib\websocket-sharp.dll + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + --> \ No newline at end of file diff --git a/Libraries/RosBridgeClient/RosSocket.cs b/Libraries/RosBridgeClient/RosSocket.cs index 5dd7923ac..f3c6b0038 100644 --- a/Libraries/RosBridgeClient/RosSocket.cs +++ b/Libraries/RosBridgeClient/RosSocket.cs @@ -13,6 +13,9 @@ You may obtain a copy of the License at limitations under the License. */ +// Adding BSON (de-)seriliazation option +// Shimadzu corp , 2019, Akira NODA (a-noda@shimadzu.co.jp / you.akira.noda@gmail.com) + using System; using System.Collections.Generic; using System.Linq; @@ -26,15 +29,18 @@ namespace RosSharp.RosBridgeClient public class RosSocket { public IProtocol protocol; + public enum SerializerEnum { JSON, BSON } private Dictionary Publishers = new Dictionary(); private Dictionary Subscribers = new Dictionary(); private Dictionary ServiceProviders = new Dictionary(); private Dictionary ServiceConsumers = new Dictionary(); + private SerializerEnum Serializer; - public RosSocket(IProtocol protocol) + public RosSocket(IProtocol protocol,SerializerEnum serializer=SerializerEnum.JSON) { this.protocol = protocol; + this.Serializer = serializer; this.protocol.OnReceive += (sender, e) => Receive(sender, e); this.protocol.Connect(); } @@ -176,16 +182,38 @@ private List SubscribersOf(string topic) return Subscribers.Where(pair => pair.Key.StartsWith(topic + ":")).Select(pair => pair.Value).ToList(); } - private static byte[] Serialize(T obj) + private byte[] Serialize(T obj) { - string json = JsonConvert.SerializeObject(obj); - return Encoding.ASCII.GetBytes(json); + switch (Serializer) + { + case SerializerEnum.JSON: + string json = JsonConvert.SerializeObject(obj); + return Encoding.ASCII.GetBytes(json); + case SerializerEnum.BSON: + System.IO.MemoryStream ms = new System.IO.MemoryStream(); + Newtonsoft.Json.Bson.BsonDataWriter writer = new Newtonsoft.Json.Bson.BsonDataWriter(ms); + JsonSerializer serializer = new JsonSerializer(); + serializer.Serialize(writer, obj); + return ms.ToArray(); + default: + throw new ArgumentException("Invalid Serializer"); + } } - private static T Deserialize(byte[] buffer) + private T Deserialize(byte[] buffer) { - string ascii = Encoding.ASCII.GetString(buffer, 0, buffer.Length); - return JsonConvert.DeserializeObject(ascii); + switch (Serializer) + { + case SerializerEnum.JSON: + string ascii = Encoding.ASCII.GetString(buffer, 0, buffer.Length); + return JsonConvert.DeserializeObject(ascii); + case SerializerEnum.BSON: + System.IO.MemoryStream ms = new System.IO.MemoryStream(buffer); + Newtonsoft.Json.Bson.BsonDataReader reader = new Newtonsoft.Json.Bson.BsonDataReader(ms); + return new JsonSerializer().Deserialize(reader); + default: + throw new ArgumentException("Invalid Serializer"); + } } private static string GetUnusedCounterID(Dictionary dictionary, string name) diff --git a/Libraries/RosBridgeClient/packages.config b/Libraries/RosBridgeClient/packages.config index afdd9bd6d..737e72e95 100644 --- a/Libraries/RosBridgeClient/packages.config +++ b/Libraries/RosBridgeClient/packages.config @@ -1,5 +1,6 @@ - - - + + + + \ No newline at end of file diff --git a/Libraries/RosBridgeClientTest/RosBridgeClientTest.csproj b/Libraries/RosBridgeClientTest/RosBridgeClientTest.csproj index 832eaecbd..6f97f5a3e 100644 --- a/Libraries/RosBridgeClientTest/RosBridgeClientTest.csproj +++ b/Libraries/RosBridgeClientTest/RosBridgeClientTest.csproj @@ -1,90 +1,93 @@ - - - - - - - Debug - AnyCPU - {4E1BB3D0-8254-4D23-BD1B-F6EC7AA562FC} - Exe - Properties - RosBridgeClientTest - RosBridgeClientTest - v4.6 - 512 - - - - - - true - full - false - bin\Debug\ - DEBUG;TRACE - prompt - 4 - - - full - true - bin\Release\ - DEBUG;TRACE - prompt - 4 - true - - - RosSharp.RosBridgeClientTest.UrdfTransferToRosConsoleExample - - - - ..\packages\Newtonsoft.Json.12.0.1\lib\net45\Newtonsoft.Json.dll - - - ..\packages\NUnit.3.11.0\lib\net45\nunit.framework.dll - - - - - - - - - - - - - - - - - - - - - - - {27cd898a-8840-4cdd-a475-4a672fd2ef50} - RosBridgeClient - - - - - - - - - - - - - This project references NuGet package(s) that are missing on this computer. Use NuGet Package Restore to download them. For more information, see http://go.microsoft.com/fwlink/?LinkID=322105. The missing file is {0}. - - - - + + + + + + + Debug + AnyCPU + {4E1BB3D0-8254-4D23-BD1B-F6EC7AA562FC} + Exe + Properties + RosBridgeClientTest + RosBridgeClientTest + v4.6 + 512 + + + + + + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + full + true + bin\Release\ + DEBUG;TRACE + prompt + 4 + true + + + RosSharp.RosBridgeClientTest.UrdfTransferToRosConsoleExample + + + + ..\packages\Newtonsoft.Json.12.0.1\lib\net45\Newtonsoft.Json.dll + + + ..\packages\Newtonsoft.Json.Bson.1.0.2\lib\net45\Newtonsoft.Json.Bson.dll + + + ..\packages\NUnit.3.10.1\lib\net45\nunit.framework.dll + + + + + + + + + + + + + + + + + + + + + + + {27cd898a-8840-4cdd-a475-4a672fd2ef50} + RosBridgeClient + + + + + + + + + + + + + This project references NuGet package(s) that are missing on this computer. Use NuGet Package Restore to download them. For more information, see http://go.microsoft.com/fwlink/?LinkID=322105. The missing file is {0}. + + + +