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