Main MRPT website > C++ reference for MRPT 1.9.9
CNavigatorManualSequence.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-2018, 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 
17 
18 using namespace mrpt::nav;
19 
21  CRobot2NavInterface& robot_interface)
22  : CAbstractNavigator(robot_interface)
23 {
24 }
25 
26 // Dtor:
30 {
31 }
32 
35 {
36  const std::string s = "CNavigatorManualSequence";
37 
38  programmed_orders.clear();
39  std::vector<std::string> lstKeys;
40  c.getAllKeys(s, lstKeys);
41 
42  for (size_t i = 0; i < lstKeys.size(); i++)
43  {
44  std::string str = c.read_string(s, lstKeys[i], "", true);
45  std::vector<std::string> toks;
46  mrpt::system::tokenize(str, " \t\r\n", toks);
47  ASSERTMSG_(
48  toks.size() > 2,
49  std::string("Wrong format while parsing CNavigatorManualSequence "
50  "cfg file in entry: ") +
51  lstKeys[i]);
52 
53  const double t = atof(toks[0].c_str());
54  TVelCmd krc;
55 
56  const size_t nComps = toks.size() - 1;
57  switch (nComps)
58  {
59  case 2:
62  break;
63  case 4:
66  break;
67  default:
68  THROW_EXCEPTION("Expected 2 or 4 velocity components!");
69  };
70 
71  for (size_t j = 0; j < nComps; j++)
72  krc.cmd_vel->setVelCmdElement(j, atof(toks[j + 1].c_str()));
73 
74  // insert:
75  programmed_orders[t] = krc;
76  }
77 }
78 
80 {
81  ASSERT_(!programmed_orders.empty());
83 }
84 
85 /** Overriden in this class to ignore the cancel/pause/... commands */
87 {
88  if (programmed_orders.empty()) return;
89 
90  const double t = m_robot.getNavigationTime();
91 
92  if (t >= programmed_orders.begin()->first)
93  {
94  const TVelCmd& krc = programmed_orders.begin()->second;
95  // Send cmd:
96  logFmt(
98  "[CNavigatorManualSequence] Sending cmd: t=%f\n",
99  programmed_orders.begin()->first);
100 
101  if (!this->changeSpeeds(*krc.cmd_vel))
102  {
103  this->stop(true /*not emergency*/);
104  logFmt(
106  "[CNavigatorManualSequence] **ERROR** sending cmd to robot.");
107  return;
108  }
109  // remove:
110  programmed_orders.erase(programmed_orders.begin());
111  }
112 }
GLdouble GLdouble t
Definition: glext.h:3689
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
virtual void resetNavigationTimer()
see getNavigationTime()
mrpt::kinematics::CVehicleVelCmd::Ptr cmd_vel
all with the same meaning than in CRobot2NavInterface::changeSpeeds()
virtual double getNavigationTime()
Returns the number of seconds ellapsed since the constructor of this class was invoked, or since the last call of resetNavigationTimer().
GLdouble s
Definition: glext.h:3676
void tokenize(const std::string &inString, const std::string &inDelimiters, OUT_CONTAINER &outTokens, bool skipBlankTokens=true) noexcept
Tokenizes a string according to a set of delimiting characters.
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed().
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This class allows loading and storing values and vectors of different types from a configuration text...
void navigationStep() override
Overriden in this class to ignore the cancel/pause/...
const GLubyte * c
Definition: glext.h:6313
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
GLsizei const GLchar ** string
Definition: glext.h:4101
virtual void saveConfigFile(mrpt::config::CConfigFileBase &c) const override
Saves all current options to a config file.
CRobot2NavInterface & m_robot
The navigator-robot interface.
void initialize() override
Must be called for loading collision grids, etc.
std::shared_ptr< T > make_aligned_shared(Args &&... args)
Creates a shared_ptr with aligned memory via aligned_allocator_cpp11<>.
CNavigatorManualSequence(CRobot2NavInterface &react_iterf_impl)
virtual void loadConfigFile(const mrpt::config::CConfigFileBase &c) override
Loads all params from a file.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
This is the base class for any reactive/planned navigation system.
Kinematic model for Ackermann-like or differential-driven vehicles.
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop().
std::map< double, TVelCmd > programmed_orders
map [time_in_secs_since_beginning] -> orders.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019