Main MRPT website > C++ reference for MRPT 1.5.7
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-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 
17 
18 using namespace mrpt::nav;
19 
21  CAbstractNavigator( robot_interface )
22 {
23 }
24 
25 // Dtor:
27 {
28 }
29 
31 {
32 }
33 
35 {
36  const std::string s = "CNavigatorManualSequence";
37 
38  programmed_orders.clear();
39  mrpt::vector_string lstKeys;
40  c.getAllKeys(s, lstKeys);
41 
42  for (size_t i=0;i<lstKeys.size();i++)
43  {
44  std::string s = c.read_string(s,lstKeys[i],"",true);
45  std::vector<std::string> toks;
46  mrpt::system::tokenize(s," \t\r\n",toks);
47  ASSERTMSG_(toks.size()>2,std::string("Wrong format while parsing CNavigatorManualSequence cfg file in entry: ")+lstKeys[i]);
48 
49  const double t = atof(toks[0].c_str());
50  TVelCmd krc;
51 
52  const size_t nComps = toks.size() - 1;
53  switch (nComps)
54  {
57  default:
58  THROW_EXCEPTION("Expected 2 or 4 velocity components!");
59  };
60 
61  for (size_t i=0;i<nComps;i++)
62  krc.cmd_vel->setVelCmdElement(i, atof(toks[i+1].c_str() ));
63 
64  // insert:
65  programmed_orders[t] = krc;
66  }
67 }
68 
70 {
71  ASSERT_(!programmed_orders.empty())
73 }
74 
75 /** Overriden in this class to ignore the cancel/pause/... commands */
77 {
78  if (programmed_orders.empty())
79  return;
80 
81  const double t = m_robot.getNavigationTime();
82 
83  if (t>=programmed_orders.begin()->first)
84  {
85  const TVelCmd &krc = programmed_orders.begin()->second;
86  // Send cmd:
87  logFmt( mrpt::utils::LVL_DEBUG, "[CNavigatorManualSequence] Sending cmd: t=%f\n",programmed_orders.begin()->first);
88 
89  if (!this->changeSpeeds(*krc.cmd_vel) )
90  {
91  this->stop(true /*not emergency*/);
92  logFmt( mrpt::utils::LVL_ERROR, "[CNavigatorManualSequence] **ERROR** sending cmd to robot.");
93  return;
94  }
95  // remove:
96  programmed_orders.erase( programmed_orders.begin() );
97  }
98 }
99 
100 
virtual void saveConfigFile(mrpt::utils::CConfigFileBase &c) const MRPT_OVERRIDE
Saves all current options to a config file.
GLdouble GLdouble t
Definition: glext.h:3610
static CVehicleVelCmd_DiffDrivenPtr Create()
void navigationStep() MRPT_OVERRIDE
Overriden in this class to ignore the cancel/pause/...
virtual void resetNavigationTimer()
see getNavigationTime()
#define THROW_EXCEPTION(msg)
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:3602
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd)
Default: forward call to m_robot.changeSpeed(). Can be overriden.
This class allows loading and storing values and vectors of different types from a configuration text...
void initialize() MRPT_OVERRIDE
Must be called for loading collision grids, etc. before invoking any navigation command.
const GLubyte * c
Definition: glext.h:5590
GLsizei const GLchar ** string
Definition: glext.h:3919
std::map< double, TVelCmd > programmed_orders
map [time_in_secs_since_beginning] -> orders.
CRobot2NavInterface & m_robot
The navigator-robot interface.
mrpt::kinematics::CVehicleVelCmdPtr cmd_vel
all with the same meaning than in CRobot2NavInterface::changeSpeeds()
#define ASSERT_(f)
static CVehicleVelCmd_HoloPtr Create()
CNavigatorManualSequence(CRobot2NavInterface &react_iterf_impl)
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
std::vector< std::string > vector_string
A type for passing a vector of strings.
#define ASSERTMSG_(f, __ERROR_MSG)
This is the base class for any reactive/planned navigation system.
void BASE_IMPEXP tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) MRPT_NO_THROWS
Tokenizes a string according to a set of delimiting characters.
virtual bool stop(bool isEmergencyStop)
Default: forward call to m_robot.stop(). Can be overriden.
virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE
Loads all params from a file.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019