ControllerHandler.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  * @author VS_CODE_USER_NAME ( VS_CODE_USER_EMAIL )
17  * @date 2023
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #pragma once
23 
24 #include <filesystem>
25 #include <memory>
26 
29 
34 #include <armarx/view_selection/gaze_targets/aron/GazeTarget.aron.generated.h>
36 
38 
40 {
42  {
45  };
46 
48  {
49  std::string robotName;
50  };
51 
52  template <armarx::view_selection::common::ControllerType ControllerT>
54  {
55  public:
58 
59  ControllerHandler(const InjectedServices& srv, const Params& params) :
60  srv(srv), params(params)
61  {
62  }
63 
64  ~ControllerHandler() override
65  {
66  // ctrl.reset();
67  }
68 
69  void
70  activateController() override
71  {
72  //Properties are loaded OK
73  ARMARX_INFO << "Activating controller";
74  ctrl->activate();
75  }
76 
77  void
78  createController() override
79  {
80  // initialize controller
81  ARMARX_INFO << "Initializing controller";
82  {
84  auto builder = srv.controlComponentPlugin->createControllerBuilder<ControllerT>();
85  builder.withNodeSet("Head");
86 
87  const std::string controllerTypeID =
89 
90  // Load the default or robot-specific config
91  {
92  // prefer robot-specific config if available
93  const armarx::PackagePath robotSpecificConfigPath(
94  "armarx_view_selection",
95  "controller_config/" + controllerTypeID + "/" + params.robotName + ".json");
96 
97  if (std::filesystem::exists(robotSpecificConfigPath.toSystemPath()))
98  {
99  ARMARX_INFO << "Using robot specific config: " << robotSpecificConfigPath;
100  builder.withConfig(robotSpecificConfigPath.toSystemPath());
101  }
102  else // fallback: use default config
103  {
104  const armarx::PackagePath defaultConfigPath(
105  "armarx_view_selection",
106  "controller_config/" + controllerTypeID + "/default.json");
107 
108  ARMARX_INFO << "Using default config: " << defaultConfigPath;
109 
110  ARMARX_CHECK(std::filesystem::exists(defaultConfigPath.toSystemPath()))
111  << defaultConfigPath;
112 
113  ARMARX_VERBOSE << "builder.withNodeSet().withConfig";
114  builder.withConfig(defaultConfigPath.toSystemPath());
115  }
116  }
117 
118 
119  ARMARX_VERBOSE << "builder.create()";
120  auto ctrlWrapper = builder.create();
121  ARMARX_TRACE;
122  ARMARX_CHECK_NOT_NULL(ctrlWrapper);
123 
124  ARMARX_VERBOSE << "Successfully created controller";
125  ctrl = std::move(ctrlWrapper);
126  }
127 
128 
129  ARMARX_TRACE;
130  ARMARX_CHECK_NOT_NULL(ctrl);
131  }
132 
133  void
135  {
136  ARMARX_VERBOSE << "Sending new target to the controller " << gazeTarget;
137  armarx::view_selection::gaze_targets::arondto::GazeTarget dto;
138  toAron(dto, gazeTarget);
139 
140  if (not ctrl->ctrl()->isControllerActive())
141  {
142  ARMARX_INFO << "Activating controller because it is not active yet.";
143  ctrl->ctrl()->activateController();
145  }
146 
147  ctrl->config.target = dto;
148  ctrl->updateConfig();
149  }
150 
151 
152  protected:
153  private:
154  std::unique_ptr<armarx::control::client::ControllerWrapper<ControllerT>> ctrl;
155 
156  const InjectedServices srv;
157 
158  const Params params;
159  };
160 } // namespace armarx::view_selection::gaze_scheduler
armarx::view_selection::gaze_scheduler::ControllerHandlerParams::robotName
std::string robotName
Definition: ControllerHandler.h:49
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
armarx::view_selection::gaze_scheduler::ControllerHandler::updateControllerTarget
void updateControllerTarget(const gaze_targets::GazeTarget &gazeTarget) override
Definition: ControllerHandler.h:134
armarx::view_selection::gaze_scheduler::ControllerHandler::createController
void createController() override
Definition: ControllerHandler.h:78
armarx::core::time::Clock::WaitFor
static void WaitFor(const Duration &duration)
Wait for a certain duration on the virtual clock.
Definition: Clock.cpp:99
armarx::view_selection::gaze_scheduler::ControllerHandler::Params
ControllerHandlerParams Params
Definition: ControllerHandler.h:57
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
controller_types.h
armarx::view_selection::gaze_scheduler::ControllerHandler
Definition: ControllerHandler.h:53
armarx::view_selection::gaze_targets::GazeTarget
Business Object (BO) class of GazeTarget.
Definition: GazeTarget.h:39
std::experimental::fundamentals_v2::observer_ptr
Definition: ManagedIceObject.h:53
armarx::control::client::ControllerDescription
Definition: ControllerDescription.h:103
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
aron_conversions.h
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:77
armarx::view_selection::gaze_scheduler::ControllerHandler::activateController
void activateController() override
Definition: ControllerHandler.h:70
armarx::PackagePath::toSystemPath
static std::filesystem::path toSystemPath(const data::PackagePath &pp)
Definition: PackagePath.cpp:47
armarx::view_selection::gaze_scheduler::ControllerHandler::~ControllerHandler
~ControllerHandler() override
Definition: ControllerHandler.h:64
armarx::view_selection::gaze_scheduler::ControllerHandlerInterface
Definition: ControllerHandlerInterface.h:29
armarx::view_selection::gaze_scheduler::ControllerHandler::ControllerHandler
ControllerHandler(const InjectedServices &srv, const Params &params)
Definition: ControllerHandler.h:59
ExpressionException.h
armarx::view_selection::gaze_scheduler
This file is part of ArmarX.
Definition: Component.cpp:57
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
aron_conversions.h
armarx::view_selection::gaze_scheduler::ControllerHandlerInjectedServices::controlComponentPlugin
std::experimental::observer_ptr< armarx::control::client::ComponentPlugin > controlComponentPlugin
Definition: ControllerHandler.h:44
ComponentPlugin.h
armarx::toAron
void toAron(arondto::PackagePath &dto, const PackageFileLocation &bo)
armarx::view_selection::gaze_scheduler::ControllerHandler::InjectedServices
ControllerHandlerInjectedServices InjectedServices
Definition: ControllerHandler.h:56
armarx::PackagePath
Definition: PackagePath.h:52
armarx::core::time::Duration::MilliSeconds
static Duration MilliSeconds(std::int64_t milliSeconds)
Constructs a duration in milliseconds.
Definition: Duration.cpp:48
armarx::view_selection::gaze_scheduler::ControllerHandlerInjectedServices
Definition: ControllerHandler.h:41
ControllerHandlerInterface.h
ControllerWrapper.h
PackagePath.h
armarx::view_selection::gaze_scheduler::ControllerHandlerParams
Definition: ControllerHandler.h:47