Component.h
Go to the documentation of this file.
1 /**
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package Navigation::ArmarXObjects::Navigator
17  * @author Fabian Reister ( fabian dot reister at kit dot edu )
18  * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
19  * @date 2021
20  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
21  * GNU General Public License
22  */
23 
24 
25 #pragma once
26 
27 
28 #include <optional>
29 #include <string>
30 #include <vector>
31 
32 #include <Eigen/Core>
33 
34 #include <Ice/Object.h>
35 
40 
41 #include <ArmarXGui/interface/RemoteGuiInterface.h>
43 
52 
68 
70 {
71 
72  /**
73  * @defgroup Component-Navigator Navigator
74  * @ingroup armarx_navigation-Components
75  * A description of the component Navigator.
76  *
77  * @class Navigator
78  * @ingroup Component-Navigator
79  * @brief Brief description of class Navigator.
80  *
81  * Detailed description of class Navigator.
82  */
83  class Component :
84  virtual public armarx::Component,
85  virtual public client::NavigatorInterface,
86  virtual public ObjectPoseClientPluginUser,
87  virtual public ArVizComponentPluginUser,
90  // virtual public armem::ListeningClientPluginUser
91  {
92 
93  public:
94  Component();
95  ~Component() override;
96 
97  /// @see armarx::ManagedIceObject::getDefaultName()
98  std::string getDefaultName() const override;
99 
100  static std::string GetDefaultName();
101 
102  void createConfig(const aron::data::dto::DictPtr& stackConfig,
103  const std::string& callerId,
104  const Ice::Current& c = Ice::emptyCurrent) override;
105 
106  void moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
107  const std::string& navigationMode,
108  const std::string& callerId,
109  const Ice::Current& c = Ice::emptyCurrent) override;
110 
111  void moveTo2(const client::detail::Waypoints& waypoints,
112  const std::string& navigationMode,
113  const std::string& callerId,
114  const Ice::Current& c = Ice::emptyCurrent) override;
115 
116  void moveToLocation(const std::string& location,
117  const std::string& callerId,
118  const Ice::Current& c = Ice::emptyCurrent) override;
119 
120  void updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
121  const std::string& navigationMode,
122  const std::string& callerId,
123  const Ice::Current& c = Ice::emptyCurrent) override;
124 
125  void moveTowards(const Eigen::Vector3f& direction,
126  const std::string& navigationMode,
127  const std::string& callerId,
128  const Ice::Current& c = Ice::emptyCurrent) override;
129 
130  void pause(const std::string& callerId, const Ice::Current& c = Ice::emptyCurrent) override;
131 
132  void resume(const std::string& callerId,
133  const Ice::Current& c = Ice::emptyCurrent) override;
134 
135  void stop(const std::string& callerId, const Ice::Current& c = Ice::emptyCurrent) override;
136 
137  void stopAll(const Ice::Current& c = Ice::emptyCurrent) override;
138 
139  bool isPaused(const std::string& callerId,
140  const Ice::Current& c = Ice::emptyCurrent) override;
141 
142  bool isStopped(const std::string& callerId,
143  const Ice::Current& c = Ice::emptyCurrent) override;
144 
145  const core::Scene& scene() const;
146 
147  protected:
148  /// @see PropertyUser::createPropertyDefinitions()
150 
151  /// @see armarx::ManagedIceObject::onInitComponent()
152  void onInitComponent() override;
153 
154  /// @see armarx::ManagedIceObject::onConnectComponent()
155  void onConnectComponent() override;
156 
157  /// @see armarx::ManagedIceObject::onDisconnectComponent()
158  void onDisconnectComponent() override;
159 
160  /// @see armarx::ManagedIceObject::onExitComponent()
161  void onExitComponent() override;
162 
163  void onReconnectComponent();
164  void initializeScene();
165 
166  // core::StaticScene staticScene();
167 
168  // [[nodiscard]] VirtualRobot::RobotPtr getRobot();
169  // void updateRobot();
170 
172  std::optional<std::string> activeNavigatorId() const;
173 
174  private:
175  void visualizeSPFA();
176 
177  RemoteGuiInterfacePrx remoteGui;
178 
179  // core::Scene scene;
180 
181  std::optional<server::PlatformControllerExecutor> executor;
182  std::optional<server::ArvizIntrospector> introspector;
183  // std::optional<server::MemoryIntrospector> memoryIntrospector;
184  std::optional<server::scene_provider::SceneProvider> sceneProvider;
185 
186  std::unordered_map<std::string, server::Navigator> navigators;
187 
188  std::mutex propertiesMutex;
189 
190  std::optional<armarx::navigation::components::navigator::RemoteGui> navRemoteGui;
191 
192  // unique_ptr to avoid dangling refs
193  std::vector<std::unique_ptr<server::MemoryIntrospector>> memoryIntrospectors;
194 
195  // `navigation` memory reader and writer
197  parameterizationReaderPlugin = nullptr;
199  parameterizationWriterPlugin = nullptr;
201  eventsWriterPlugin = nullptr;
203  resultsWriterPlugin = nullptr;
205  graphReaderPlugin = nullptr;
207  costmapReaderPlugin = nullptr;
209  humanReaderPlugin = nullptr;
211  laserScannerFeaturesReaderPlugin = nullptr;
212 
213  // armem::vision::occupancy_grid::client::Reader occupancyGridReader;
214 
215  // `robot_state` memory reader and writer
216  std::optional<armem::robot::RobotDescription> robotDescription;
218  virtualRobotReaderPlugin = nullptr;
219 
220 
221  server::MemoryParameterizationService parameterizationService;
222  // server::MemoryPublisher publisher;
223 
224  // key is `callerId`
225  std::unordered_map<std::string, std::unique_ptr<server::MemoryPublisher>> memoryPublishers;
226 
227  struct Parameters
228  {
229  float occupiedGridThreshold{0.55F};
230  bool disableExecutor{false};
231 
233 
234  bool rtUnitDynamicallyLoadLibraries = false;
235  };
236 
237  Parameters params;
238  };
239 } // namespace armarx::navigation::components::navigator
armarx::navigation::components::navigator::Component::activeNavigator
server::Navigator * activeNavigator()
Definition: Component.cpp:573
ArVizComponentPlugin.h
armarx::navigation::components::navigator::Component::isStopped
bool isStopped(const std::string &callerId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Component.cpp:477
armarx::navigation::components::navigator::Component::activeNavigatorId
std::optional< std::string > activeNavigatorId() const
Definition: Component.cpp:586
armarx::navigation::components::navigator::Component::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: Component.cpp:486
ObjectPoseClientPlugin.h
armarx::navigation::components::navigator::Component::pause
void pause(const std::string &callerId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Component.cpp:409
armarx::armem::client::plugins::ReaderWriterPlugin
A component plugin offering client-side access to a reader or writer and manages the lifecycle,...
Definition: ReaderWriterPlugin.h:44
armarx::navigation::client::NavigatorInterface::stopAll
idempotent void stopAll()
PeriodicTask.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::navigation::components::navigator::Component::stop
void stop(const std::string &callerId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Component.cpp:426
armarx::navigation::server::Navigator
Definition: Navigator.h:62
Reader.h
ReaderWriterPlugin.h
armarx::ObjectPoseClientPluginUser
Provides an objpose::ObjectPoseTopicPrx objectPoseTopic as member variable.
Definition: ObjectPoseClientPlugin.h:67
MemoryParameterizationService.h
armarx::navigation::components::navigator::Component::updateMoveTo
void updateMoveTo(const std::vector< Eigen::Matrix4f > &waypoints, const std::string &navigationMode, const std::string &callerId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Component.cpp:328
armarx::navigation::components::navigator::Component::scene
const core::Scene & scene() const
Definition: Component.cpp:470
armarx::navigation::components::navigator::Component::moveToLocation
void moveToLocation(const std::string &location, const std::string &callerId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Component.cpp:377
Reader.h
armarx::navigation::components::navigator::Component
Definition: Component.h:83
armarx::navigation::components::navigator::Component::onDisconnectComponent
void onDisconnectComponent() override
Definition: Component.cpp:213
LightweightRemoteGuiComponentPlugin.h
armarx::control::client::ComponentPluginUser
Definition: ComponentPlugin.h:147
armarx::navigation::components::navigator::Component::resume
void resume(const std::string &callerId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Component.cpp:417
Reader.h
RemoteGui.h
armarx::navigation::server::scene_provider::SceneProvider::Config
Definition: SceneProvider.h:86
armarx::ArVizComponentPluginUser
Provides a ready-to-use ArViz client arviz as member variable.
Definition: ArVizComponentPlugin.h:36
armarx::navigation::client::detail::Waypoints
sequence< Waypoint > Waypoints
Definition: NavigatorInterface.ice:58
armarx::navigation::components::navigator::Component::moveTo2
void moveTo2(const client::detail::Waypoints &waypoints, const std::string &navigationMode, const std::string &callerId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Component.cpp:359
DebugObserverComponentPlugin.h
VirtualRobotReader.h
armarx::navigation::components::navigator::Component::moveTowards
void moveTowards(const Eigen::Vector3f &direction, const std::string &navigationMode, const std::string &callerId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Component.cpp:392
PlatformControllerExecutor.h
plugins.h
armarx::navigation::client::NavigatorInterface
Definition: NavigatorInterface.ice:62
armarx::navigation::components::navigator::Component::onReconnectComponent
void onReconnectComponent()
Definition: Component.cpp:206
armarx::navigation::core::Scene
Definition: types.h:71
Reader.h
MemoryIntrospector.h
Component.h
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:95
MemoryPublisher.h
armarx::navigation::components::navigator::Component::moveTo
void moveTo(const std::vector< Eigen::Matrix4f > &waypoints, const std::string &navigationMode, const std::string &callerId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Component.cpp:298
armarx::aron::data::DictPtr
std::shared_ptr< Dict > DictPtr
Definition: Dict.h:41
TripleBuffer.h
armarx::navigation::components::navigator::Component::onConnectComponent
void onConnectComponent() override
Definition: Component.cpp:146
IceUtil::Handle< class PropertyDefinitionContainer >
types.h
armarx::DebugObserverComponentPluginUser
Definition: DebugObserverComponentPlugin.h:82
armarx::navigation::components::navigator::Component::Component
Component()
Definition: Component.cpp:117
Writer.h
ComponentPlugin.h
armarx::navigation::components::navigator::Component::GetDefaultName
static std::string GetDefaultName()
Definition: Component.cpp:240
armarx::navigation::components::navigator::Component::~Component
~Component() override
armarx::navigation::components::navigator::Component::initializeScene
void initializeScene()
Definition: Component.cpp:227
armarx::navigation::components::navigator::Component::isPaused
bool isPaused(const std::string &callerId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Component.cpp:461
armarx::navigation::components::navigator::Component::getDefaultName
std::string getDefaultName() const override
Definition: Component.cpp:234
armarx::navigation::components::navigator::Component::createConfig
void createConfig(const aron::data::dto::DictPtr &stackConfig, const std::string &callerId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: Component.cpp:246
types.h
Navigator.h
armarx::navigation::server::MemoryParameterizationService
Definition: MemoryParameterizationService.h:36
Reader.h
armarx::navigation::components::navigator
This file is part of ArmarX.
Definition: Component.cpp:92
RobotUnitComponentPlugin.h
armarx::navigation::components::navigator::Component::onInitComponent
void onInitComponent() override
Definition: Component.cpp:141
armarx::navigation::components::navigator::Component::onExitComponent
void onExitComponent() override
Definition: Component.cpp:222
ArvizIntrospector.h
SceneProvider.h