40 m_alphaValuesCount(0),
41 m_score_priority(1.0),
42 m_clearance_num_points(5),
43 m_clearance_decimated_paths(15),
45 m_nav_dyn_state_target_k(INVALID_PTG_PATH_INDEX),
46 m_is_initialized(false)
50 void CParameterizedTrajectoryGenerator::loadDefaultParams()
110 const int WN = 25, WV = 30;
114 "Number of discrete paths (`resolution`) in the PTG");
117 "Maximum distance (meters) for building trajectories (visibility " 121 "When used in path planning, a multiplying factor (default=1.0) for " 122 "the scores for this PTG. Assign values <1 to PTGs with low priority.");
125 "Number of steps for the piecewise-constant approximation of clearance " 130 "Number of decimated paths for estimation of clearance (Default=15).");
135 "(Only for debugging) Current robot velocity vx [m/s].");
138 "(Only for debugging) Current robot velocity vy [m/s].");
141 WV,
"(Only for debugging) Current robot velocity omega [deg/s].");
145 "(Only for debugging) Relative target x [m].");
148 "(Only for debugging) Relative target y [m].");
151 WN, WV,
"(Only for debugging) Relative target phi [deg].");
155 "(Only for debugging) Desired relative speed at target [0,1]");
178 bool old_use_approx_clearance;
179 in >> old_use_approx_clearance;
207 uint16_t k,
const unsigned int num_paths)
210 return M_PI * (-1.0 + 2.0 * (k + 0.5) / num_paths);
219 double alpha,
const unsigned int num_paths)
224 if (k >= static_cast<int>(num_paths)) k = num_paths - 1;
235 const double decimate_distance,
const double max_path_distance)
const 241 double last_added_dist = 0.0;
242 for (
size_t n = 0;
n < nPointsInPath;
n++)
248 if (max_path_distance >= 0.0 && d >= max_path_distance)
break;
250 if (d < last_added_dist + decimate_distance &&
n != 0)
269 std::vector<double>& TP_Obstacles)
const 276 uint16_t k,
double& TP_Obstacle_k)
const 296 const string sFilTxt_x =
297 mrpt::format(
"%s/PTGs/PTG%s_x.txt", sPath, ptg_name.c_str());
298 const string sFilTxt_y =
299 mrpt::format(
"%s/PTGs/PTG%s_y.txt", sPath, ptg_name.c_str());
300 const string sFilTxt_phi =
301 mrpt::format(
"%s/PTGs/PTG%s_phi.txt", sPath, ptg_name.c_str());
302 const string sFilTxt_t =
303 mrpt::format(
"%s/PTGs/PTG%s_t.txt", sPath, ptg_name.c_str());
304 const string sFilTxt_d =
305 mrpt::format(
"%s/PTGs/PTG%s_d.txt", sPath, ptg_name.c_str());
307 ofstream fx(sFilTxt_x.c_str());
308 if (!fx.is_open())
return false;
309 ofstream fy(sFilTxt_y.c_str());
310 if (!fy.is_open())
return false;
311 ofstream fp(sFilTxt_phi.c_str());
312 if (!fp.is_open())
return false;
313 ofstream fd(sFilTxt_d.c_str());
314 if (!fd.is_open())
return false;
319 fx <<
"% PTG data file for 'x'. Each row is the trajectory for a different " 320 "'alpha' parameter value." 322 fy <<
"% PTG data file for 'y'. Each row is the trajectory for a different " 323 "'alpha' parameter value." 325 fp <<
"% PTG data file for 'phi'. Each row is the trajectory for a " 326 "different 'alpha' parameter value." 328 fd <<
"% PTG data file for 'd'. Each row is the trajectory for a different " 329 "'alpha' parameter value." 332 vector<size_t> path_length(nPaths);
335 size_t maxPoints = 0;
336 for (
size_t k = 0; k < nPaths; k++)
337 maxPoints = max(maxPoints, path_length[k]);
339 for (
size_t k = 0; k < nPaths; k++)
341 for (
size_t n = 0;
n < maxPoints;
n++)
343 const size_t nn =
std::min(
n, path_length[k] - 1);
367 const bool force_update)
390 double target_norm_d;
395 target_k, target_norm_d, 1.0 );
396 if (target_norm_d > 0.01 && target_norm_d < 0.99 && target_k >= 0 &&
407 const std::string& cacheFilename,
const bool verbose)
412 !cacheFilename.empty()
429 const double ox,
const double oy,
const double new_tp_obs_dist,
430 double& inout_tp_obs)
const 433 if (!is_obs_inside_robot_shape)
480 const double numStepsPerIncr =
484 for (
double step_pointer_dbl = 0.0; step_pointer_dbl < numPathSteps;
485 step_pointer_dbl += numStepsPerIncr)
488 const double dist_over_path = this->
getPathDist(real_k, step);
489 cl_path[dist_over_path] = 1.0;
519 const double ox,
const double oy,
const uint16_t k,
521 bool treat_as_obstacle)
const 523 bool had_collision =
false;
526 ASSERT_(numPathSteps > inout_realdist2clearance.size());
528 const double numStepsPerIncr =
529 (numPathSteps - 1.0) / (inout_realdist2clearance.size());
531 double step_pointer_dbl = 0.0;
535 for (
auto& e : inout_realdist2clearance)
537 step_pointer_dbl += numStepsPerIncr;
540 double& inout_clearance = e.second;
547 inout_clearance = .0;
556 const double this_clearance =
559 if (this_clearance <= .0 && treat_as_obstacle)
562 had_collision =
true;
563 inout_clearance = .0;
568 const double this_clearance_norm =
578 : curVelLocal(0, 0, 0),
579 relTarget(20.0, 0, 0),
597 out << curVelLocal << relTarget << targetRelSpeed;
608 in >> curVelLocal >> relTarget >> targetRelSpeed;
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
void readFromStream(mrpt::serialization::CArchive &in)
virtual bool supportSpeedAtTarget() const
Returns true if this PTG takes into account the desired velocity at target.
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
void initialize(const std::string &cacheFilename=std::string(), const bool verbose=true)
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Spac...
virtual bool supportVelCmdNOP() const
Returns true if it is possible to stop sending velocity commands to the robot and, still, the robot controller will be able to keep following the last sent trajectory ("NOP" velocity commands).
GLclampf GLclampf GLclampf alpha
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose
double targetRelSpeed
Desired relative speed [0,1] at target.
virtual void internal_writeToStream(mrpt::serialization::CArchive &out) const
size_t get_decimated_num_paths() const
static PTG_collision_behavior_t & COLLISION_BEHAVIOR()
Defines the behavior when there is an obstacle inside the robot shape right at the beginning of a PTG...
bool createDirectory(const std::string &dirName)
Creates a directory.
virtual void internal_initialize(const std::string &cacheFilename=std::string(), const bool verbose=true)=0
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Spac...
unsigned __int16 uint16_t
IMPLEMENTS_VIRTUAL_SERIALIZABLE(CParameterizedTrajectoryGenerator, CSerializable, mrpt::nav) CParameterizedTrajectoryGenerator
double RAD2DEG(const double x)
Radians to degrees.
virtual void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
void appendLineStrip(float x, float y, float z)
Appends a line whose starting point is the end point of the last line (similar to OpenGL's GL_LINE_ST...
#define THROW_EXCEPTION(msg)
dist2clearance_t & get_path_clearance_decimated(size_t decim_k)
virtual void renderPathAsSimpleLine(const uint16_t k, mrpt::opengl::CSetOfLines &gl_obj, const double decimate_distance=0.1, const double max_path_distance=-1.0) const
Returns the representation of one trajectory of this PTG as a 3D OpenGL object (a simple curved line)...
#define ASSERT_BELOW_(__A, __B)
virtual bool isPointInsideRobotShape(const double x, const double y) const =0
Returns true if the point lies within the robot shape.
void initTPObstacles(std::vector< double > &TP_Obstacles) const
Resizes and populates the initial appropriate contents in a vector of tp-obstacles (collision-free ra...
Favor getting back from too-close (almost collision) obstacles.
virtual double maxTimeInVelCmdNOP(int path_k) const
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
bool operator==(const TNavDynamicState &o) const
void updateClearancePost(ClearanceDiagram &cd, const std::vector< double > &TP_obstacles) const
void updateNavDynamicState(const TNavDynamicState &newState, const bool force_update=false)
To be invoked by the navigator before each navigation step, to let the PTG to react to changing dynam...
uint16_t m_nav_dyn_state_target_k
Update in updateNavDynamicState(), contains the path index (k) for the target.
Clearance information for one particular PTG and one set of obstacles.
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores ('_') or any other user-given char. ...
virtual size_t getPathStepCount(uint16_t k) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps" along the trajectory.
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
virtual double getPathDist(uint16_t k, uint32_t step) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.
virtual void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const =0
Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.
bool debugDumpInFiles(const std::string &ptg_name) const
Dump PTG trajectories in four text files: `.
void internal_TPObsDistancePostprocess(const double ox, const double oy, const double new_tp_obs_dist, double &inout_tp_obs) const
To be called by implementors of updateTPObstacle() and updateTPObstacleSingle() to honor the user set...
void initTPObstacleSingle(uint16_t k, double &TP_Obstacle_k) const
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
#define ASSERT_(f)
Defines an assertion mechanism.
This is the base class for any user-defined PTG.
This class allows loading and storing values and vectors of different types from a configuration text...
size_t decimated_k_to_real_k(size_t k) const
CParameterizedTrajectoryGenerator()
Default ctor.
void appendLine(const mrpt::math::TSegment3D &sgm)
Appends a line to the set.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
size_t get_actual_num_paths() const
double vx
Velocity components: X,Y (m/s)
mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
bool isInitialized() const
Returns true if initialize() has been called and there was no errors, so the PTG is ready to be queri...
mrpt::math::TTwist2D curVelLocal
Current vehicle velocity (local frame of reference)
virtual void onNewNavDynamicState()=0
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
static std::string OUTPUT_DEBUG_PATH_PREFIX
PTG_collision_behavior_t
Defines behaviors for where there is an obstacle inside the robot shape right at the beginning of a P...
void initClearanceDiagram(ClearanceDiagram &cd) const
Must be called to resize a CD to its correct size, before calling updateClearance() ...
mrpt::math::TPose2D relTarget
Current relative target location.
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
void resize(size_t actual_num_paths, size_t decimated_num_paths)
Initializes the container to allocate decimated_num_paths entries, as a decimated subset of a total o...
GLsizei const GLchar ** string
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
void writeToStream(mrpt::serialization::CArchive &out) const
void write(const std::string §ion, const std::string &name, enum_t value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
uint16_t m_clearance_num_points
Number of steps for the piecewise-constant approximation of clearance from TPS distances [0...
uint16_t getAlphaValuesCount() const
Get the number of different, discrete paths in this family.
double norm() const
Point norm.
Totally dissallow any movement if there is any too-close (almost collision) obstacles.
void updateClearance(const double ox, const double oy, ClearanceDiagram &cd) const
Updates the clearance diagram given one (ox,oy) obstacle point, in coordinates relative to the PTG pa...
unsigned __int64 uint64_t
void deinitialize()
This must be called to de-initialize the PTG if some parameter is to be changed.
Virtual base class for "archives": classes abstracting I/O streams.
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
uint16_t m_clearance_decimated_paths
Number of paths for the decimated paths analysis of clearance.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
virtual void evalClearanceSingleObstacle(const double ox, const double oy, const uint16_t k, ClearanceDiagram::dist2clearance_t &inout_realdist2clearance, bool treat_as_obstacle=true) const
Evals the robot clearance for each robot pose along path k, for the real distances in the key of the ...
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
static const uint16_t INVALID_PTG_PATH_INDEX
static std::string & OUTPUT_DEBUG_PATH_PREFIX()
The path used as defaul output in, for example, debugDumpInFiles.
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
virtual double evalClearanceToRobotShape(const double ox, const double oy) const =0
Evals the clearance from an obstacle (ox,oy) in coordinates relative to the robot center...
A set of independent lines (or segments), one line with its own start and end positions (X...
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
TNavDynamicState m_nav_dyn_state
Updated before each nav step by.
static PTG_collision_behavior_t COLLISION_BEHAVIOR
Dynamic state that may affect the PTG path parameterization.
virtual void internal_deinitialize()=0
This must be called to de-initialize the PTG if some parameter is to be changed.
double phi
Orientation (rads)
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
virtual double getMaxRobotRadius() const =0
Returns an approximation of the robot radius.
double omega
Angular velocity (rad/s)
virtual bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_normalized_d, double tolerance_dist=0.10) const =0
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) C...
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.
int round(const T value)
Returns the closer integer (int) to x.