protected override void InitializeMessages()
{
simTime = new messages.Time();
realTime = new messages.Time();
timeInfo = new messages.Param();
timeInfo.Name = "timeInfo";
timeInfo.Value = new messages.Any();
timeInfo.Value.Type = messages.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;
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;
timeInfo.Childrens.Add(realTimeParam);
}
private void SetWheelInfoResponse(ref MemoryStream msWheelInfo)
{
if (msWheelInfo == null)
{
return;
}
var wheelInfo = new messages.Param();
wheelInfo.Name = "wheelInfo";
wheelInfo.Value = new Any {
Type = Any.ValueType.None
};
var baseInfo = new messages.Param();
baseInfo.Name = "base";
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 Any {
Type = Any.ValueType.Double, DoubleValue = micomSensor.WheelRadius
};
wheelInfo.Childrens.Add(sizeInfo);
ClearMemoryStream(ref msWheelInfo);
Serializer.Serialize <messages.Param>(msWheelInfo, wheelInfo);
}
private void SetROS2TransformInfoResponse(ref MemoryStream msRos2Info)
{
if (msRos2Info == null)
{
return;
}
var ros2CommonInfo = new messages.Param();
ros2CommonInfo.Name = "ros2";
ros2CommonInfo.Value = new Any {
Type = Any.ValueType.None
};
var ros2TransformInfo = new messages.Param();
ros2TransformInfo.Name = "transform_name";
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 Any {
Type = Any.ValueType.String, StringValue = imu_name
};
ros2TransformInfo.Childrens.Add(imuInfo);
var wheelsInfo = new messages.Param();
wheelsInfo.Name = "wheels";
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 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 Any {
Type = Any.ValueType.String, StringValue = wheel_right_name
};
wheelsInfo.Childrens.Add(wheelRightInfo);
ClearMemoryStream(ref msRos2Info);
Serializer.Serialize <messages.Param>(msRos2Info, ros2TransformInfo);
}
protected override void GenerateMessage()
{
try
{
micomWritingData = GetMessageData <messages.Param>();
}
catch
{
Debug.LogWarning("GetMessageData: ERROR");
}
if (micomWritingData.Name.Equals("control_type") &&
micomWritingData.Childrens.Count == 2)
{
var child0 = micomWritingData.Childrens[0];
var child1 = micomWritingData.Childrens[1];
if (micomWritingData.Value.IntValue == 0)
{
controlType = VelocityType.LinearAndAngular;
linearVelocity
= (!child0.Name.Equals("LinearVelocity")) ? 0 : (float)child0.Value.DoubleValue;
angularVelocity
= (!child1.Name.Equals("AngularVelocity")) ? 0 : (float)child1.Value.DoubleValue;
}
else if (micomWritingData.Value.IntValue == 1)
{
controlType = VelocityType.LeftAndRight;
wheelLinearVelocityLeft
= (!child0.Name.Equals("LeftWheelVelocity")) ? 0 : (float)child0.Value.DoubleValue;
wheelLinearVelocityRight
= (!child1.Name.Equals("RightWheelVelocity")) ? 0 : (float)child1.Value.DoubleValue;
}
else
{
controlType = VelocityType.Unknown;
Debug.LogWarningFormat("MicomInput: Unsupported Control Type({0}", controlType);
}
}
}
protected override void GenerateMessage()
{
micomWritingData = GetMessageData <messages.Param>();
if (micomWritingData.Name.Equals("control_type") &&
micomWritingData.Value.IntValue == 1 &&
micomWritingData.Childrens.Count == 2)
{
wheelVelocityLeft
= (!micomWritingData.Childrens[0].Name.Equals("nLeftWheelVelocity")) ?
0 : micomWritingData.Childrens[0].Value.IntValue;
wheelVelocityRight
= (!micomWritingData.Childrens[1].Name.Equals("nRightWheelVelocity")) ?
0 : micomWritingData.Childrens[1].Value.IntValue;
// Debug.Log("nLeftWheelVel: " + wheelVelocityLeft + ", nRightWheelVel : " + wheelVelocityRight);
}
// Debug.Log("MicomInput: Working OK...");
}
protected override void InitializeMessages()
{
micomWritingData = null;
}