Merge pull request #33 from lge-ros2/develop

Merge 'develop' branch into 'master'

Assets/Scripts/DevicePlugins/DevicePlugin.InfoService.cs

@@ -9,6 +9,7 @@
 using UnityEngine;
 using ProtoBuf;
 using messages = gazebo.msgs;
+using Any = gazebo.msgs.Any;
 
 public abstract partial class DevicePlugin : DeviceTransporter, IDevicePlugin
 {
@@ -54,9 +55,7 @@ protected static void SetTransformInfoResponse(ref MemoryStream msTransformInfo,
 
 		var objectTransformInfo = new messages.Param();
 		objectTransformInfo.Name = "transform";
-		objectTransformInfo.Value = new messages.Any();
-		objectTransformInfo.Value.Type = messages.Any.ValueType.Pose3d;
-		objectTransformInfo.Value.Pose3dValue = objectPose;
+		objectTransformInfo.Value = new Any { Type = Any.ValueType.Pose3d, Pose3dValue = objectPose };
 
 		ClearMemoryStream(ref msTransformInfo);
 		Serializer.Serialize<messages.Param>(msTransformInfo, objectTransformInfo);
@@ -71,20 +70,15 @@ protected static void SetROS2CommonInfoResponse(ref MemoryStream msRos2Info, in
 
 		var ros2CommonInfo = new messages.Param();
 		ros2CommonInfo.Name = "ros2";
-		ros2CommonInfo.Value = new messages.Any();
-		ros2CommonInfo.Value.Type = messages.Any.ValueType.None;
+		ros2CommonInfo.Value = new Any { Type = Any.ValueType.None };
 
 		var ros2TopicName = new messages.Param();
 		ros2TopicName.Name = "topic_name";
-		ros2TopicName.Value = new messages.Any();
-		ros2TopicName.Value.Type = messages.Any.ValueType.String;
-		ros2TopicName.Value.StringValue = topicName;
+		ros2TopicName.Value = new Any { Type = Any.ValueType.String, StringValue = topicName };
 
 		var ros2FrameId = new messages.Param();
 		ros2FrameId.Name = "frame_id";
-		ros2FrameId.Value = new messages.Any();
-		ros2FrameId.Value.Type = messages.Any.ValueType.String;
-		ros2FrameId.Value.StringValue = frameId;
+		ros2FrameId.Value = new Any { Type = Any.ValueType.String, StringValue = frameId };
 
 		ros2CommonInfo.Childrens.Add(ros2TopicName);
 		ros2CommonInfo.Childrens.Add(ros2FrameId);

Assets/Scripts/DevicePlugins/ElevatorSystem.cs

@@ -45,7 +45,7 @@ public ElevatorTask(in int index)
 	private Dictionary<int, ElevatorEntity> elevatorList = new Dictionary<int, ElevatorEntity>();
 	private ConcurrentQueue<ElevatorTask> elevatorTaskQueue = new ConcurrentQueue<ElevatorTask>();
 
-	private string hashKey = string.Empty;
+	private string elevatorSystemName = string.Empty;
 
 	protected override void OnAwake()
 	{
@@ -99,6 +99,7 @@ protected override void OnReset()
 	public void ReadElevatorContext()
 	{
 		// ex) plugin parameters example
+		// <system_name>ElevatorSystem_00</system_name>
 		// <elevator prefix_name="Elevator_" speed="2">
 		//   <floor>floor_collision</floor>
 		//   <doors speed="0.6" closing_timer="10.0">
@@ -112,7 +113,7 @@ public void ReadElevatorContext()
 		//     </outside>
 		//   </doors>
 		// </elevator>
-
+		elevatorSystemName = parameters.GetValue<string>("system_name");
 		var elevatorPrefixName = parameters.GetAttribute<string>("elevator", "prefix_name");
 		var elevatorSpeed = parameters.GetAttribute<float>("elevator", "speed");
 		var elevatorFloor = parameters.GetValue<string>("elevator/floor");
@@ -243,12 +244,6 @@ private void GenerateResponseMessage()
 		responseMessage.Childrens.Add(heightParam);
 	}
 
-	private void SetResponseMessage(in string elevatorSystemName, in string serviceName)
-	{
-		responseMessage.Name = elevatorSystemName;
-		responseMessage.Childrens[0].Value.StringValue = serviceName;
-	}
-
 	private void SetResponseMessage(in bool result, in int elevatorIndex, in string currentFloor, in float height)
 	{
 		responseMessage.Childrens[1].Value.BoolValue = result;
@@ -265,21 +260,41 @@ private void ServiceThread()
 
 			if (receivedBuffer != null)
 			{
-				var receivedMessage = ParsingInfoRequest(receivedBuffer, ref msForService);
-				var streamToResponse = HandleServiceRequest(receivedMessage);
+				var requestMessage = ParsingInfoRequest(receivedBuffer, ref msForService);
 
-				SendResponse(streamToResponse);
+				if (requestMessage.Name.Equals("request_system_name"))
+				{
+					SetSystemNameResponse(requestMessage);
+					SendResponse(msForService);
+				}
+				else
+				{
+					var streamToResponse = HandleServiceRequest(requestMessage);
+					SendResponse(streamToResponse);
+				}
 			}
 
 			ThreadWait();
 		}
 	}
 
+	private void SetSystemNameResponse(in Param receivedMessage)
+	{
+		var response = new Param();
+		response.Name = "request_system_name";
+		response.Value = new Any { Type = Any.ValueType.String, StringValue = elevatorSystemName };
+		ClearMemoryStream(ref msForService);
+		Serializer.Serialize<Param>(msForService, response);
+	}
+
 	private MemoryStream HandleServiceRequest(in Param receivedMessage)
 	{
 		Param param = null;
 
-		var elevatorSystemName = receivedMessage.Name;
+		if (!elevatorSystemName.Equals(receivedMessage.Name))
+		{
+			Debug.LogWarningFormat("It's differnt elevator system name({0}) vs received({1})", elevatorSystemName, receivedMessage.Name);
+		}
 
 		var serviceName = string.Empty;
 		param = receivedMessage.Childrens[0];
@@ -313,9 +328,10 @@ private MemoryStream HandleServiceRequest(in Param receivedMessage)
 			elevatorIndex = param.Value.IntValue;
 		}
 
-		Debug.LogFormat("Parsed {0} {1} {2} {3} {4}", elevatorSystemName, serviceName, currentFloor, targetFloor, elevatorIndex);
+		// Debug.LogFormat("Parsed {0} {1} {2} {3} {4}", elevatorSystemName, serviceName, currentFloor, targetFloor, elevatorIndex);
 
-		SetResponseMessage(elevatorSystemName, serviceName);
+		responseMessage.Name = elevatorSystemName;
+		responseMessage.Childrens[0].Value.StringValue = serviceName;
 
 		HandleService(serviceName, currentFloor, targetFloor, elevatorIndex);
 

Assets/Scripts/DevicePlugins/MicomPlugin.cs

@@ -8,6 +8,7 @@
 using ProtoBuf;
 using Stopwatch = System.Diagnostics.Stopwatch;
 using messages = gazebo.msgs;
+using Any = gazebo.msgs.Any;
 
 public class MicomPlugin : DevicePlugin
 {
@@ -115,44 +116,34 @@ private void SetROS2TransformInfoResponse(ref MemoryStream msRos2Info)
 
 		var ros2CommonInfo = new messages.Param();
 		ros2CommonInfo.Name = "ros2";
-		ros2CommonInfo.Value = new messages.Any();
-		ros2CommonInfo.Value.Type = messages.Any.ValueType.None;
+		ros2CommonInfo.Value = new Any { Type = Any.ValueType.None };
 
 		var ros2TransformInfo = new messages.Param();
 		ros2TransformInfo.Name = "transform_name";
-		ros2TransformInfo.Value = new messages.Any();
-		ros2TransformInfo.Value.Type = messages.Any.ValueType.None;
+		ros2TransformInfo.Value = new Any { Type = Any.ValueType.None };
 		ros2CommonInfo.Childrens.Add(ros2TransformInfo);
 
 		var imu_name = parameters.GetValue<string>("ros2/transform_name/imu");
 		var imuInfo = new messages.Param();
 		imuInfo.Name = "imu";
-		imuInfo.Value = new messages.Any();
-		imuInfo.Value.Type = messages.Any.ValueType.String;
-		imuInfo.Value.StringValue = imu_name;
+		imuInfo.Value = new Any { Type = Any.ValueType.String, StringValue = imu_name };
 		ros2TransformInfo.Childrens.Add(imuInfo);
 
 		var wheelsInfo = new messages.Param();
 		wheelsInfo.Name = "wheels";
-		wheelsInfo.Value = new messages.Any();
-		wheelsInfo.Value.Type = messages.Any.ValueType.String;
-		wheelsInfo.Value.StringValue = imu_name;
+		wheelsInfo.Value = new Any { Type = Any.ValueType.None };
 		ros2TransformInfo.Childrens.Add(wheelsInfo);
 
 		var wheel_left_name = parameters.GetValue<string>("ros2/transform_name/wheels/left");
 		var wheelLeftInfo = new messages.Param();
 		wheelLeftInfo.Name = "left";
-		wheelLeftInfo.Value = new messages.Any();
-		wheelLeftInfo.Value.Type = messages.Any.ValueType.String;
-		wheelLeftInfo.Value.StringValue = wheel_left_name;
+		wheelLeftInfo.Value = new Any { Type = Any.ValueType.String, StringValue = wheel_left_name };
 		wheelsInfo.Childrens.Add(wheelLeftInfo);
 
 		var wheel_right_name = parameters.GetValue<string>("ros2/transform_name/wheels/right");
 		var wheelRightInfo = new messages.Param();
 		wheelRightInfo.Name = "right";
-		wheelRightInfo.Value = new messages.Any();
-		wheelRightInfo.Value.Type = messages.Any.ValueType.String;
-		wheelRightInfo.Value.StringValue = wheel_left_name;
+		wheelRightInfo.Value = new Any { Type = Any.ValueType.String, StringValue = wheel_right_name };
 		wheelsInfo.Childrens.Add(wheelRightInfo);
 
 		ClearMemoryStream(ref msRos2Info);
@@ -168,21 +159,16 @@ private void SetWheelInfoResponse(ref MemoryStream msWheelInfo)
 
 		var wheelInfo = new messages.Param();
 		wheelInfo.Name = "wheelInfo";
-		wheelInfo.Value = new messages.Any();
-		wheelInfo.Value.Type = messages.Any.ValueType.None;
+		wheelInfo.Value = new Any { Type = Any.ValueType.None };
 
 		var baseInfo = new messages.Param();
 		baseInfo.Name = "base";
-		baseInfo.Value = new messages.Any();
-		baseInfo.Value.Type = messages.Any.ValueType.Double;
-		baseInfo.Value.DoubleValue = micomSensor.WheelBase;
+		baseInfo.Value = new Any { Type = Any.ValueType.Double, DoubleValue = micomSensor.WheelBase };
 		wheelInfo.Childrens.Add(baseInfo);
 
 		var sizeInfo = new messages.Param();
 		sizeInfo.Name = "radius";
-		sizeInfo.Value = new messages.Any();
-		sizeInfo.Value.Type = messages.Any.ValueType.Double;
-		sizeInfo.Value.DoubleValue = micomSensor.WheelRadius;
+		sizeInfo.Value = new Any { Type = Any.ValueType.Double, DoubleValue = micomSensor.WheelRadius };
 		wheelInfo.Childrens.Add(sizeInfo);
 
 		ClearMemoryStream(ref msWheelInfo);

Assets/Scripts/DevicePlugins/MultiCameraPlugin.cs

@@ -9,6 +9,7 @@
 using ProtoBuf;
 using Stopwatch = System.Diagnostics.Stopwatch;
 using messages = gazebo.msgs;
+using Any = gazebo.msgs.Any;
 
 public class MultiCameraPlugin : DevicePlugin
 {
@@ -103,22 +104,18 @@ private void SetROS2FramesIdInfoResponse(ref MemoryStream msForInfoResponse, in
 
 		var ros2CommonInfo = new messages.Param();
 		ros2CommonInfo.Name = "ros2";
-		ros2CommonInfo.Value = new messages.Any();
-		ros2CommonInfo.Value.Type = messages.Any.ValueType.None;
+		ros2CommonInfo.Value = new Any { Type = Any.ValueType.None };
 
 		var ros2FramesIdInfo = new messages.Param();
 		ros2FramesIdInfo.Name = "frames_id";
-		ros2FramesIdInfo.Value = new messages.Any();
-		ros2FramesIdInfo.Value.Type = messages.Any.ValueType.None;
+		ros2FramesIdInfo.Value = new Any { Type = Any.ValueType.None };
 		ros2CommonInfo.Childrens.Add(ros2FramesIdInfo);
 
 		foreach (var frame_id in frames_id)
 		{
 			var ros2FrameId = new messages.Param();
 			ros2FrameId.Name = "frame_id";
-			ros2FrameId.Value = new messages.Any();
-			ros2FrameId.Value.Type = messages.Any.ValueType.String;
-			ros2FrameId.Value.StringValue = frame_id;
+			ros2FrameId.Value = new Any { Type = Any.ValueType.String, StringValue = frame_id };
 			ros2FramesIdInfo.Childrens.Add(ros2FrameId);
 		}
 

Assets/Scripts/DevicePlugins/RealSensePlugin.cs

@@ -8,8 +8,8 @@
 using System.IO;
 using UnityEngine;
 using ProtoBuf;
-using Stopwatch = System.Diagnostics.Stopwatch;
 using messages = gazebo.msgs;
+using Any = gazebo.msgs.Any;
 
 public class RealSensePlugin : DevicesPlugin
 {
@@ -25,14 +25,14 @@ protected override void OnAwake()
 
 	protected override void OnStart()
 	{
-		var rgbName = parameters.GetValue<string>("activate/module[@name='rgb']");
+		var colorName = parameters.GetValue<string>("activate/module[@name='color']");
 		var leftImagerName = parameters.GetValue<string>("activate/module[@name='left_imager']");
 		var rightImagerName = parameters.GetValue<string>("activate/module[@name='right_imager']");
 		var depthName = parameters.GetValue<string>("activate/module[@name='depth']");
 
-		if (rgbName != null)
+		if (colorName != null)
 		{
-			FindAndAddPlugin(rgbName);
+			FindAndAddPlugin(colorName);
 		}
 
 		if (leftImagerName != null)
@@ -109,16 +109,13 @@ protected static void SetModuleListInfoResponse(ref MemoryStream msModuleInfo, i
 
 		var modulesInfo = new messages.Param();
 		modulesInfo.Name = "activated_modules";
-		modulesInfo.Value = new messages.Any();
-		modulesInfo.Value.Type = messages.Any.ValueType.None;
+		modulesInfo.Value = new Any { Type = Any.ValueType.None };
 
 		foreach (var module in modules)
 		{
 			var moduleInfo = new messages.Param();
 			moduleInfo.Name = "module";
-			moduleInfo.Value = new messages.Any();
-			moduleInfo.Value.Type = messages.Any.ValueType.String;
-			moduleInfo.Value.StringValue = module;
+			moduleInfo.Value = new Any { Type = Any.ValueType.String, StringValue = module };
 			modulesInfo.Childrens.Add(moduleInfo);
 		}
 

Assets/Scripts/Devices/Clock.cs

@@ -7,6 +7,7 @@
 using System.Collections;
 using UnityEngine;
 using messages = gazebo.msgs;
+using Any = gazebo.msgs.Any;
 
 public class Clock : Device
 {
@@ -41,21 +42,16 @@ protected override void InitializeMessages()
 
 		timeInfo = new messages.Param();
 		timeInfo.Name = "timeInfo";
-		timeInfo.Value = new messages.Any();
-		timeInfo.Value.Type = messages.Any.ValueType.None;
+		timeInfo.Value = new Any { Type = Any.ValueType.None };
 
 		var simTimeParam = new messages.Param();
 		simTimeParam.Name = "simTime";
-		simTimeParam.Value = new messages.Any();
-		simTimeParam.Value.Type = messages.Any.ValueType.Time;
-		simTimeParam.Value.TimeValue = simTime;
+		simTimeParam.Value = new Any { Type = Any.ValueType.Time, TimeValue = simTime };
 		timeInfo.Childrens.Add(simTimeParam);
 
 		var realTimeParam = new messages.Param();
 		realTimeParam.Name = "realTime";
-		realTimeParam.Value = new messages.Any();
-		realTimeParam.Value.Type = messages.Any.ValueType.Time;
-		realTimeParam.Value.TimeValue = realTime;
+		realTimeParam.Value = new Any { Type = Any.ValueType.Time, TimeValue = realTime };
 		timeInfo.Childrens.Add(realTimeParam);
 	}
 

Assets/Scripts/UI/FollowingTargetList.cs

@@ -3,13 +3,15 @@
  *
  * SPDX-License-Identifier: MIT
  */
- 
+
 using UnityEngine;
 using TMPro;
 
 public class FollowingTargetList : MonoBehaviour
 {
 	private TMP_Dropdown dropdown = null;
+	private TMP_Dropdown.OptionData emptyOption_ = null;
+
 
 	private GameObject modelRoot = null;
 
@@ -28,6 +30,8 @@ void Start()
 		if (dropdown != null)
 		{
 			dropdown.onValueChanged.AddListener(OnDropDownValueChanged);
+
+			emptyOption_ = new TMP_Dropdown.OptionData("-- unfollowing --");
 		}
 	}
 
@@ -45,7 +49,7 @@ private void OnDropDownValueChanged(int choice)
 		}
 	}
 
-	public void UpdateList()
+	public void UpdateList(in int selectIndex = 0)
 	{
 		if (dropdown == null)
 		{
@@ -54,8 +58,7 @@ public void UpdateList()
 		}
 
 		dropdown.options.Clear();
-		var emptyOption = new TMP_Dropdown.OptionData() { text = "-- unfollowing --" };
-		dropdown.options.Add(emptyOption);
+		dropdown.options.Add(emptyOption_);
 
 		if (modelRoot != null)
 		{
@@ -70,7 +73,7 @@ public void UpdateList()
 			}
 		}
 
-		dropdown.value = 0;
+		dropdown.value = selectIndex;
 		dropdown.Select();
 		dropdown.RefreshShownValue();
 	}