RemoteGui.cpp
Go to the documentation of this file.
1 #include "RemoteGui.h"
2 
3 #include <cmath>
4 #include <mutex>
5 
6 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
7 #include <ArmarXCore/core/time/CycleUtil.h>
8 #include <ArmarXCore/statechart/ParameterMapping.h>
9 #include <ArmarXCore/util/CPPUtility/trace.h>
10 
11 #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
12 #include <ArmarXGui/libraries/RemoteGui/WidgetBuilder/IntegerWidgets.h>
13 #include <ArmarXGui/libraries/RemoteGui/WidgetBuilder/LayoutWidgets.h>
14 #include <ArmarXGui/libraries/RemoteGui/WidgetBuilder/StaticWidgets.h>
15 #include <ArmarXGui/interface/RemoteGuiInterface.h>
16 #include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
17 
18 #include <armarx/navigation/client/NavigationStackConfig.h>
19 #include <armarx/navigation/core/types.h>
20 #include <armarx/navigation/factories/NavigationStackFactory.h>
21 #include <armarx/navigation/global_planning/AStar.h>
22 #include <armarx/navigation/global_planning/Point2Point.h>
23 #include <armarx/navigation/global_planning/SPFA.h>
24 #include <armarx/navigation/local_planning/TimedElasticBandsParams.h>
25 #include <armarx/navigation/server/Navigator.h>
26 #include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
27 #include <armarx/navigation/util/util.h>
28 
29 #include "Component.h"
30 
31 namespace armarx::navigation::components::navigator
32 {
33  namespace gui = RemoteGui;
34 
35  RemoteGui::RemoteGui(const RemoteGuiInterfacePrx& remoteGui, Component& navigator) :
36  remoteGui(remoteGui),
37  runningTask(new RunningTask<RemoteGui>(this, &RemoteGui::run)),
38  navigator(navigator)
39  {
40  createRemoteGuiTab();
41 
42  tabPrx = gui::TabProxy(remoteGui, REMOTE_GUI_TAB_MAME);
43 
44  runningTask->start();
45  }
46 
47  RemoteGui::~RemoteGui()
48  {
49  shutdown();
50  }
51 
52  void
53  RemoteGui::createRemoteGuiTab()
54  {
55  ARMARX_TRACE;
56 
57  ARMARX_CHECK_NOT_NULL(remoteGui); // createRemoteGui() has to be called first
58 
59  ARMARX_TRACE;
60  using namespace armarx::RemoteGui::Client;
61 
62  // Setup the widgets.
63 
64  // every widget needs a unique id.
65  int id = 0;
66 
67  {
68  tab.targetPoseGroup.group.setLabel("Target pose");
69 
70  {
71  float max = 10'000;
72  tab.targetPoseGroup.targetPoseX.setRange(-max, max);
73  tab.targetPoseGroup.targetPoseX.setDecimals(0);
74  tab.targetPoseGroup.targetPoseX.setSteps(int(max / 10));
75  tab.targetPoseGroup.targetPoseX.setValue(0.F);
76  }
77 
78  {
79  float max = 10'000;
80  tab.targetPoseGroup.targetPoseY.setRange(-max, max);
81  tab.targetPoseGroup.targetPoseY.setDecimals(0);
82  tab.targetPoseGroup.targetPoseY.setSteps(int(max / 10));
83  tab.targetPoseGroup.targetPoseY.setValue(0.F);
84  }
85 
86  {
87  float max = 10'000;
88  tab.targetPoseGroup.targetPoseZ.setRange(-max, max);
89  tab.targetPoseGroup.targetPoseZ.setDecimals(0);
90  tab.targetPoseGroup.targetPoseZ.setSteps(int(max / 10));
91  tab.targetPoseGroup.targetPoseZ.setValue(0.F);
92  }
93 
94  {
95  float max = 180;
96  tab.targetPoseGroup.targetPoseRoll.setRange(-max, max);
97  tab.targetPoseGroup.targetPoseRoll.setDecimals(0);
98  tab.targetPoseGroup.targetPoseRoll.setSteps(2 * max);
99  tab.targetPoseGroup.targetPoseRoll.setValue(0.F);
100  }
101 
102  {
103  float max = 180;
104  tab.targetPoseGroup.targetPosePitch.setRange(-max, max);
105  tab.targetPoseGroup.targetPosePitch.setDecimals(0);
106  tab.targetPoseGroup.targetPosePitch.setSteps(2 * max);
107  tab.targetPoseGroup.targetPosePitch.setValue(0.F);
108  }
109 
110  {
111  float max = 180;
112  tab.targetPoseGroup.targetPoseYaw.setRange(-max, max);
113  tab.targetPoseGroup.targetPoseYaw.setDecimals(0);
114  tab.targetPoseGroup.targetPoseYaw.setSteps(2 * max);
115  tab.targetPoseGroup.targetPoseYaw.setValue(0.F);
116  }
117 
118  // Setup the layout.
119 
120  GridLayout grid;
121  {
122 
123  grid.add(Label("X"), {id, 0}).add(tab.targetPoseGroup.targetPoseX, {id, 1});
124  ++id;
125 
126  grid.add(Label("Y"), {id, 0}).add(tab.targetPoseGroup.targetPoseY, {id, 1});
127  ++id;
128 
129  grid.add(Label("Z"), {id, 0}).add(tab.targetPoseGroup.targetPoseZ, {id, 1});
130  ++id;
131 
132  grid.add(armarx::RemoteGui::Client::VSpacer(), {id, 1});
133  ++id;
134 
135  grid.add(Label("Roll"), {id, 0}).add(tab.targetPoseGroup.targetPoseRoll, {id, 1});
136  ++id;
137 
138  grid.add(Label("Pitch"), {id, 0}).add(tab.targetPoseGroup.targetPosePitch, {id, 1});
139  ++id;
140 
141  grid.add(Label("Yaw"), {id, 0}).add(tab.targetPoseGroup.targetPoseYaw, {id, 1});
142  ++id;
143  }
144 
145  VBoxLayout root = {grid, VSpacer()};
146 
147  tab.targetPoseGroup.group.addChild(root);
148  }
149 
150  {
151  tab.controlGroup.group.setLabel("Control");
152 
153  tab.controlGroup.moveToButton.setLabel("MoveTo");
154  tab.controlGroup.continueButton.setLabel("Continue");
155  tab.controlGroup.pauseButton.setLabel("Pause");
156  tab.controlGroup.stopButton.setLabel("Stop");
157  tab.controlGroup.stopAllButton.setLabel("StopAll");
158 
159  GridLayout grid;
160  {
161  grid.add(tab.controlGroup.moveToButton, {id, 1});
162  ++id;
163 
164  grid.add(armarx::RemoteGui::Client::VSpacer(), {id, 1});
165  ++id;
166 
167  grid.add(tab.controlGroup.pauseButton, {id, 1});
168  ++id;
169 
170  grid.add(tab.controlGroup.continueButton, {id, 1});
171  ++id;
172 
173  grid.add(tab.controlGroup.stopButton, {id, 1});
174  ++id;
175 
176  grid.add(armarx::RemoteGui::Client::VSpacer(), {id, 1});
177  ++id;
178 
179  grid.add(tab.controlGroup.stopAllButton, {id, 1});
180  ++id;
181  }
182 
183  VBoxLayout root = {grid, VSpacer()};
184  tab.controlGroup.group.addChild(root);
185  }
186 
187  ARMARX_TRACE;
188 
189  tab.hbox.addChild(tab.targetPoseGroup.group);
190  tab.hbox.addChild(tab.controlGroup.group);
191 
192  tab.create(REMOTE_GUI_TAB_MAME, remoteGui, tab.hbox);
193  }
194 
195  void
196  RemoteGui::run()
197  {
198  constexpr int kCycleDurationMs = 100;
199 
200  CycleUtil c(kCycleDurationMs);
201  while (!runningTask->isStopped())
202  {
203  {
204 
205  {
206  std::lock_guard g{mtx};
207 
208  ARMARX_TRACE;
209  tab.receiveUpdates();
210  tabPrx.receiveUpdates();
211  }
212 
213  ARMARX_TRACE;
214  handleEvents(tabPrx);
215 
216  {
217  std::lock_guard g{mtx};
218 
219  ARMARX_TRACE;
220  tab.sendUpdates();
221  tabPrx.sendUpdates();
222  }
223  }
224 
225  c.waitForCycleDuration();
226  }
227  }
228 
229  void
230  RemoteGui::handleEvents(::armarx::RemoteGui::TabProxy& tab3)
231  {
232  ARMARX_TRACE;
233 
234  const std::string configId = "RemoteGui";
235 
236  if (tab.controlGroup.moveToButton.wasClicked())
237  {
238  ARMARX_INFO << "MoveTo from RemoteGUI requested";
239 
240  // const auto& scene = navigator.getScene();
241 
242  // server::NavigationStack stack
243  // {
244  // .globalPlanner =
245  // std::make_shared<global_planning::Point2Point>(global_planning::Point2PointParams(), scene),
246  // .trajectoryControl = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
247  // traj_ctrl::TrajectoryFollowingControllerParams(), scene)};
248 
249  // server::NavigationStack stack
250  // {
251  // .globalPlanner =
252  // std::make_shared<global_planning::AStar>(global_planning::AStarParams(), scene),
253  // .trajectoryControl = std::make_shared<traj_ctrl::TrajectoryFollowingController>(
254  // traj_ctrl::TrajectoryFollowingControllerParams(), scene)};
255 
256  client::NavigationStackConfig cfg;
257  cfg.globalPlanner(global_planning::SPFAParams());
258  cfg.localPlanner(local_planning::TimedElasticBandsParams());
259  cfg.trajectoryController(traj_ctrl::local::TrajectoryFollowingControllerParams());
260 
261  const auto stackConfig = cfg.toAron();
262 
263  ARMARX_TRACE;
264 
265  navigator.createConfig(stackConfig, "RemoteGui");
266 
267  const Eigen::Vector3f targetPos{tab.targetPoseGroup.targetPoseX.getValue(),
268  tab.targetPoseGroup.targetPoseY.getValue(),
269  tab.targetPoseGroup.targetPoseZ.getValue()};
270 
271  const auto degToRad = [](float deg) -> float { return deg * M_PIf32 / 180.f; };
272 
273  const Eigen::Vector3f targetOri{
274  degToRad(tab.targetPoseGroup.targetPoseRoll.getValue()),
275  degToRad(tab.targetPoseGroup.targetPosePitch.getValue()),
276  degToRad(tab.targetPoseGroup.targetPoseYaw.getValue())};
277 
278  const core::Pose targetPose = core::Pose(Eigen::Translation3f(targetPos)) *
279  core::Pose(simox::math::rpy_to_mat3f(targetOri));
280 
281  std::vector<Eigen::Matrix4f> waypoints;
282  waypoints.emplace_back(targetPose.matrix());
283 
284  ARMARX_TRACE;
285 
286  try
287  {
288  navigator.moveTo(
289  waypoints,
290  core::NavigationFrameNames.to_name(core::NavigationFrame::Absolute),
291  configId);
292  }
293  catch (const armarx::LocalException& ex)
294  {
295  ARMARX_WARNING << LogSender::eYellow
296  << "Failed moving to target. Reason: " << GetHandledExceptionString()
297  << LogSender::eReset;
298  }
299  };
300 
301  if (tab.controlGroup.pauseButton.wasClicked())
302  {
303  ARMARX_INFO << "pauseMovement() from RemoteGUI requested";
304  navigator.pause(configId);
305  }
306 
307  if (tab.controlGroup.continueButton.wasClicked())
308  {
309  ARMARX_INFO << "resumeMovement() from RemoteGUI requested";
310  navigator.resume(configId);
311  }
312 
313  if (tab.controlGroup.stopButton.wasClicked())
314  {
315  ARMARX_INFO << "stop() from RemoteGUI requested";
316  navigator.stop(configId);
317  }
318 
319  if (tab.controlGroup.stopAllButton.wasClicked())
320  {
321  ARMARX_INFO << "stop() from RemoteGUI requested";
322  navigator.stopAll();
323  }
324  }
325 
326  void
327  RemoteGui::shutdown()
328  {
329  // std::lock_guard g{mtx};
330 
331  if (!runningTask->isStopped())
332  {
333  runningTask->stop();
334  }
335 
336  ARMARX_DEBUG << "Removing tab: " << REMOTE_GUI_TAB_MAME;
337  remoteGui->removeTab(REMOTE_GUI_TAB_MAME);
338  }
339 
340  void
341  RemoteGui::enable()
342  {
343  ARMARX_TRACE;
344 
345  // std::lock_guard g{mtx};
346  tabPrx.internalSetDisabled(tab.controlGroup.moveToButton.getName(), false);
347  tabPrx.internalSetDisabled(tab.controlGroup.pauseButton.getName(), false);
348  tabPrx.internalSetDisabled(tab.controlGroup.continueButton.getName(), false);
349  tabPrx.internalSetDisabled(tab.controlGroup.stopButton.getName(), false);
350  tabPrx.internalSetDisabled(tab.controlGroup.stopAllButton.getName(), false);
351  }
352 
353  void
354  RemoteGui::disable()
355  {
356  ARMARX_TRACE;
357 
358  // std::unique_lock g{mtx};
359 
360  // it might happen, that this function is triggered from handleEvents
361  // std::unique_lock ul{handleEventsMtx};
362  // ul.try_lock();
363 
364  tabPrx.internalSetDisabled(tab.controlGroup.moveToButton.getName(), true);
365  tabPrx.internalSetDisabled(tab.controlGroup.pauseButton.getName(), true);
366  tabPrx.internalSetDisabled(tab.controlGroup.continueButton.getName(), true);
367  tabPrx.internalSetDisabled(tab.controlGroup.stopButton.getName(), true);
368  tabPrx.internalSetDisabled(tab.controlGroup.stopAllButton.getName(), true);
369  }
370 
371  void
372  RemoteGui::reset()
373  {
374  ARMARX_TRACE;
375 
376  // std::lock_guard g{mtx};
377 
378  tab.targetPoseGroup.targetPoseX.setValue(0.F);
379  tab.targetPoseGroup.targetPoseY.setValue(0.F);
380  tab.targetPoseGroup.targetPoseZ.setValue(0.F);
381  tab.targetPoseGroup.targetPoseRoll.setValue(0.F);
382  tab.targetPoseGroup.targetPosePitch.setValue(0.F);
383  tab.targetPoseGroup.targetPoseYaw.setValue(0.F);
384 
385  tab.sendUpdates();
386  tab.sendUpdates();
387  }
388 
389 } // namespace armarx::navigation::components::navigator
SPFA.h
armarx::navigation::components::navigator::RemoteGui::~RemoteGui
~RemoteGui()
Definition: RemoteGui.cpp:47
StaticWidgets.h
armarx::navigation::components::navigator::RemoteGui::shutdown
void shutdown()
Definition: RemoteGui.cpp:327
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
trace.h
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:267
TimedElasticBandsParams.h
ParameterMapping.h
IntegerWidgets.h
NavigationStackFactory.h
armarx::RunningTask
Definition: ArmarXMultipleObjectsScheduler.h:35
armarx::navigation::components::navigator::Component
Definition: Component.h:83
Point2Point.h
armarx::RemoteGui::TabProxy
Definition: WidgetProxy.h:17
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
RemoteGui.h
util.h
NavigationStackConfig.h
LayoutWidgets.h
TrajectoryFollowingController.h
armarx::navigation::components::navigator::RemoteGui::RemoteGui
RemoteGui(const RemoteGuiInterfacePrx &remoteGui, Component &navigator)
Definition: RemoteGui.cpp:35
WidgetProxy.h
armarx::navigation::components::navigator::RemoteGui
Definition: RemoteGui.h:40
Component.h
CycleUtil.h
ExpressionException.h
F
Definition: ExportDialogControllerTest.cpp:16
Widgets.h
armarx::RemoteGui::Client
Definition: EigenWidgets.cpp:8
types.h
Navigator.h
armarx::navigation::components::navigator
This file is part of ArmarX.
Definition: Component.cpp:92
AStar.h