agentic-mobile-manipulator/sim/Gem/Source/MobileManipulatorDemoSystemComponent.cpp at e8aa697f6f6e34a772e0a427334a94ef41938ad9 · RobotecAI/agentic-mobile-manipulator · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113

#include <AzCore/Serialization/SerializeContext.h>

#include "MobileManipulatorDemoSystemComponent.h"

#include <MobileManipulatorDemo/MobileManipulatorDemoTypeIds.h>

#include <ROS2/ROS2Bus.h>
#include "SpawnEntityServiceHandler.h"
#include "SpawnEntitiesServiceHandler.h"

namespace MobileManipulatorDemo
{
    AZ_COMPONENT_IMPL(MobileManipulatorDemoSystemComponent, "MobileManipulatorDemoSystemComponent",
        MobileManipulatorDemoSystemComponentTypeId);

    void MobileManipulatorDemoSystemComponent::Reflect(AZ::ReflectContext* context)
    {
        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
        {
            serializeContext->Class<MobileManipulatorDemoSystemComponent, AZ::Component>()
                ->Version(0)
                ;
        }
    }

    void MobileManipulatorDemoSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
    {
        provided.push_back(AZ_CRC_CE("MobileManipulatorDemoService"));
    }

    void MobileManipulatorDemoSystemComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
    {
        incompatible.push_back(AZ_CRC_CE("MobileManipulatorDemoService"));
    }

    void MobileManipulatorDemoSystemComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required)
    {
        required.push_back(AZ_CRC_CE("ROS2Service"));
    }

    void MobileManipulatorDemoSystemComponent::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent)
    {
    }

    MobileManipulatorDemoSystemComponent::MobileManipulatorDemoSystemComponent()
        : m_nodeHandler([this] (std::shared_ptr<rclcpp::Node> node)
            {
                if (node)
                {
                    TryRegisterSpawnServiceHandler();
                }
                else
                {
                    DestroyHandlers();
                }
            })
    {
        if (MobileManipulatorDemoInterface::Get() == nullptr)
        {
            MobileManipulatorDemoInterface::Register(this);
        }
    }

    MobileManipulatorDemoSystemComponent::~MobileManipulatorDemoSystemComponent()
    {
        if (MobileManipulatorDemoInterface::Get() == this)
        {
            MobileManipulatorDemoInterface::Unregister(this);
        }
    }

    void MobileManipulatorDemoSystemComponent::Init()
    {
    }

    void MobileManipulatorDemoSystemComponent::TryRegisterSpawnServiceHandler()
    {
        auto ros2Node = ROS2::ROS2Interface::Get()->GetNode();
        if (!ros2Node)
        {
            return;
        }

        RegisterInterface<SpawnEntityServiceHandler>(ros2Node);
        RegisterInterface<SpawnEntitiesServiceHandler>(ros2Node);
    }

    void MobileManipulatorDemoSystemComponent::DestroyHandlers()
    {
        for (auto& [handlerType, handler] : m_availableRos2Interface)
        {
            handler.reset();
        }
        m_availableRos2Interface.clear();
    }

    void MobileManipulatorDemoSystemComponent::Activate()
    {
        MobileManipulatorDemoRequestBus::Handler::BusConnect();

        ROS2::ROS2Interface::Get()->ConnectOnNodeChanged(m_nodeHandler);
        TryRegisterSpawnServiceHandler();
    }

    void MobileManipulatorDemoSystemComponent::Deactivate()
    {
        MobileManipulatorDemoRequestBus::Handler::BusDisconnect();

        m_nodeHandler.Disconnect();
        DestroyHandlers();
    }
}