ROSHandler.cpp

Go to the documentation of this file.

// Copyright (c) 2023 FrostBit Software Lab at the Lapland University of Applied Sciences
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#include "ROSHandler.h"
 
#include "Containers/StringConv.h"
#include "Engine/World.h"
 
void UROSHandler::Setup(bool RosIsConnected)
{
    // Bind to OnROSBridgeChanged event
    OnROSStateChanged.AddUniqueDynamic(this, &UROSHandler::OnROSBridgeChanged);
 
    // Set ROS connected state
    ROSConnected = RosIsConnected;
    ROSState = ROSConnected == true ? EROSState::Connected : EROSState::Disconnected;
}
 
void UROSHandler::LaunchROSBridge()
{
    if (ROSConnected)
    {
#if WITH_EDITOR
        UE_LOG(LogTemp, Error, TEXT("ROSHandler.cpp: ROS is already active."));
#endif
        return;
    }
 
#if PLATFORM_LINUX
    FString command = "gnome-terminal -- bash -c \"roslaunch rosbridge_server rosbridge_tcp.launch bson_only_mode:=True""\"";
 
#if WITH_EDITOR
    UE_LOG(LogTemp, Warning, TEXT("ROSHandler.cpp: Attempting to launch ROS with command: %s"), *command);
#endif
    // Attempt to launch ROS on Linux
    system(TCHAR_TO_ANSI(*command));
 
#elif PLATFORM_WINDOWS
 
    // Attempt to launch ROS on Windows via WSL
 
    // Create the process
    FString Application = "wsl";
    FString CommandParams = "-e bash -lic \"roslaunch rosbridge_server rosbridge_tcp.launch bson_only_mode:=True""\"";
 
    // Added LaunchParams for ROS1 to ROS2 bridge. ROS2 command is in .bashrc, check build_windows.md
    FString BridgeCommands = "-e bash -lic \"ROS2 && ros2 run ros1_bridge dynamic_bridge --bridge-all-topics""\"";
 
    ROSProcessHandle = FPlatformProcess::CreateProc(*Application, *CommandParams, true, false, false, nullptr, 0, nullptr, nullptr);
    ROSBridgeHandle = FPlatformProcess::CreateProc(*Application, *BridgeCommands, true, false, false, nullptr, 0, nullptr, nullptr);
 
#if WITH_EDITOR
    if (!ROSProcessHandle.IsValid())
    {
        UE_LOG(LogTemp, Error, TEXT("ROSHandler.cpp: Failed to create wsl process"));
    }
 
    if (!ROSBridgeHandle.IsValid())
    {
        UE_LOG(LogTemp, Error, TEXT("ROSHandler.cpp: Bridge not installed"));
    }
#endif
 
#endif
}
 
void UROSHandler::StopROSBridge()
{
    if (!ROSConnected)
    {
        return;
    }
 
    FString StopCommand = "killall -9 rosmaster";
 
#if PLATFORM_LINUX
    // Attempt to stop ROS on Linux
    system(TCHAR_TO_ANSI(*StopCommand));
 
#elif PLATFORM_WINDOWS
    // Attempt to stop ROS on Windows
    if (ROSBridgeHandle.IsValid())
    {
        if (FPlatformProcess::IsProcRunning(ROSBridgeHandle))
        {
            FPlatformProcess::TerminateProc(ROSBridgeHandle);
        }
    }
 
    if (ROSProcessHandle.IsValid())
    {
        if (FPlatformProcess::IsProcRunning(ROSProcessHandle))
        {
            FPlatformProcess::TerminateProc(ROSProcessHandle);
        }
    }
    else
    {
        // Else ROS Bridge might have been opened by the user, so let's
        // stop it by opening new wsl window and giving StopCommand.
        FString Application = "wsl";
        FProcHandle Handle = FPlatformProcess::CreateProc(*Application, *StopCommand, true, false, false, nullptr, 0, nullptr, nullptr);
    }
#endif
 
    ROSConnected = false;
}
 
void UROSHandler::RestartROSBridge()
{
    StopROSBridge();
    LaunchROSBridge();
}
 
void UROSHandler::OnROSBridgeChanged(EROSState newRosState)
{
    ROSState = newRosState;
    ROSConnected = ROSState == EROSState::Connected ? true : false;
 
#if WITH_EDITOR
    UE_LOG(LogTemp, Warning, TEXT("ROSHandler.cpp: ROS bridge state changed. ROS is connected: %s"), (ROSConnected ? TEXT("true") : TEXT("false")));
#endif
}