Main MRPT website > C++ reference for MRPT 1.5.6
CPTG_RobotShape_Circular.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 
14 #include <mrpt/utils/CStream.h>
15 #include <mrpt/utils/round.h>
16 
17 using namespace mrpt::nav;
18 
20  m_robotRadius(.0)
21 {
22 }
23 
25 {
26 }
27 
28 void CPTG_RobotShape_Circular::setRobotShapeRadius(const double robot_radius)
29 {
30  m_robotRadius = robot_radius;
32 }
33 
35 {
36  m_robotRadius = 0.2;
37 }
38 
40 {
41  const double old_R = m_robotRadius;
42  MRPT_LOAD_HERE_CONFIG_VAR(robot_radius, double, m_robotRadius, cfg, sSection);
43 
44  if (m_robotRadius != old_R)
46 }
48 {
49  const int WN = 25, WV = 30;
50 
51  cfg.write(sSection, "robot_radius", m_robotRadius, WN, WV, "Robot radius [m].");
52 }
53 
55  mrpt::opengl::CSetOfLines &gl_shape,
56  const mrpt::poses::CPose2D &origin) const
57 {
58  const double R = m_robotRadius;
59  const int N = 17;
60  // Transform coordinates:
61  mrpt::math::CVectorDouble shap_x(N), shap_y(N), shap_z(N);
62  for (int i = 0; i<N; i++) {
63  origin.composePoint(
64  R*cos(i * 2 * M_PI / (N - 1)), R*sin(i * 2 * M_PI / (N - 1)), 0,
65  shap_x[i], shap_y[i], shap_z[i]);
66  }
67  // Draw a "radius" to identify the "forward" orientation (phi=0)
68  gl_shape.appendLine(origin.x(), origin.y(), .0, shap_x[0], shap_y[0], shap_z[0]);
69  for (int i = 1; i <= shap_x.size(); i++) {
70  const int idx = i % shap_x.size();
71  gl_shape.appendLineStrip(shap_x[idx], shap_y[idx], shap_z[idx]);
72  }
73  // Draw a "cross" to identify the robot center
74  gl_shape.appendLine(origin.x() - R*0.02, origin.y(), .0, origin.x() + R*0.02, origin.y(), .0);
75  gl_shape.appendLine(origin.x(), origin.y() - R*0.02, .0, origin.x(), origin.y() + R*0.02, .0);
76 }
77 
78 
80 {
82  in >> version;
83 
84  switch (version)
85  {
86  case 0:
87  in >> m_robotRadius;
88  break;
89  default:
91  }
92 }
93 
95 {
96  uint8_t version = 0;
97  out << version;
98 
99  out << m_robotRadius;
100 }
101 
103 {
104  return m_robotRadius;
105 }
106 
107 bool CPTG_RobotShape_Circular::isPointInsideRobotShape(const double x, const double y) const
108 {
109  return mrpt::math::hypot_fast(x, y) < m_robotRadius;
110 }
111 
112 double CPTG_RobotShape_Circular::evalClearanceToRobotShape(const double ox, const double oy) const
113 {
114  return mrpt::math::hypot_fast(ox, oy) - m_robotRadius;
115 }
116 
117 
118 
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
void loadShapeFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
void internal_shape_saveToStream(mrpt::utils::CStream &out) const
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...
Definition: CSetOfLines.h:84
double getMaxRobotRadius() const MRPT_OVERRIDE
Returns an approximation of the robot radius.
void add_robotShape_to_setOfLines(mrpt::opengl::CSetOfLines &gl_shape, const mrpt::poses::CPose2D &origin=mrpt::poses::CPose2D()) const MRPT_OVERRIDE
Auxiliary function for rendering.
bool isPointInsideRobotShape(const double x, const double y) const MRPT_OVERRIDE
Returns true if the point lies within the robot shape.
virtual void internal_processNewRobotShape()=0
Will be called whenever the robot shape is set / updated.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
#define M_PI
Definition: bits.h:78
void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
GLuint in
Definition: glew.h:7146
This class allows loading and storing values and vectors of different types from a configuration text...
unsigned char uint8_t
Definition: rptypes.h:43
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void appendLine(const mrpt::math::TSegment3D &sgm)
Appends a line to the set.
Definition: CSetOfLines.h:70
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
void setRobotShapeRadius(const double robot_radius)
Robot shape must be set before initialization, either from ctor params or via this method...
int version
Definition: mrpt_jpeglib.h:898
void internal_shape_loadFromStream(mrpt::utils::CStream &in)
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
Definition: CPose2D.cpp:178
GLsizei const GLcharARB ** string
Definition: glew.h:3293
#define MRPT_LOAD_HERE_CONFIG_VAR(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
void write(const std::string &section, const std::string &name, const data_t &value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
T hypot_fast(const T x, const T y)
Faster version of std::hypot(), to use when overflow is not an issue and we prefer fast code...
virtual double evalClearanceToRobotShape(const double ox, const double oy) const MRPT_OVERRIDE
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...
Definition: CSetOfLines.h:35
void loadDefaultParams() MRPT_OVERRIDE
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.



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