MRPT  1.9.9
CNavigatorManualSequence.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 
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 (const auto& lstKey : lstKeys)
43  {
44  std::string str = c.read_string(s, lstKey, "", 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  lstKey);
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:
60  krc.cmd_vel = std::make_shared<
62  break;
63  case 4:
65  break;
66  default:
67  THROW_EXCEPTION("Expected 2 or 4 velocity components!");
68  };
69 
70  for (size_t j = 0; j < nComps; j++)
71  krc.cmd_vel->setVelCmdElement(j, atof(toks[j + 1].c_str()));
72 
73  // insert:
74  programmed_orders[t] = krc;
75  }
76 }
77 
79 {
80  ASSERT_(!programmed_orders.empty());
82 }
83 
84 /** Overriden in this class to ignore the cancel/pause/... commands */
86 {
87  if (programmed_orders.empty()) return;
88 
89  const double t = m_robot.getNavigationTime();
90 
91  if (t >= programmed_orders.begin()->first)
92  {
93  const TVelCmd& krc = programmed_orders.begin()->second;
94  // Send cmd:
95  logFmt(
97  "[CNavigatorManualSequence] Sending cmd: t=%f\n",
98  programmed_orders.begin()->first);
99 
100  if (!this->changeSpeeds(*krc.cmd_vel))
101  {
102  this->stop(true /*not emergency*/);
103  logFmt(
105  "[CNavigatorManualSequence] **ERROR** sending cmd to robot.");
106  return;
107  }
108  // remove:
109  programmed_orders.erase(programmed_orders.begin());
110  }
111 }
GLdouble GLdouble t
Definition: glext.h:3695
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:67
virtual void resetNavigationTimer()
see getNavigationTime()
mrpt::kinematics::CVehicleVelCmd::Ptr cmd_vel
all with the same meaning than in CRobot2NavInterface::changeSpeeds()
static Ptr Create(Args &&... args)
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:3682
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:120
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:6406
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
GLsizei const GLchar ** string
Definition: glext.h:4116
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.
CNavigatorManualSequence(CRobot2NavInterface &react_iterf_impl)
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: cb8dd5fc8 Sat Dec 7 21:55:39 2019 +0100 at sáb dic 7 22:00:13 CET 2019