protected override void InitializeMessages()
{
simTime = new messages.Time();
realTime = new messages.Time();
timeInfo = new messages.Param();
timeInfo.Name = "timeInfo";
timeInfo.Value = new Any {
Type = Any.ValueType.None
};
var simTimeParam = new messages.Param();
simTimeParam.Name = "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 Any {
Type = Any.ValueType.Time, 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, ros2CommonInfo);
}
protected override void GenerateMessage()
{
try
{
micomWritingData = GetMessageData <messages.Param>();
}
catch
{
Debug.LogWarning("GetMessageData: ERROR");
}
if (micomWritingData.Name.Equals("control") &&
micomWritingData.Childrens.Count == 2)
{
var child0 = micomWritingData.Childrens[0];
var child1 = micomWritingData.Childrens[1];
// Set reversed value due to differnt direction
// Right-handed -> Left-handed direction of rotation
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;
// Debug.Log(linearVelocity.ToString("F10") + ", " + (-child0.Value.DoubleValue).ToString("F10") + " | " + angularVelocity.ToString("F10") + ", " + (-child1.Value.DoubleValue).ToString("F10"));
}
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);
}
}
else if (micomWritingData.Name.Equals("command"))
{
if (micomWritingData.Value.StringValue.Equals("reset_odometry"))
{
micomForWheelDrive.Reset();
Debug.Log("MicomInput::command(reset_odometry)");
}
}
}
private void SetOutputTypeResponse(ref MemoryStream msInfo)
{
var output_type = parameters.GetValue <string>("output_type", "LaserScan");
var outputTypeInfo = new messages.Param();
outputTypeInfo.Name = "output_type";
outputTypeInfo.Value = new Any {
Type = Any.ValueType.String, StringValue = output_type
};
ClearMemoryStream(ref msInfo);
Serializer.Serialize <messages.Param>(msInfo, outputTypeInfo);
}
protected override void InitializeMessages()
{
micomWritingData = null;
}