MRPT  2.0.4
CHolonomicVFF.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
11 
14 
15 using namespace mrpt;
16 using namespace std;
17 using namespace mrpt::nav;
18 
22 
23 /*---------------------------------------------------------------
24  initialize
25  ---------------------------------------------------------------*/
26 CHolonomicVFF::CHolonomicVFF(const mrpt::config::CConfigFileBase* INI_FILE)
28 {
29  if (INI_FILE != nullptr) initialize(*INI_FILE);
30 }
31 
33 {
34  options.loadFromConfigFile(INI_FILE, getConfigFileSectionName());
35 }
36 void CHolonomicVFF::saveConfigFile(mrpt::config::CConfigFileBase& c) const
37 {
38  options.saveToConfigFile(c, getConfigFileSectionName());
39 }
40 
41 /*---------------------------------------------------------------
42  navigate
43  ---------------------------------------------------------------*/
44 void CHolonomicVFF::navigate(const NavInput& ni, NavOutput& no)
45 {
46  const auto ptg = getAssociatedPTG();
47  const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;
48 
49  // Create a log record for returning data.
50  no.logRecord = std::make_shared<CLogFileRecord_VFF>();
51 
52  // Forces vector:
53  mrpt::math::TPoint2D resultantForce(0, 0), instantaneousForce(0, 0);
54 
55  // Obstacles:
56  {
57  const size_t n = ni.obstacles.size();
58  const double inc_ang = 2 * M_PI / n;
59  double ang = -M_PI + 0.5 * inc_ang;
60  for (size_t i = 0; i < n; i++, ang += inc_ang)
61  {
62  // Compute force strength:
63  // const double mod = exp(- obstacles[i] );
64  const double mod = std::min(1e6, 1.0 / ni.obstacles[i]);
65 
66  // Add repulsive force:
67  instantaneousForce.x = -cos(ang) * mod;
68  instantaneousForce.y = -sin(ang) * mod;
69  resultantForce += instantaneousForce;
70  }
71  }
72 
73  const double obstcl_weight = 20.0 / ni.obstacles.size();
74  resultantForce *= obstcl_weight;
75 
76  const double obstacleNearnessFactor =
77  std::min(1.0, 6.0 / resultantForce.norm());
78 
79  // Target:
80  ASSERT_(!ni.targets.empty());
81  const auto trg = *ni.targets.rbegin();
82 
83  const double ang = atan2(trg.y, trg.x);
84  const double mod = options.TARGET_ATTRACTIVE_FORCE;
85  resultantForce += mrpt::math::TPoint2D(cos(ang) * mod, sin(ang) * mod);
86 
87  // Result:
88  no.desiredDirection = (resultantForce.y == 0 && resultantForce.x == 0)
89  ? 0
90  : atan2(resultantForce.y, resultantForce.x);
91 
92  // Speed control: Reduction factors
93  // ---------------------------------------------
94  if (m_enableApproachTargetSlowDown)
95  {
96  const double targetNearnessFactor = std::min(
97  1.0, trg.norm() /
98  (options.TARGET_SLOW_APPROACHING_DISTANCE / ptg_ref_dist));
99  no.desiredSpeed =
100  ni.maxRobotSpeed *
101  std::min(obstacleNearnessFactor, targetNearnessFactor);
102  }
103 }
104 
105 uint8_t CHolonomicVFF::serializeGetVersion() const { return 0; }
106 void CHolonomicVFF::serializeTo(mrpt::serialization::CArchive& out) const
107 {
108  out << options.TARGET_ATTRACTIVE_FORCE
109  << options.TARGET_SLOW_APPROACHING_DISTANCE;
110 }
111 void CHolonomicVFF::serializeFrom(
112  mrpt::serialization::CArchive& in, uint8_t version)
113 {
114  switch (version)
115  {
116  case 0:
117  {
118  in >> options.TARGET_ATTRACTIVE_FORCE >>
119  options.TARGET_SLOW_APPROACHING_DISTANCE;
120  }
121  break;
122  default:
124  };
125 }
126 
127 uint8_t CLogFileRecord_VFF::serializeGetVersion() const { return 0; }
128 void CLogFileRecord_VFF::serializeTo(mrpt::serialization::CArchive& out) const
129 {
130 }
131 
132 void CLogFileRecord_VFF::serializeFrom(
133  [[maybe_unused]] mrpt::serialization::CArchive& in, uint8_t version)
134 {
135  switch (version)
136  {
137  case 0:
138  {
139  }
140  break;
141  default:
143  };
144 }
145 
146 /*---------------------------------------------------------------
147  TOptions
148  ---------------------------------------------------------------*/
149 CHolonomicVFF::TOptions::TOptions()
150 
151  = default;
152 
153 void CHolonomicVFF::TOptions::loadFromConfigFile(
154  const mrpt::config::CConfigFileBase& source, const std::string& section)
155 {
156  MRPT_START
157 
158  // Load from config text:
160  TARGET_SLOW_APPROACHING_DISTANCE, double, source, section);
161  MRPT_LOAD_CONFIG_VAR(TARGET_ATTRACTIVE_FORCE, double, source, section);
162 
163  MRPT_END
164 }
165 
166 void CHolonomicVFF::TOptions::saveToConfigFile(
167  mrpt::config::CConfigFileBase& c, const std::string& s) const
168 {
169  MRPT_START
170 
172  TARGET_SLOW_APPROACHING_DISTANCE, "For stopping gradually");
174  TARGET_ATTRACTIVE_FORCE,
175  "Dimension-less (may have to be tuned depending on the density of "
176  "obstacle sampling)");
177 
178  MRPT_END
179 }
A holonomic reactive navigation method, based on Virtual Force Fields (VFF).
Definition: CHolonomicVFF.h:47
#define MRPT_START
Definition: exceptions.h:241
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
app initialize(argc, argv)
A class for storing extra information about the execution of CHolonomicVFF navigation.
Definition: CHolonomicVFF.h:25
std::vector< double > obstacles
Distance to obstacles in polar coordinates, relative to the robot.
A base class for holonomic reactive navigation methods.
STL namespace.
std::vector< mrpt::math::TPoint2D > targets
Relative location (x,y) of target point(s).
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
double maxRobotSpeed
Maximum robot speed, in the same units than obstacles, per second.
This class allows loading and storing values and vectors of different types from a configuration text...
A base class for log records for different holonomic navigation methods.
double desiredDirection
The desired motion direction, in the range [-PI, PI].
mrpt::config::CConfigFileBase CConfigFileBase
CHolonomicLogFileRecord::Ptr logRecord
The navigation method will create a log record and store it here via a smart pointer.
IMPLEMENTS_SERIALIZABLE(CHolonomicVFF, CAbstractHolonomicReactiveMethod, mrpt::nav) CHolonomicVFF
double desiredSpeed
The desired motion speed in that direction, from 0 up to NavInput::maxRobotSpeed. ...
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
Output for CAbstractHolonomicReactiveMethod::navigate()



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020