MRPT  1.9.9
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-2019, 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 
18 using namespace mrpt::nav;
19 
23 
24 /*---------------------------------------------------------------
25  initialize
26  ---------------------------------------------------------------*/
27 CHolonomicVFF::CHolonomicVFF(const mrpt::config::CConfigFileBase* INI_FILE)
29 {
30  if (INI_FILE != nullptr) initialize(*INI_FILE);
31 }
32 
34 {
35  options.loadFromConfigFile(INI_FILE, getConfigFileSectionName());
36 }
37 void CHolonomicVFF::saveConfigFile(mrpt::config::CConfigFileBase& c) const
38 {
39  options.saveToConfigFile(c, getConfigFileSectionName());
40 }
41 
42 /*---------------------------------------------------------------
43  navigate
44  ---------------------------------------------------------------*/
45 void CHolonomicVFF::navigate(const NavInput& ni, NavOutput& no)
46 {
47  const auto ptg = getAssociatedPTG();
48  const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;
49 
50  // Create a log record for returning data.
51  no.logRecord = std::make_shared<CLogFileRecord_VFF>();
52 
53  // Forces vector:
54  mrpt::math::TPoint2D resultantForce(0, 0), instantaneousForce(0, 0);
55 
56  // Obstacles:
57  {
58  const size_t n = ni.obstacles.size();
59  const double inc_ang = 2 * M_PI / n;
60  double ang = -M_PI + 0.5 * inc_ang;
61  for (size_t i = 0; i < n; i++, ang += inc_ang)
62  {
63  // Compute force strength:
64  // const double mod = exp(- obstacles[i] );
65  const double mod = std::min(1e6, 1.0 / ni.obstacles[i]);
66 
67  // Add repulsive force:
68  instantaneousForce.x = -cos(ang) * mod;
69  instantaneousForce.y = -sin(ang) * mod;
70  resultantForce += instantaneousForce;
71  }
72  }
73 
74  const double obstcl_weight = 20.0 / ni.obstacles.size();
75  resultantForce *= obstcl_weight;
76 
77  const double obstacleNearnessFactor =
78  std::min(1.0, 6.0 / resultantForce.norm());
79 
80  // Target:
81  ASSERT_(!ni.targets.empty());
82  const auto trg = *ni.targets.rbegin();
83 
84  const double ang = atan2(trg.y, trg.x);
85  const double mod = options.TARGET_ATTRACTIVE_FORCE;
86  resultantForce += mrpt::math::TPoint2D(cos(ang) * mod, sin(ang) * mod);
87 
88  // Result:
89  no.desiredDirection = (resultantForce.y == 0 && resultantForce.x == 0)
90  ? 0
91  : atan2(resultantForce.y, resultantForce.x);
92 
93  // Speed control: Reduction factors
94  // ---------------------------------------------
95  if (m_enableApproachTargetSlowDown)
96  {
97  const double targetNearnessFactor = std::min(
98  1.0, trg.norm() /
99  (options.TARGET_SLOW_APPROACHING_DISTANCE / ptg_ref_dist));
100  no.desiredSpeed =
101  ni.maxRobotSpeed *
102  std::min(obstacleNearnessFactor, targetNearnessFactor);
103  }
104 }
105 
106 uint8_t CHolonomicVFF::serializeGetVersion() const { return 0; }
107 void CHolonomicVFF::serializeTo(mrpt::serialization::CArchive& out) const
108 {
109  out << options.TARGET_ATTRACTIVE_FORCE
110  << options.TARGET_SLOW_APPROACHING_DISTANCE;
111 }
112 void CHolonomicVFF::serializeFrom(
113  mrpt::serialization::CArchive& in, uint8_t version)
114 {
115  switch (version)
116  {
117  case 0:
118  {
119  in >> options.TARGET_ATTRACTIVE_FORCE >>
120  options.TARGET_SLOW_APPROACHING_DISTANCE;
121  }
122  break;
123  default:
125  };
126 }
127 
128 uint8_t CLogFileRecord_VFF::serializeGetVersion() const { return 0; }
129 void CLogFileRecord_VFF::serializeTo(mrpt::serialization::CArchive& out) const
130 {
131 }
132 
133 void CLogFileRecord_VFF::serializeFrom(
134  mrpt::serialization::CArchive& in, uint8_t version)
135 {
137  switch (version)
138  {
139  case 0:
140  {
141  }
142  break;
143  default:
145  };
146 }
147 
148 /*---------------------------------------------------------------
149  TOptions
150  ---------------------------------------------------------------*/
151 CHolonomicVFF::TOptions::TOptions()
152 
153  = default;
154 
155 void CHolonomicVFF::TOptions::loadFromConfigFile(
156  const mrpt::config::CConfigFileBase& source, const std::string& section)
157 {
158  MRPT_START
159 
160  // Load from config text:
162  TARGET_SLOW_APPROACHING_DISTANCE, double, source, section);
163  MRPT_LOAD_CONFIG_VAR(TARGET_ATTRACTIVE_FORCE, double, source, section);
164 
165  MRPT_END
166 }
167 
168 void CHolonomicVFF::TOptions::saveToConfigFile(
170 {
171  MRPT_START;
172 
174  TARGET_SLOW_APPROACHING_DISTANCE, "For stopping gradually");
176  TARGET_ATTRACTIVE_FORCE,
177  "Dimension-less (may have to be tuned depending on the density of "
178  "obstacle sampling)");
179 
180  MRPT_END;
181 }
A holonomic reactive navigation method, based on Virtual Force Fields (VFF).
Definition: CHolonomicVFF.h:47
#define MRPT_START
Definition: exceptions.h:241
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.
GLenum GLsizei n
Definition: glext.h:5136
A base class for holonomic reactive navigation methods.
STL namespace.
GLdouble s
Definition: glext.h:3682
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...
const GLubyte * c
Definition: glext.h:6406
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.
GLsizei const GLchar ** string
Definition: glext.h:4116
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
GLuint in
Definition: glext.h:7391
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
Lightweight 2D point.
Definition: TPoint2D.h:31
Output for CAbstractHolonomicReactiveMethod::navigate()
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 836b840ab Mon Nov 18 00:58:29 2019 +0100 at lun nov 18 01:00:14 CET 2019