DSObstacleAvoidance.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 RobotAPI::ArmarXObjects::DSObstacleAvoidance
17  * @author Christian R. G. Dreher <c.dreher@kit.edu>
18  * @date 2020
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 
24 #pragma once
25 
26 
27 // STD/STL
28 #include <array>
29 #include <deque>
30 #include <shared_mutex>
31 #include <string>
32 #include <map>
33 #include <mutex>
34 #include <vector>
35 
36 // Eigen
37 #include <Eigen/Geometry>
38 
39 // Ice
40 #include <Ice/Current.h>
41 
42 // DynamicObstacleAvoidance
43 #include <DynamicObstacleAvoidance/Agent.hpp>
44 #include <DynamicObstacleAvoidance/Environment.hpp>
45 
46 // ArmarX
48 #include <ArmarXCore/util/tasks.h>
49 #include <ArmarXCore/util/time.h>
50 
51 // RobotAPI
53 #include <RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.h>
54 
55 
56 namespace armarx
57 {
58 
60  virtual public Component,
61  virtual public DSObstacleAvoidanceInterface,
62  virtual public ArVizComponentPluginUser
63  {
64 
65  public:
66 
68 
69  /**
70  * @see armarx::ManagedIceObject::getDefaultName()
71  */
72  std::string
74  const override;
75 
76  dsobstacleavoidance::Config
77  getConfig(const Ice::Current& = Ice::emptyCurrent)
78  override;
79 
80  void
82  const obstacledetection::Obstacle& obstacle,
83  const Ice::Current& = Ice::emptyCurrent)
84  override;
85 
86  std::vector<obstacledetection::Obstacle>
87  getObstacles(const Ice::Current& = Ice::emptyCurrent)
88  override;
89 
90  void
92  const std::string& obstacle_name,
93  const Ice::Current& = Ice::emptyCurrent)
94  override;
95 
96  Eigen::Vector3f
98  const obstacleavoidance::Agent& agent,
99  const Ice::Current& = Ice::emptyCurrent)
100  override;
101 
102  void
103  updateEnvironment(const Ice::Current& = Ice::emptyCurrent)
104  override;
105 
106  void
107  writeDebugPlots(const std::string& filename, const Ice::Current& = Ice::emptyCurrent)
108  override;
109 
110  protected:
111 
112  /**
113  * @see armarx::ManagedIceObject::onInitComponent()
114  */
115  void
117  override;
118 
119  /**
120  * @see armarx::ManagedIceObject::onConnectComponent()
121  */
122  void
124  override;
125 
126  /**
127  * @see armarx::ManagedIceObject::onDisconnectComponent()
128  */
129  void
131  override;
132 
133  /**
134  * @see armarx::ManagedIceObject::onExitComponent()
135  */
136  void
138  override;
139 
140  /**
141  * @see PropertyUser::createPropertyDefinitions()
142  */
145  override;
146 
147  private:
148 
149  void
150  sanity_check_obstacle(const obstacledetection::Obstacle& obstacle)
151  const;
152 
153  void
154  run_watchdog();
155 
156  void
157  run_visualization();
158 
159  public:
160 
161  static const std::string default_name;
162 
163  private:
164 
165  struct visualization
166  {
167  bool enabled = true;
169  unsigned task_interval = 20;
170  mutable std::mutex mutex;
171  bool needs_update = true;
172  };
173 
174  struct watchdog
175  {
176  bool enabled = true;
178  unsigned task_interval = 100;
179  IceUtil::Time threshold = IceUtil::Time::milliSeconds(500);
180  };
181 
182  struct dynamic_obstacle_avoidance
183  {
184  dsobstacleavoidance::Config cfg;
185  DynamicObstacleAvoidance::Environment env;
186  mutable std::shared_mutex mutex;
187  std::string load_obstacles_from_file =
188  "RobotAPI/obstacle_avoidance/r034_2d_scene_obstacles.json";
189  };
190 
191  struct buffer
192  {
193  enum class update_type
194  {
195  update,
196  removal
197  };
198 
199  struct update
200  {
202  std::string name;
203  DSObstacleAvoidance::buffer::update_type type;
204  };
205 
206  mutable std::mutex mutex;
207  std::vector<DSObstacleAvoidance::buffer::update> update_log;
208  std::map<std::string, obstacledetection::Obstacle> obstacles;
209  bool needs_env_update;
210  IceUtil::Time last_env_update;
211  };
212 
213  DSObstacleAvoidance::visualization m_vis;
214 
215  DSObstacleAvoidance::watchdog m_watchdog;
216 
217  DSObstacleAvoidance::dynamic_obstacle_avoidance m_doa;
218 
219  DSObstacleAvoidance::buffer m_buf;
220 
221  };
222 
223 }
armarx::DSObstacleAvoidance::removeObstacle
void removeObstacle(const std::string &obstacle_name, const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:298
armarx::DSObstacleAvoidance::getObstacles
std::vector< obstacledetection::Obstacle > getObstacles(const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:317
armarx::DSObstacleAvoidance::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: DSObstacleAvoidance.cpp:655
ArVizComponentPlugin.h
time.h
armarx::DSObstacleAvoidance::buffer::update
Definition: DSObstacleAvoidance.h:199
armarx::DSObstacleAvoidance::buffer::update::type
DSObstacleAvoidance::buffer::update_type type
Definition: DSObstacleAvoidance.h:203
armarx::DSObstacleAvoidance::onDisconnectComponent
void onDisconnectComponent() override
Definition: DSObstacleAvoidance.cpp:231
armarx::DSObstacleAvoidance::DSObstacleAvoidance
DSObstacleAvoidance()
Definition: DSObstacleAvoidance.cpp:149
armarx::DSObstacleAvoidance::buffer::update::time
IceUtil::Time time
Definition: DSObstacleAvoidance.h:201
armarx::DSObstacleAvoidance::getDefaultName
std::string getDefaultName() const override
Definition: DSObstacleAvoidance.cpp:259
armarx::DSObstacleAvoidance::default_name
static const std::string default_name
Definition: DSObstacleAvoidance.h:161
armarx::ArVizComponentPluginUser
Provides a ready-to-use ArViz client arviz as member variable.
Definition: ArVizComponentPlugin.h:36
armarx::DSObstacleAvoidance::modulateVelocity
Eigen::Vector3f modulateVelocity(const obstacleavoidance::Agent &agent, const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:358
armarx::DSObstacleAvoidance::updateEnvironment
void updateEnvironment(const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:408
enabled
std::atomic< bool > * enabled
Definition: RemoteGuiWidgetController.cpp:75
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::DSObstacleAvoidance::updateObstacle
void updateObstacle(const obstacledetection::Obstacle &obstacle, const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:274
armarx::DSObstacleAvoidance::onConnectComponent
void onConnectComponent() override
Definition: DSObstacleAvoidance.cpp:206
tasks.h
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:67
Component.h
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:95
armarx::DSObstacleAvoidance::buffer::update::name
std::string name
Definition: DSObstacleAvoidance.h:202
armarx::DSObstacleAvoidance
Definition: DSObstacleAvoidance.h:59
armarx::DSObstacleAvoidance::writeDebugPlots
void writeDebugPlots(const std::string &filename, const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:487
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::DSObstacleAvoidance::onInitComponent
void onInitComponent() override
Definition: DSObstacleAvoidance.cpp:164
armarx::DSObstacleAvoidance::onExitComponent
void onExitComponent() override
Definition: DSObstacleAvoidance.cpp:250
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::DSObstacleAvoidance::getConfig
dsobstacleavoidance::Config getConfig(const Ice::Current &=Ice::emptyCurrent) override
Definition: DSObstacleAvoidance.cpp:267