Main MRPT website > C++ reference for MRPT 1.5.7
CRobot2NavInterfaceForSimulator.h
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 #pragma once
10 
14 
15 namespace mrpt
16 {
17  namespace nav
18  {
19  /** CRobot2NavInterface implemented for a simulator object based on mrpt::kinematics::CVehicleSimul_Holo.
20  * Only `senseObstacles()` remains virtual for the user to implement it.
21  *
22  * \sa CReactiveNavigationSystem, CAbstractNavigator, mrpt::kinematics::CVehicleSimulVirtualBase
23  * \ingroup nav_reactive
24  */
26  {
27  private:
29  double m_simul_time_start; //!< for getNavigationTime
30 
31  public:
32  CRobot2NavInterfaceForSimulator_Holo(mrpt::kinematics::CVehicleSimul_Holo &simul) : m_simul(simul),m_simul_time_start(.0) {}
33 
35  {
36  curPose = m_simul.getCurrentGTPose();
37  curVel = m_simul.getCurrentGTVel();
38  timestamp = mrpt::system::now();
39  curOdometry = m_simul.getCurrentOdometricPose();
40  return true; // ok
41  }
42 
44  {
45  m_simul.sendVelCmd(vel_cmd);
46  return true; // ok
47  }
48 
49  bool stop(bool isEmergencyStop) MRPT_OVERRIDE
50  {
51  m_simul.sendVelRampCmd(0.0 /*vel*/, 0 /*dir*/, isEmergencyStop ? 0.1 : 1.0 /* ramp_time */, 0.0 /*rot speed */);
52  return true;
53  }
54 
55  mrpt::kinematics::CVehicleVelCmdPtr getEmergencyStopCmd() MRPT_OVERRIDE
56  {
57  return mrpt::kinematics::CVehicleVelCmdPtr(new mrpt::kinematics::CVehicleVelCmd_Holo(0.0,0.0,0.1,0.0));
58  }
59 
60  mrpt::kinematics::CVehicleVelCmdPtr getStopCmd() MRPT_OVERRIDE
61  {
62  return mrpt::kinematics::CVehicleVelCmdPtr(new mrpt::kinematics::CVehicleVelCmd_Holo(0.0,0.0,1.0,0.0));
63  }
64 
65  mrpt::kinematics::CVehicleVelCmdPtr getAlignCmd(const double relative_heading_radians) MRPT_OVERRIDE
66  {
67  return mrpt::kinematics::CVehicleVelCmdPtr(new mrpt::kinematics::CVehicleVelCmd_Holo(
68  0.0, // vel
69  relative_heading_radians, // local_dir
70  0.5, // ramp_time
71  mrpt::utils::signWithZero(relative_heading_radians) * mrpt::utils::DEG2RAD(40.0) // rotvel
72  ));
73  }
74 
75  /** See CRobot2NavInterface::getNavigationTime(). In this class, simulation time is returned instead of wall-clock time. */
77  return m_simul.getTime()-m_simul_time_start;
78  }
79  /** See CRobot2NavInterface::resetNavigationTimer() */
81  m_simul_time_start = m_simul.getTime();
82  }
83  };
84 
85 
86  /** CRobot2NavInterface implemented for a simulator object based on mrpt::kinematics::CVehicleSimul_DiffDriven
87  * Only `senseObstacles()` remains virtual for the user to implement it.
88  *
89  * \sa CReactiveNavigationSystem, CAbstractNavigator, mrpt::kinematics::CVehicleSimulVirtualBase
90  * \ingroup nav_reactive
91  */
93  {
94  private:
96  double m_simul_time_start; //!< for getNavigationTime
97 
98  public:
100 
102  {
103  curPose = m_simul.getCurrentGTPose();
104  curVel = m_simul.getCurrentGTVel();
105  timestamp = mrpt::system::now();
106  curOdometry = m_simul.getCurrentOdometricPose();
107  return true; // ok
108  }
109 
111  {
112  m_simul.sendVelCmd(vel_cmd);
113  return true; // ok
114  }
115 
116  bool stop(bool isEmergencyStop) MRPT_OVERRIDE
117  {
119  cmd.setToStop();
120  m_simul.sendVelCmd(cmd);
121  return true;
122  }
123 
124 
125  mrpt::kinematics::CVehicleVelCmdPtr getStopCmd() MRPT_OVERRIDE
126  {
127  mrpt::kinematics::CVehicleVelCmdPtr cmd(new mrpt::kinematics::CVehicleVelCmd_DiffDriven());
128  cmd->setToStop();
129  return cmd;
130  }
131  mrpt::kinematics::CVehicleVelCmdPtr getEmergencyStopCmd() MRPT_OVERRIDE
132  {
133  return getStopCmd();
134  }
135 
136  /** See CRobot2NavInterface::getNavigationTime(). In this class, simulation time is returned instead of wall-clock time. */
138  return m_simul.getTime()-m_simul_time_start;
139  }
140  /** See CRobot2NavInterface::resetNavigationTimer() */
142  m_simul_time_start = m_simul.getTime();
143  }
144  };
145 
146  }
147 }
148 
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
int signWithZero(T x)
Returns the sign of X as "0", "1" or "-1".
mrpt::kinematics::CVehicleVelCmdPtr getEmergencyStopCmd() MRPT_OVERRIDE
Gets the emergency stop command for the current robot.
mrpt::kinematics::CVehicleVelCmdPtr getAlignCmd(const double relative_heading_radians) MRPT_OVERRIDE
Gets a motion command to make the robot to align with a given relative heading, without translating...
void resetNavigationTimer() MRPT_OVERRIDE
See CRobot2NavInterface::resetNavigationTimer()
bool stop(bool isEmergencyStop) MRPT_OVERRIDE
Stop the robot right now.
bool stop(bool isEmergencyStop) MRPT_OVERRIDE
Stop the robot right now.
double DEG2RAD(const double x)
Degrees to radians.
void resetNavigationTimer() MRPT_OVERRIDE
See CRobot2NavInterface::resetNavigationTimer()
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
void sendVelRampCmd(double vel, double dir, double ramp_time, double rot_speed)
Sends a velocity cmd to the holonomic robot.
double getNavigationTime() MRPT_OVERRIDE
See CRobot2NavInterface::getNavigationTime().
const mrpt::math::TTwist2D & getCurrentGTVel() const
Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in world coordinates.
const mrpt::math::TPose2D & getCurrentGTPose() const
Returns the instantaneous, ground truth pose in world coordinates.
Virtual base for velocity commands of different kinematic models of planar mobile robot...
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
CRobot2NavInterface implemented for a simulator object based on mrpt::kinematics::CVehicleSimul_DiffD...
CRobot2NavInterfaceForSimulator_DiffDriven(mrpt::kinematics::CVehicleSimul_DiffDriven &simul)
void sendVelCmd(const CVehicleVelCmd &cmd_vel) MRPT_OVERRIDE
Sends a velocity command to the robot.
Simulates the kinematics of a differential-driven planar mobile robot/vehicle, including odometry err...
double getTime() const
Get the current simulation time.
double getNavigationTime() MRPT_OVERRIDE
See CRobot2NavInterface::getNavigationTime().
GLsizei const GLchar ** string
Definition: glext.h:3919
virtual bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd) MRPT_OVERRIDE
Sends a velocity command to the robot.
void setToStop() MRPT_OVERRIDE
Set to a command that means "do not move" / "stop".
mrpt::kinematics::CVehicleVelCmdPtr getStopCmd() MRPT_OVERRIDE
Gets the emergency stop command for the current robot.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Kinematic simulator of a holonomic 2D robot capable of moving in any direction, with "blended" veloci...
Lightweight 2D pose.
bool getCurrentPoseAndSpeeds(mrpt::math::TPose2D &curPose, mrpt::math::TTwist2D &curVel, mrpt::system::TTimeStamp &timestamp, mrpt::math::TPose2D &curOdometry, std::string &frame_id) MRPT_OVERRIDE
Get the current pose and velocity of the robot.
CRobot2NavInterface implemented for a simulator object based on mrpt::kinematics::CVehicleSimul_Holo...
mrpt::kinematics::CVehicleVelCmdPtr getEmergencyStopCmd() MRPT_OVERRIDE
Gets the emergency stop command for the current robot.
bool getCurrentPoseAndSpeeds(mrpt::math::TPose2D &curPose, mrpt::math::TTwist2D &curVel, mrpt::system::TTimeStamp &timestamp, mrpt::math::TPose2D &curOdometry, std::string &frame_id) MRPT_OVERRIDE
Get the current pose and velocity of the robot.
void sendVelCmd(const CVehicleVelCmd &cmd_vel) MRPT_OVERRIDE
Sends a velocity command to the robot.
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
mrpt::kinematics::CVehicleVelCmdPtr getStopCmd() MRPT_OVERRIDE
Gets the emergency stop command for the current robot.
const mrpt::math::TPose2D & getCurrentOdometricPose() const
Returns the current pose according to (noisy) odometry.
Kinematic model for Ackermann-like or differential-driven vehicles.
CRobot2NavInterfaceForSimulator_Holo(mrpt::kinematics::CVehicleSimul_Holo &simul)
bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd) MRPT_OVERRIDE
Sends a velocity command to the robot.



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