RobotecAI / ros2-for-unity

High-performance ROS2 solution for Unity3D
Apache License 2.0
446 stars 58 forks source link

OccupencyGrid Subsciber Could Only Get a quarter of map #106

Open AlrayQiu opened 3 months ago

AlrayQiu commented 3 months ago

when I forward a occupency Grid, I find that It only Recieve a Quarter Of All Map, Actually , the Origin Map is Compressed Into Quarters

Normally, the two pictures should be the same

image

right is the origin map and left is forwarded

there is the code

void CallBack(OccupancyGrid msg)
    {
        lock (lock_)
        {
            gridMsg.Data = new sbyte[(int)(msg.Info.Height * msg.Info.Width)];
            for (int i = 0; i < msg.Info.Width; i++)
                for (int j = 0; j < msg.Info.Height; j++)
                {
                    gridMsg.Data[i * msg.Info.Height + j] = msg.Data[i * msg.Info.Height + j];
                    Debug.DrawLine(new Vector3(i, 0, j), new Vector3(i, 0, j) + Vector3.forward * msg.Info.Resolution, new Color((msg.Data[i * msg.Info.Height + j] * 128) / 255.0f, 1 - (msg.Data[i * msg.Info.Height + j] * 128) / 255.0f, 0), 0.5f);
                }
            gridMsg.Info.Height = msg.Info.Height;
            gridMsg.Info.Width = msg.Info.Width;
            gridMsg.Info.Resolution = msg.Info.Resolution;
            gridMsg.Header = new Header() { Frame_id = Frameid };
            msg.Dispose();
        }
    }
    IEnumerator Publish()
    {
        while (true)
        {
            lock (lock_)
            {
                publisher.Publish(gridMsg);
            }
            yield return new WaitForEndOfFrame();
        }
    }

Env: