Subscribing to OccupancyGrid

[youngbin-song]

Hi. First of all, thank you for making this plugin.

I've been trying to get the OccupancyGrid data from ROS but I'm having a lot of trouble doing it. I was able to subscribe to the Topicd but I couldn't get any callback strings from the code.

If I refer to #70, a converter for OccupancyGrid was added to the repo. Does that mean that I need to replace the converter in Topic.cpp to accommodate for UNavMsgsOccupancyGrid? Or does the UBaseMessageConverter converter work with it?

I think it would be best if an example code would help me but any help is appreciated!

[Sanic]

Hi @youngbin-song
Have you been able to set up a minimal working example of a topic subscription that works? For example a topic with a string? As far as i understand your task, i would have expected that you only need to take care of the cast/conversion in the callback to the concrete data type (the occupancygrid). If the callback is never triggered, i would suggest to check first if the rosbridge node is actually subscribed to the topic you are interested in and check if errors occur in the rosbridge and the UE4 logs.

[youngbin-song]

Hi @Sanic

Thank you for you reply. Yes, I've managed to get the minimal example working on my side so there is no problem with the connection.

This is the BeginPlay part of my subscriber. (Ignore the actor name)

void AROSSubscriber::BeginPlay()
{
    // Initialize a topic
    ExampleTopic = NewObject<UTopic>(UTopic::StaticClass());
    UROSIntegrationGameInstance* rosinst = Cast<UROSIntegrationGameInstance>(GetGameInstance());
    ExampleTopic->Init(rosinst->ROSIntegrationCore, TEXT("/vision60_1/map"), TEXT("nav_msgs/OccupancyGrid"));

    // Create a std::function callback object
    UE_LOG(LogTemp, Log, TEXT("BEGIN PLAYING@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@"));
    std::function<void(TSharedPtr<FROSBaseMsg>)> SubscribeCallback = [](TSharedPtr<FROSBaseMsg> msg) -> void
    {
        bool subscribed = true;
        auto ConcreteNavMessage = StaticCastSharedPtr<ROSMessages::nav_msgs::OccupancyGrid>(msg);
        if (ConcreteNavMessage.IsValid())
        {
            const uint32 width = ConcreteNavMessage->info.width;
            const uint32 height = ConcreteNavMessage->info.height;

            UE_LOG(LogTemp, Log, TEXT("Incoming string was: %s"), width);
        }

        else 
        {
            UE_LOG(LogTemp, Log, TEXT("not valid"));
        }
        return;
    };

i would have expected that you only need to take care of the cast/conversion in the callback to the concrete data type (the occupancygrid)

Does this refer to the auto ConcreteNavMessage = StaticCastSharedPtr<ROSMessages::nav_msgs::OccupancyGrid>(msg); part of the code? Please let me know if I've made a mistake in implementing it. I'm fairly new to the C++ side of Unreal Engine.

Running this currently causes a crash. The crash report says

LoginId:4e8de6e84bdb7b4a57f8bdbde96c94bc
EpicAccountId:66067d7de61b4f5aaaca51f509ccc7fd

Unhandled Exception: EXCEPTION_ACCESS_VIOLATION reading address 0x00007ffe0000007d

ucrtbase
ucrtbase
ucrtbase
ucrtbase
UE4Editor_Core
UE4Editor_CPPTest_exe!<lambda_802baacf9133baab46ee77bff8c6dc30>::operator()() [C:\Users\Youngbin\Documents\Unreal Projects\CPPTest\Source\CPPTest\ROSPublisher.cpp:39]
UE4Editor_CPPTest_exe!std::_Func_impl_no_alloc<<lambda_802baacf9133baab46ee77bff8c6dc30>,void,TSharedPtr<FROSBaseMsg,0> >::_Do_call() [C:\Program Files (x86)\Microsoft Visual Studio\2019\Community\VC\Tools\MSVC\14.29.30133\INCLUDE\functional:921]
UE4Editor_ROSIntegration_exe!UTopic::Impl::MessageCallback() [C:\Users\Youngbin\Documents\Unreal Projects\CPPTest\Plugins\ROSIntegration\Source\ROSIntegration\Private\Topic.cpp:161]
UE4Editor_ROSIntegration_exe!rosbridge2cpp::ROSBridge::HandleIncomingPublishMessage() [C:\Users\Youngbin\Documents\Unreal Projects\CPPTest\Plugins\ROSIntegration\Source\ROSIntegration\Private\rosbridge2cpp\ros_bridge.cpp:172]
UE4Editor_ROSIntegration_exe!rosbridge2cpp::ROSBridge::IncomingMessageCallback() [C:\Users\Youngbin\Documents\Unreal Projects\CPPTest\Plugins\ROSIntegration\Source\ROSIntegration\Private\rosbridge2cpp\ros_bridge.cpp:242]
UE4Editor_ROSIntegration_exe!TCPConnection::ReceiverThreadFunction() [C:\Users\Youngbin\Documents\Unreal Projects\CPPTest\Plugins\ROSIntegration\Source\ROSIntegration\Private\rosbridge2cpp\TCPConnection.cpp:187]
UE4Editor_ROSIntegration_exe!std::thread::_Invoke<std::tuple<int (__cdecl TCPConnection::*)(void),TCPConnection *>,0,1>() [C:\Program Files (x86)\Microsoft Visual Studio\2019\Community\VC\Tools\MSVC\14.29.30133\INCLUDE\thread:56]
ucrtbase
kernel32
ntdll

Before the crash, the callback was working fine. However, it wouldn't print the 'width' value from the OccupancyGrid (from the MapMetaData). Also an additional info is that these OccupancyGrid data is not taken from UE4 but from an external ROS topic.

Thanks again.

[Sanic]

Can you narrow the error down a bit more?
Is the crash caused by the calls on the ConcreteNavMessage? or on the cast?
If it's the first, could you check if you can access attributes from the header?
Can you access attributes from the FROSBaseMsg? For example _MessageType?

[youngbin-song]

Hey @Sanic. Sorry for the late reply to the thread. I was taking care of another project.

I just got to test the code I've posted previously and it seems like the crash is caused by the calls on the ConcreteNavMessages. I updated the code to see if there was anything wrong with the messages and I've put them here.

void AOccupancyGridSubscriber::BeginPlay()
{
    UPROPERTY();
    // Initialize a topic
    UE_LOG(LogTemp, Warning, TEXT("BEGIN PLAYING @@@@@@@@@@@@@@"));
    ExampleTopic = NewObject<UTopic>(UTopic::StaticClass());
    UROSIntegrationGameInstance* rosinst = Cast<UROSIntegrationGameInstance>(GetGameInstance());
    ExampleTopic->Init(rosinst->ROSIntegrationCore, TEXT("/vision60_1/map"), TEXT("nav_msgs/OccupancyGrid"));


    // Create a std::function callback object
    std::function<void(TSharedPtr<FROSBaseMsg>)> SubscribeCallback = [](TSharedPtr<FROSBaseMsg> msg) -> void
    {
        bool subscribed = true;
        auto ConcreteNavMessage = StaticCastSharedPtr<ROSMessages::nav_msgs::OccupancyGrid>(msg);
        if (ConcreteNavMessage.IsValid())
        {
            UE_LOG(LogTemp, Warning, TEXT("Message Valid!"));
            const uint32 seq = ConcreteNavMessage->header.seq; // Error is here (Message is valid but this part causes a crash)

            UE_LOG(LogTemp, Warning, TEXT("Incoming string was: %s"), seq);
        }

        else
        {
            UE_LOG(LogTemp, Warning, TEXT("Not valid"));
        }
        return;
    };
    ExampleTopic->Subscribe(SubscribeCallback);

	Super::BeginPlay();
}

The log was able to print the line Message Valid! before crashing. Without the line to access the ConcreteNavMsgs, it wouldn't crash at all (even though I've changed the accessed variable from width to header.seq).

As to access the attributes from the FROSBaseMsg, I'm not entirely sure how to access them as I'm new to the whole C++ thing in UE4.

[youngbin-song]

After some digging around, I was able to confirm that the message type Occupancy Grid from the callback function using this code.

void AOccupancyGridSubscriber::BeginPlay()
{
    UPROPERTY();
    // Initialize a topic
    UE_LOG(LogTemp, Warning, TEXT("BEGIN PLAYING @@@@@@@@@@@@@@"));
    ExampleTopic = NewObject<UTopic>(UTopic::StaticClass());
    UROSIntegrationGameInstance* rosinst = Cast<UROSIntegrationGameInstance>(GetGameInstance());
    ExampleTopic->Init(rosinst->ROSIntegrationCore, TEXT("/vision60_1/map"), TEXT("nav_msgs/OccupancyGrid"));


    // Create a std::function callback object
    std::function<void(TSharedPtr<FROSBaseMsg>)> SubscribeCallback = [](TSharedPtr<FROSBaseMsg> msg) -> void
    {
        bool subscribed = true;
        auto ConcreteNavMessage = StaticCastSharedPtr<ROSMessages::nav_msgs::OccupancyGrid>(msg);
        if (ConcreteNavMessage.IsValid())
        {
            UE_LOG(LogTemp, Warning, TEXT("Message Valid! Message type is %s"), (*(ConcreteNavMessage->_MessageType)));
            // const uint32 seq = ConcreteNavMessage->header.seq; // Error is here (Message is valid but this part causes a crash)
            
            // UE_LOG(LogTemp, Warning, TEXT("Incoming string was: %s"), seq);
        }

        else
        {
            UE_LOG(LogTemp, Warning, TEXT("Not valid"));
        }
        return;
    };
    ExampleTopic->Subscribe(SubscribeCallback);

	Super::BeginPlay();
}

I also noticed that the updating time for Unreal Engine wasn't the same as the ROS Bag. I'm not sure if this provides any clarity to the problem. If I remember correctly, the callback function was called approximately 4 times and then it wasn't called at all after that. Please let me know if there are any info require from me.

Update # 1

Seems like trying to print the seq in the log will cause the editor to crash.

Update # 2

I've figured it out. Because I was new to the whole C++ side, I made a very simple mistake. The editor crashed when running the line
// UE_LOG(LogTemp, Warning, TEXT("Incoming string was: %s"), seq);

I assume that it's because the log was expecting a string but received an integer instead which caused the editor to crash. Everything seems to work out fine except that the rate of the callback is not what I expect too.

It seems that, at some point, I get an error like below.

LogTemp: Warning: Message Valid! Message type is nav_msgs/OccupancyGrid
LogTemp: Warning: Incoming integer was: 163
LogTemp: Warning: Message Valid! Message type is nav_msgs/OccupancyGrid
LogTemp: Warning: Incoming integer was: 163
LogROS: Error: Error on BSON parse - Ignoring message