Exemplo n.º 1
0
    IEnumerator SendImu()
    {
        yield return(new WaitForEndOfFrame());

        var now            = DateTime.Now;
        var timeSpan       = now - lastFrame;
        var timeSinceStart = now - camStart;
        var timeMessage    = new TimeMsg(timeSinceStart.Seconds, timeSinceStart.Milliseconds);
        var headerMessage  = new HeaderMsg(count, timeMessage, "imu");

        Quaternion vehicleImu;

        vehicleImu = vehicle.transform.rotation;
        double xey     = vehicleImu.x;
        double yey     = vehicleImu.y;
        double zey     = vehicleImu.z;
        double wey     = vehicleImu.w;
        var    imuData = new QuaternionMsg(xey, yey, zey, wey);

        double[] arg3 = { 0.00, 0.00, 0.00 };

        var vect3 = new Vector3Msg(0.00, 0.00, 0.00);

        var imumessage = new ImuMessage(headerMessage, imuData, arg3, vect3, arg3, vect3, arg3);

        ros.Publish(imuPublisher.GetMessageTopic(), imumessage);
        ros.Render();
    }
    public void PublishData(Texture2D texture2D, int qualityLevel, int sequenceId)
    {
        HeaderMsg          header   = new HeaderMsg(sequenceId, new TimeMsg(0, 0), "raspicam");
        CompressedImageMsg imageMsg = new CompressedImageMsg(header, "jpg", texture2D.EncodeToJPG(qualityLevel));

        _publisher.PublishData(imageMsg);
    }
Exemplo n.º 3
0
 public OdometryMsg(PoseWithCovarianceMsg pose)
 {
     _header         = new HeaderMsg(0, new TimeMsg(0, 0), "0");
     _child_frame_id = "0";
     _pose           = pose;
     _twist          = new TwistWithCovarianceMsg(new TwistMsg(new Vector3Msg(0, 0, 0), new Vector3Msg(0, 0, 0)), new double[36]);
 }
 public CompressedImageMsg(HeaderMsg header, string format,
                           byte[] data)
 {
     _header = header;
     _format = format;
     _data   = data;
 }
Exemplo n.º 5
0
    IEnumerator SendImage()
    {
        yield return(new WaitForEndOfFrame());

        int       width  = 640;
        int       height = 480;
        Texture2D tex    = new Texture2D(width, height, TextureFormat.RGB24, false);

        tex.ReadPixels(new Rect(0, 0, width, height), 0, 0);
        tex.Apply();
        byte[] data = tex.EncodeToJPG();

        var now            = DateTime.Now;
        var timeSpan       = now - lastFrame;
        var timeSinceStart = now - camStart;
        var timeMessage    = new TimeMsg(timeSinceStart.Seconds, timeSinceStart.Milliseconds);
        var headerMessage  = new HeaderMsg(count, timeMessage, "camera");

        Debug.Log("data length: " + data.Length);
        string format             = "jpeg";
        var    compressedImageMsg = new CompressedImageMsg(headerMessage, format, data);

        Debug.Log(compressedImageMsg);
        ros.Publish(CompressedImagePublisher.GetMessageTopic(), compressedImageMsg);
        Destroy(tex);
        ros.Render();
    }
Exemplo n.º 6
0
        public RNDataMsg(JSONNode node)
        {
            header       = new HeaderMsg(node["header"]);
            width        = node["width"].AsInt;
            is_bigendian = node["is_bigendian"].AsBool;
            point_step   = node["point_step"].AsInt;
            scalar_step  = node["scalar_step"].AsInt;
            is_dense     = node["is_dense"].AsBool;

            JSONArray fields_arr = node["fields"].AsArray;

            fields = new RNPointFieldMsg[fields_arr.Count];
            for (int i = 0; i < fields.Length; i++)
            {
                fields[i] = new RNPointFieldMsg(fields_arr[i]);
            }
            fields_arr    = node["scalar_fields"].AsArray;
            scalar_fields = new RNPointFieldMsg[fields_arr.Count];
            for (int i = 0; i < scalar_fields.Length; i++)
            {
                scalar_fields[i] = new RNPointFieldMsg(fields_arr[i]);
            }

            data = ROSBridgeUtils.ParseJSONRawData(node["data"], is_bigendian);
        }
Exemplo n.º 7
0
    void Update()
    {
        ros.Render();

        if (!_running)
        {
            return;
        }

        timer -= Time.deltaTime;

        if (timer <= 0)
        {
            timer = pollRate;

            PointMsg               point  = new PointMsg(1, 2, 3);
            QuaternionMsg          quat   = new QuaternionMsg(1, 2, 3, 4);
            PoseMsg                pose   = new PoseMsg(point, quat);
            PoseWithCovarianceMsg  posec  = new PoseWithCovarianceMsg(pose);
            Vector3Msg             vec3   = new Vector3Msg(1, 2, 3);
            TwistMsg               twist  = new TwistMsg(vec3, vec3);
            TwistWithCovarianceMsg twistc = new TwistWithCovarianceMsg(twist, new double[36]);
            HeaderMsg              header = new HeaderMsg(1, new TimeMsg(1, 1), "0");

            PoseStampedMsg ps  = new PoseStampedMsg(header, pose);
            PathMsg        msg = new PathMsg(header, new PoseStampedMsg[] { ps, ps, ps });

            BoolMsg             boolmsg = new BoolMsg(true);
            StringMsg           str     = new StringMsg("This is a test");
            RegionOfInterestMsg roi     = new RegionOfInterestMsg(0, 1, 2, 3, true);
            CameraInfoMsg       caminfo = new CameraInfoMsg(header, 100, 200, "plumb_bob", new double[5], new double[9], new double[9], new double[12], 14, 123, roi);

            _genericPub.PublishData(caminfo);
        }
    }
Exemplo n.º 8
0
 public OdometryMsg(HeaderMsg header, string child_frame_id, PoseWithCovarianceMsg pose, TwistWithCovarianceMsg twist)
 {
     _header         = header;
     _child_frame_id = child_frame_id;
     _pose           = pose;
     _twist          = twist;
 }
Exemplo n.º 9
0
        public void SetTFTrackingSettings(TFTrackingSettings tfTrackingType, HeaderMsg headerMsg)
        {
            switch (tfTrackingType.type)
            {
            case TFTrackingType.Exact:
            {
                TFFrame frame = TFSystem.instance.GetTransform(headerMsg, tfTrackingType.tfTopic);
                transform.position = frame.translation;
                transform.rotation = frame.rotation;
            }
            break;

            case TFTrackingType.TrackLatest:
            {
                transform.parent        = TFSystem.instance.GetTransformObject(headerMsg.frame_id, tfTrackingType.tfTopic).transform;
                transform.localPosition = Vector3.zero;
                transform.localRotation = Quaternion.identity;
            }
            break;

            case TFTrackingType.None:
                transform.localPosition = Vector3.zero;
                transform.localRotation = Quaternion.identity;
                break;
            }
        }
Exemplo n.º 10
0
 public CameraInfoMsg(JSONNode msg)
 {
     _header           = new HeaderMsg(msg["header"]);
     _height           = uint.Parse(msg["height"]);
     _distortion_model = msg["distortion_model"];
     _d = new double[msg["D"].Count];
     _k = new double[msg["K"].Count];
     _r = new double[msg["R"].Count];
     _p = new double[msg["P"].Count];
     for (int i = 0; i < msg["D"].Count; i++)
     {
         _d[i] = double.Parse(msg["D"][i], CultureInfo.InvariantCulture);
     }
     for (int i = 0; i < msg["K"].Count; i++)
     {
         _k[i] = double.Parse(msg["K"][i], CultureInfo.InvariantCulture);
     }
     for (int i = 0; i < msg["R"].Count; i++)
     {
         _r[i] = double.Parse(msg["R"][i], CultureInfo.InvariantCulture);
     }
     for (int i = 0; i < msg["P"].Count; i++)
     {
         _p[i] = double.Parse(msg["P"][i], CultureInfo.InvariantCulture);
     }
     _binning_x = uint.Parse(msg["binning_x"], CultureInfo.InvariantCulture);
     _binning_y = uint.Parse(msg["binning_y"], CultureInfo.InvariantCulture);
     _roi       = new RegionOfInterestMsg(msg["roi"]);
 }
Exemplo n.º 11
0
 public OdometryMsg(JSONNode msg)
 {
     _header         = new HeaderMsg(msg["header"]);
     _child_frame_id = msg["child_frame_id"];
     _pose           = new PoseWithCovarianceMsg(msg["pose"]);
     _twist          = new TwistWithCovarianceMsg(msg["twist"]);
 }
Exemplo n.º 12
0
        private IEnumerator SendPathToROS(ROS ros)
        {
            while (Application.isPlaying)
            {
                if (ros.IsConnected())
                {
                    Vector3[]        points   = agent.path.corners;
                    PoseStampedMsg[] poses    = new PoseStampedMsg[points.Length];
                    HeaderMsg        head     = new HeaderMsg(0, new TimeMsg(DateTime.Now.Second, 0), "map");
                    Quaternion       rotation = transform.rotation;
                    for (int i = 0; i < points.Length; i++)
                    {
                        head.SetSeq(i);
                        if (i > 0)
                        {
                            rotation = Quaternion.FromToRotation(points[i - 1], points[i]);
                        }

                        poses[i] = new PoseStampedMsg(head, new PoseMsg(points[i], rotation, true));
                    }

                    HeaderMsg globalHead = new HeaderMsg(0, new TimeMsg(DateTime.Now.Second, 0), "map");
                    PathMsg   pathmsg    = new PathMsg(globalHead, poses);
                    ros.Publish(Path_pub.GetMessageTopic(), pathmsg);
                }
                yield return(new WaitForSeconds(rOSFrecuencyPath));
            }
        }
Exemplo n.º 13
0
 public override void Deserialize(JSONNode msg)
 {
     _header         = new HeaderMsg(msg["header"]);
     _child_frame_id = msg["child_frame_id"].ToString();
     _pose           = new PoseWithCovarianceMsg(msg["pose"]);
     _twist          = new TwistWithCovarianceMsg(msg["twist"]);
 }
                public BatteryStateMsg(JSONNode msg)
                {
                    _header = new HeaderMsg(msg["header"]);
                    if (!msg["capacity"].Value.Equals("null"))
                    {
                        _capacity = float.Parse(msg["capacity"], System.Globalization.CultureInfo.InvariantCulture);
                    }
                    if (!msg["charge"].Value.Equals("null"))
                    {
                        _charge = float.Parse(msg["charge"], System.Globalization.CultureInfo.InvariantCulture);
                    }
                    _current         = float.Parse(msg["current"], System.Globalization.CultureInfo.InvariantCulture);
                    _design_capacity = float.Parse(msg["design_capacity"], System.Globalization.CultureInfo.InvariantCulture);
                    if (!msg["percentage"].Value.Equals("null"))
                    {
                        _percentage = float.Parse(msg["percentage"], System.Globalization.CultureInfo.InvariantCulture);
                    }
                    _voltage = float.Parse(msg["voltage"], System.Globalization.CultureInfo.InvariantCulture);

                    _cell_voltage = new float[msg["cell_voltage"].Count];
                    for (int i = 0; i < _cell_voltage.Length; i++)
                    {
                        _cell_voltage[i] = float.Parse(msg["cell_voltage"][i], System.Globalization.CultureInfo.InvariantCulture);
                    }
                    _location                = msg["location"];
                    _power_supply_health     = uint.Parse(msg["power_supply_health"]);
                    _power_supply_status     = uint.Parse(msg["power_supply_status"]);
                    _power_supply_technology = uint.Parse(msg["power_supply_technology"]);
                    _present       = bool.Parse(msg["present"]);
                    _serial_number = msg["serial_number"];
                }
Exemplo n.º 15
0
 public PathMsg(JSONNode msg)
 {
     _header = new HeaderMsg(msg["header"]);
     for (int i = 0; i < msg["poses"].Count; i++)
     {
         _poses[i] = (new PoseStampedMsg(msg["poses"][i]));
     }
 }
Exemplo n.º 16
0
 /**
  * C-tor with a member list
  */
 public JointStateMsg(HeaderMsg h, string [] n, float [] p, float [] v, float [] e)
 {
     _header   = h;
     _name     = n;
     _position = p;
     _velocity = v;
     _effort   = e;
 }
Exemplo n.º 17
0
        public static void GUI(this HeaderMsg message)
        {
#if !ROS2
            GUILayout.Label($"<{message.seq} {message.frame_id} {message.stamp.ToTimestampString()}>");
#else
            GUILayout.Label($"<{message.frame_id} {message.stamp.ToTimestampString()}>");
#endif
        }
Exemplo n.º 18
0
 public OctomapMsg()
 {
     this.header     = new HeaderMsg();
     this.binary     = false;
     this.id         = "";
     this.resolution = 0.0;
     this.data       = new sbyte[0];
 }
Exemplo n.º 19
0
 public BodyVelocityRequestMsg(JSONNode msg)
 {
     // parse message fields into class data
     _header       = new HeaderMsg(msg["header"]);
     _goal         = new GoalDescriptorMsg(msg["goal"]);
     _twist        = new TwistMsg(msg["twist"]);
     _disable_axis = new Bool6AxisMsg(msg["disable_axis"]);
 }
Exemplo n.º 20
0
 public VelocityMsg(HeaderMsg header, double axis_x, double axis_y, double axis_z, double yaw)
 {
     _header = header;
     _axis_x = new Float64Msg(axis_x);
     _axis_y = new Float64Msg(axis_y);
     _axis_z = new Float64Msg(axis_z);
     _yaw    = new Float64Msg(yaw);
 }
Exemplo n.º 21
0
 private OctomapMsg(MessageDeserializer deserializer)
 {
     this.header = HeaderMsg.Deserialize(deserializer);
     deserializer.Read(out this.binary);
     deserializer.Read(out this.id);
     deserializer.Read(out this.resolution);
     deserializer.Read(out this.data, sizeof(sbyte), deserializer.ReadLength());
 }
Exemplo n.º 22
0
 public OctomapMsg(HeaderMsg header, bool binary, string id, double resolution, sbyte[] data)
 {
     this.header     = header;
     this.binary     = binary;
     this.id         = id;
     this.resolution = resolution;
     this.data       = data;
 }
 public DetectionArrayMsg(JSONNode msg) {
     header = new HeaderMsg(msg["header"]);
     origin = new PoseMsg(msg["origin"]);
     detections = new DetectionMsg[msg["detections"].Count];
     for (int i = 0; i < detections.Length; i++) {
         detections[i] = new DetectionMsg(msg["detections"][i]);
     }
 }
Exemplo n.º 24
0
 public ImageMsg(HeaderMsg header, uint height, uint width, string encoding, bool is_bigendian, uint row_step, byte[] data)
 {
     _header       = header;
     _height       = height;
     _width        = width;
     _encoding     = encoding;
     _is_bigendian = is_bigendian;
     _row_step     = row_step;
 }
Exemplo n.º 25
0
 public PeopleGoalArrayMsg(JSONNode msg)
 {
     _header = new HeaderMsg(msg["header"]);
     _users  = new PersonGoalMsg[msg["users"].Count];
     for (int i = 0; i < _users.Length; i++)
     {
         _users[i] = new PersonGoalMsg(msg["users"][i]);
     }
 }
Exemplo n.º 26
0
 public override void Deserialize(JSONNode msg)
 {
     _header = new HeaderMsg(msg["header"]);
     // Treat poses
     for (int i = 0; i < msg["poses"].Count; i++)
     {
         _poses.Add(new PoseStampedMsg(msg["poses"][i]));
     }
 }
Exemplo n.º 27
0
 public SemanticObjectArrayMsg(JSONNode msg)
 {
     _header          = new HeaderMsg(msg["header"]);
     _semanticObjects = new SemanticObjectMsg[msg["semanticObjects"].Count];
     for (int i = 0; i < _semanticObjects.Length; i++)
     {
         _semanticObjects[i] = new SemanticObjectMsg(msg["semanticObjects"][i]);
     }
 }
Exemplo n.º 28
0
 public override void Deserialize(JSONNode msg)
 {
     _header         = new HeaderMsg(msg ["header"]);
     _range          = float.Parse(msg ["range"]);
     _min_range      = float.Parse(msg ["min_range"]);
     _max_range      = float.Parse(msg ["max_range"]);
     _radiation_type = uint.Parse(msg ["radiation_type"]);
     _field_of_view  = float.Parse(msg ["field_of_view"]);
 }
Exemplo n.º 29
0
 public RangeMsg(HeaderMsg header, float range, float min_range, float max_range, float field_of_view, uint radiation_type)
 {
     _header         = header;
     _range          = range;
     _min_range      = min_range;
     _max_range      = max_range;
     _field_of_view  = field_of_view;
     _radiation_type = radiation_type;
 }
Exemplo n.º 30
0
 public RangeMsg(JSONNode msg)
 {
     _header         = new HeaderMsg(msg ["header"]);
     _range          = float.Parse(msg ["range"]);
     _min_range      = float.Parse(msg ["min_range"]);
     _max_range      = float.Parse(msg ["max_range"]);
     _radiation_type = uint.Parse(msg ["radiation_type"]);
     _field_of_view  = float.Parse(msg ["field_of_view"]);
 }
Exemplo n.º 31
0
        protected void PreloadStrings(Stream input)
        {
            try
            {
                if (Strings.Count > 0) Strings.Clear();

                _bundleFileHeader = input.ReadStruct<FileHeaderStruct>();
                if (_bundleFileHeader.Magic != GETTEXT_MAGIC)
                    throw new IOException("Invalid MAGIC");

                var originalHeaders = ReadStringHeaders(input, _bundleFileHeader.OriginalTableOffset,
                                                        _bundleFileHeader.NumberOfStrings);
                var translationHeaders = ReadStringHeaders(input, _bundleFileHeader.TranslationTableOffset,
                                                           _bundleFileHeader.NumberOfStrings);

                var originals = new string[_bundleFileHeader.NumberOfStrings][];
                var translations = new string[_bundleFileHeader.NumberOfStrings][];
                for (var x = 0; x < _bundleFileHeader.NumberOfStrings; x++)
                    originals[x] = ReadTextEntry(input, originalHeaders[x].Offset, originalHeaders[x].Length);
                for (var x = 0; x < _bundleFileHeader.NumberOfStrings; x++)
                    translations[x] = ReadTextEntry(input, translationHeaders[x].Offset, translationHeaders[x].Length);

                for (var x = 0; x < _bundleFileHeader.NumberOfStrings; x++)
                {
                    switch (originals[x].Length)
                    {
                        case 1:
                            Strings.TryAdd(originals[x][0], translations[x]);
                            break;
                        case 2:
                            Strings.TryAdd(originals[x][0], new[] {translations[x][0]});
                            Strings.TryAdd(originals[x][1], translations[x]);
                            break;
                    }
                }
            }
            finally
            {
                _headerMsg = new HeaderMsg(GetString(string.Empty));
            }
        }