Main MRPT website > C++ reference for MRPT 1.5.6
CHolonomicVFF.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
11 
13 #include <mrpt/utils/CStream.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::utils;
17 using namespace std;
18 
19 using namespace mrpt::nav;
20 
23 
24 /*---------------------------------------------------------------
25  initialize
26  ---------------------------------------------------------------*/
27 CHolonomicVFF::CHolonomicVFF(const mrpt::utils::CConfigFileBase *INI_FILE) :
28  CAbstractHolonomicReactiveMethod("CHolonomicVFF")
29 {
30  if (INI_FILE!=NULL)
31  initialize( *INI_FILE );
32 }
33 
34 void CHolonomicVFF::initialize(const mrpt::utils::CConfigFileBase &INI_FILE)
35 {
36  options.loadFromConfigFile(INI_FILE, getConfigFileSectionName());
37 }
38 void CHolonomicVFF::saveConfigFile(mrpt::utils::CConfigFileBase &c) const
39 {
40  options.saveToConfigFile(c, getConfigFileSectionName());
41 }
42 
43 /*---------------------------------------------------------------
44  navigate
45  ---------------------------------------------------------------*/
46 void CHolonomicVFF::navigate(const NavInput & ni, NavOutput &no)
47 {
48  const auto ptg = getAssociatedPTG();
49  const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;
50 
51  // Create a log record for returning data.
52  no.logRecord = CLogFileRecord_VFF::Create();
53 
54  // Forces vector:
55  mrpt::math::TPoint2D resultantForce(0,0),instantaneousForce(0,0);
56 
57  // Obstacles:
58  {
59  const size_t n = ni.obstacles.size();
60  const double inc_ang = 2*M_PI/n;
61  double ang = -M_PI + 0.5*inc_ang;
62  for (size_t i=0;i<n;i++, ang+=inc_ang )
63  {
64  // Compute force strength:
65  //const double mod = exp(- obstacles[i] );
66  const double mod = std::min(1e6, 1.0/ ni.obstacles[i] );
67 
68  // Add repulsive force:
69  instantaneousForce.x = -cos(ang) * mod;
70  instantaneousForce.y = -sin(ang) * mod;
71  resultantForce += instantaneousForce;
72  }
73  }
74 
75  const double obstcl_weight = 20.0/ ni.obstacles.size();
76  resultantForce *= obstcl_weight;
77 
78  const double obstacleNearnessFactor = 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 : atan2( resultantForce.y, resultantForce.x );
91 
92  // Speed control: Reduction factors
93  // ---------------------------------------------
94  if (m_enableApproachTargetSlowDown)
95  {
96  const double targetNearnessFactor = std::min(1.0, trg.norm() / (options.TARGET_SLOW_APPROACHING_DISTANCE/ ptg_ref_dist));
97  no.desiredSpeed = ni.maxRobotSpeed * std::min(obstacleNearnessFactor, targetNearnessFactor);
98  }
99 }
100 
101 void CHolonomicVFF::writeToStream(mrpt::utils::CStream &out,int *version) const
102 {
103  if (version)
104  *version = 0;
105  else
106  {
107  out << options.TARGET_ATTRACTIVE_FORCE << options.TARGET_SLOW_APPROACHING_DISTANCE;
108  }
109 }
110 void CHolonomicVFF::readFromStream(mrpt::utils::CStream &in,int version)
111 {
112  switch(version)
113  {
114  case 0:
115  {
116  in >> options.TARGET_ATTRACTIVE_FORCE >> options.TARGET_SLOW_APPROACHING_DISTANCE;
117  } break;
118  default:
120  };
121 }
122 
123 
124 /*---------------------------------------------------------------
125  writeToStream
126  Implements the writing to a CStream capability of
127  CSerializable objects
128  ---------------------------------------------------------------*/
129 void CLogFileRecord_VFF::writeToStream(mrpt::utils::CStream &out,int *version) const
130 {
131  MRPT_UNUSED_PARAM(out);
132  if (version)
133  *version = 0;
134  else
135  {
136  }
137 }
138 
139 /*---------------------------------------------------------------
140  readFromStream
141  ---------------------------------------------------------------*/
142 void CLogFileRecord_VFF::readFromStream(mrpt::utils::CStream &in,int version)
143 {
144  MRPT_UNUSED_PARAM(in);
145  switch(version)
146  {
147  case 0:
148  {
149  } break;
150  default:
152  };
153 }
154 
155 /*---------------------------------------------------------------
156  TOptions
157  ---------------------------------------------------------------*/
158 CHolonomicVFF::TOptions::TOptions() :
159  TARGET_SLOW_APPROACHING_DISTANCE ( 0.10 ),
160  TARGET_ATTRACTIVE_FORCE ( 20.0 )
161 {
162 }
163 
165 {
166  MRPT_START
167 
168  // Load from config text:
169  MRPT_LOAD_CONFIG_VAR(TARGET_SLOW_APPROACHING_DISTANCE,double, source,section );
170  MRPT_LOAD_CONFIG_VAR(TARGET_ATTRACTIVE_FORCE,double, source,section );
171 
172  MRPT_END
173 }
174 
176 {
177  MRPT_START;
178 
179  MRPT_SAVE_CONFIG_VAR_COMMENT(TARGET_SLOW_APPROACHING_DISTANCE, "For stopping gradually");
180  MRPT_SAVE_CONFIG_VAR_COMMENT(TARGET_ATTRACTIVE_FORCE, "Dimension-less (may have to be tuned depending on the density of obstacle sampling)");
181 
182  MRPT_END;
183 }
A holonomic reactive navigation method, based on Virtual Force Fields (VFF).
Definition: CHolonomicVFF.h:53
double y
X,Y coordinates.
#define min(a, b)
A class for storing extra information about the execution of CHolonomicVFF navigation.
Definition: CHolonomicVFF.h:30
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
std::vector< double > obstacles
Distance to obstacles in polar coordinates, relative to the robot.
const GLfloat * c
Definition: glew.h:10088
A base class for holonomic reactive navigation methods.
void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &section) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
#define M_PI
Definition: bits.h:78
GLuint in
Definition: glew.h:7146
std::vector< mrpt::math::TPoint2D > targets
Relative location (x,y) of target point(s). In the same units than obstacles. If many, last targets have higher priority.
This class allows loading and storing values and vectors of different types from a configuration text...
double maxRobotSpeed
Maximum robot speed, in the same units than obstacles, per second.
GLdouble s
Definition: glew.h:1295
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
GLsizei n
Definition: glew.h:5051
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
A base class for log records for different holonomic navigation methods.
double desiredDirection
The desired motion direction, in the range [-PI, PI].
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
int version
Definition: mrpt_jpeglib.h:898
double desiredSpeed
The desired motion speed in that direction, from 0 up to NavInput::maxRobotSpeed. ...
#define MRPT_START
#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...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
#define ASSERT_(f)
Input parameters for CAbstractHolonomicReactiveMethod::navigate()
Lightweight 2D point.
Output for CAbstractHolonomicReactiveMethod::navigate()
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
CHolonomicLogFileRecordPtr logRecord
The navigation method will create a log record and store it here via a smart pointer.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018