Example #1
0
    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);
    }
Example #2
0
    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);
    }
Example #3
0
    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);
    }
Example #4
0
    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)");
            }
        }
    }
Example #5
0
    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);
    }
Example #6
0
 protected override void InitializeMessages()
 {
     micomWritingData = null;
 }