NavigateRelativeSkill.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <future>
4 
6 
11 
13 {
14 
15  template <typename R>
16  bool
17  is_ready(std::future<R> const& f)
18  {
19  if (!f.valid())
20  {
21  return true;
22  }
23  std::future_status status = f.wait_for(std::chrono::seconds(0));
24  if (status == std::future_status::ready)
25  {
26  return true;
27  }
28  else if (status == std::future_status::deferred)
29  {
30  // if the task has not been started --> start the task
31  f.wait();
32  return true;
33  }
34  else
35  {
36  return false;
37  }
38  }
39 
40  // abstract base class of a relative movement skill
41  template <class AronT>
42  class NavigateRelativeSkill : public armarx::skills::SimpleSpecializedSkill<AronT>
43  {
44 
45  public:
47 
48  struct Services
49  {
52  };
53 
54  NavigateRelativeSkill() = delete;
55 
56  NavigateRelativeSkill(const Services& srv, const armarx::skills::SkillDescription& desc) :
57  Base(desc)
58  {
59  srv_.emplace(srv);
60  }
61 
62  virtual ~NavigateRelativeSkill() = default;
63 
64  private:
65  // implemented by children returning the relative movement to execute
66  virtual Eigen::Isometry3f relativeTarget(const typename Base::SpecializedMainInput& in) = 0;
67 
69  init(const typename Base::SpecializedInitInput& in) override
70  {
71  // parameterize the navigation stack
77 
78  const std::string configId = this->getSkillId().skillName;
79 
80  // configure the `navigator` which provides a simplified and typed interface to the navigation server
81  memorySubscriber.emplace(configId, srv_->memoryNameSystem);
82 
83  // register our config
84  ARMARX_INFO << "Registering config";
85  srv_->iceNavigator.createConfig(cfg, configId);
86 
87  navigator.emplace(client::Navigator::InjectedServices{
88  .navigator = &srv_->iceNavigator, .subscriber = &memorySubscriber.value()});
89 
92  }
93 
95  main(const typename Base::SpecializedMainInput& in) override
96  {
97  const Eigen::Isometry3f relativeTarget = this->relativeTarget(in);
98 
99  ARMARX_INFO << "Moving to relative target " << VAROUT(relativeTarget.matrix());
100 
101  // excecute
102  ARMARX_INFO << "Sending navigation request";
103  navigator->moveTo(relativeTarget, core::NavigationFrame::Relative);
104 
105  // Wait until goal is reached
106  ARMARX_INFO << "Waiting until goal is reached.";
107 
108  auto future =
109  std::async(std::launch::async, [this]() { return navigator->waitForStop(); });
110  while (not is_ready(future))
111  {
113  {
114  ARMARX_INFO << "Skill was aborted by user " << Base::stopped << ", "
116  navigator->stop();
117  break;
118  }
119  }
120 
121  auto se = future.get();
122  if (se)
123  {
124  ARMARX_INFO << "Goal `" << relativeTarget.matrix() << "`reached.";
125  }
126  else
127  {
128  if (se.isSafetyStopTriggeredEvent())
129  {
130  ARMARX_ERROR << "Safety stop was triggered!";
131 
134  }
135  if (se.isUserAbortTriggeredEvent())
136  {
137  ARMARX_ERROR << "Aborted by user!";
138 
141  }
142  if (se.isInternalErrorEvent())
143  {
144  ARMARX_ERROR << "Unknown internal error occured! "
145  << se.toInternalErrorEvent().message;
146 
149  }
150  }
151 
154  }
155 
156  void
157  onStopRequested() override
158  {
159  if (navigator.has_value())
160  {
161  ARMARX_CHECK(navigator.has_value());
162  navigator->stop();
163  }
164  }
165 
166 
167  private:
168  std::optional<Services> srv_;
169 
170  // will be initialized in "init()"
171  std::optional<client::MemorySubscriber> memorySubscriber;
172  std::optional<client::Navigator> navigator;
173  };
174 
175 } // namespace armarx::navigation::skills
armarx::skills::TerminatedSkillStatus::Succeeded
@ Succeeded
armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams
Definition: TrajectoryFollowingController.h:35
armarx::skills::SkillDescription
Definition: SkillDescription.h:18
armarx::navigation::skills::NavigateRelativeSkill::NavigateRelativeSkill
NavigateRelativeSkill(const Services &srv, const armarx::skills::SkillDescription &desc)
Definition: NavigateRelativeSkill.h:56
armarx::navigation::client::NavigationStackConfig
Definition: NavigationStackConfig.h:55
armarx::navigation::client::IceNavigator
Definition: IceNavigator.h:17
armarx::navigation::client::NavigationStackConfig::globalPlanner
NavigationStackConfig & globalPlanner(const global_planning::GlobalPlannerParams &params)
Definition: NavigationStackConfig.cpp:63
armarx::skills::Skill::timeoutReached
std::atomic_bool timeoutReached
Definition: Skill.h:348
armarx::navigation::skills::NavigateRelativeSkill::NavigateRelativeSkill
NavigateRelativeSkill()=delete
SimpleSpecializedSkill.h
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::skills::Skill::stopped
std::atomic_bool stopped
Definition: Skill.h:347
armarx::navigation::skills::NavigateRelativeSkill::~NavigateRelativeSkill
virtual ~NavigateRelativeSkill()=default
Point2Point.h
MemorySubscriber.h
armarx::status
status
Definition: FiniteStateMachine.h:259
TrajectoryFollowingController.h
armarx::skills::SimpleSpecializedSkill
Definition: SimpleSpecializedSkill.h:10
client.h
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::navigation::skills::NavigateRelativeSkill::Services
Definition: NavigateRelativeSkill.h:48
armarx::skills::Skill::MainResult
A result struct for th main method of a skill.
Definition: Skill.h:48
armarx::skills::Skill::MainResult::status
TerminatedSkillStatus status
Definition: Skill.h:50
armarx::skills::TerminatedSkillStatus::Failed
@ Failed
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::navigation::global_planning::Point2PointParams
Parameters for Point2Point.
Definition: Point2Point.h:35
armarx::navigation::core::NavigationFrame::Relative
@ Relative
armarx::skills::Skill::InitResult::status
TerminatedSkillStatus status
Definition: Skill.h:38
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::skills::Skill::shouldSkillTerminate
bool shouldSkillTerminate() const
Returns whether the skill should terminate as soon as possible.
Definition: Skill.cpp:350
armarx::skills::Skill::InitResult
A result struct for skill initialization.
Definition: Skill.h:36
armarx::navigation::skills::is_ready
bool is_ready(std::future< R > const &f)
Definition: NavigateRelativeSkill.h:17
armarx::armem::client::MemoryNameSystem
The memory name system (MNS) client.
Definition: MemoryNameSystem.h:69
armarx::navigation::skills::NavigateRelativeSkill::Services::iceNavigator
client::IceNavigator & iceNavigator
Definition: NavigateRelativeSkill.h:50
armarx::navigation::client::GeneralConfig
Definition: NavigationStackConfig.h:42
armarx::navigation::skills
Definition: constants.cpp:25
armarx::navigation::skills::NavigateRelativeSkill::Services::memoryNameSystem
armem::client::MemoryNameSystem & memoryNameSystem
Definition: NavigateRelativeSkill.h:51
armarx::navigation::client::NavigationStackConfig::trajectoryController
NavigationStackConfig & trajectoryController(const traj_ctrl::local::TrajectoryControllerParams &params)
Definition: NavigationStackConfig.cpp:91
armarx::navigation::client::NavigationStackConfig::general
NavigationStackConfig & general(const GeneralConfig &cfg)
Definition: NavigationStackConfig.cpp:56