MRPT  1.9.9
CRobot2NavInterfaceForSimulator.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 #pragma once
10 
14 
15 namespace mrpt::nav
16 {
17 /** CRobot2NavInterface implemented for a simulator object based on
18  * mrpt::kinematics::CVehicleSimul_Holo.
19  * Only `senseObstacles()` remains virtual for the user to implement it.
20  *
21  * \sa CReactiveNavigationSystem, CAbstractNavigator,
22  * mrpt::kinematics::CVehicleSimulVirtualBase
23  * \ingroup nav_reactive
24  */
26 {
27  private:
29  /** for getNavigationTime */
31 
32  public:
35  : m_simul(simul), m_simul_time_start(.0)
36  {
37  }
38 
40  mrpt::math::TPose2D& curPose, mrpt::math::TTwist2D& curVel,
41  mrpt::system::TTimeStamp& timestamp, mrpt::math::TPose2D& curOdometry,
42  std::string& frame_id) override
43  {
44  curPose = m_simul.getCurrentGTPose();
45  curVel = m_simul.getCurrentGTVel();
46  timestamp = mrpt::system::now();
47  curOdometry = m_simul.getCurrentOdometricPose();
48  return true; // ok
49  }
50 
51  bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd& vel_cmd) override
52  {
53  m_simul.sendVelCmd(vel_cmd);
54  return true; // ok
55  }
56 
57  bool stop(bool isEmergencyStop) override
58  {
60  0.0 /*vel*/, 0 /*dir*/, isEmergencyStop ? 0.1 : 1.0 /* ramp_time */,
61  0.0 /*rot speed */);
62  return true;
63  }
64 
66  {
68  new mrpt::kinematics::CVehicleVelCmd_Holo(0.0, 0.0, 0.1, 0.0));
69  }
70 
72  {
74  new mrpt::kinematics::CVehicleVelCmd_Holo(0.0, 0.0, 1.0, 0.0));
75  }
76 
78  const double relative_heading_radians) override
79  {
82  0.0, // vel
83  relative_heading_radians, // local_dir
84  0.5, // ramp_time
85  mrpt::signWithZero(relative_heading_radians) *
86  mrpt::DEG2RAD(40.0) // rotvel
87  ));
88  }
89 
90  /** See CRobot2NavInterface::getNavigationTime(). In this class, simulation
91  * time is returned instead of wall-clock time. */
92  double getNavigationTime() override
93  {
95  }
96  /** See CRobot2NavInterface::resetNavigationTimer() */
97  void resetNavigationTimer() override
98  {
100  }
101 };
102 
103 /** CRobot2NavInterface implemented for a simulator object based on
104  * mrpt::kinematics::CVehicleSimul_DiffDriven
105  * Only `senseObstacles()` remains virtual for the user to implement it.
106  *
107  * \sa CReactiveNavigationSystem, CAbstractNavigator,
108  * mrpt::kinematics::CVehicleSimulVirtualBase
109  * \ingroup nav_reactive
110  */
112 {
113  private:
115  /** for getNavigationTime */
117 
118  public:
121  : m_simul(simul), m_simul_time_start(.0)
122  {
123  }
124 
126  mrpt::math::TPose2D& curPose, mrpt::math::TTwist2D& curVel,
127  mrpt::system::TTimeStamp& timestamp, mrpt::math::TPose2D& curOdometry,
128  std::string& frame_id) override
129  {
130  curPose = m_simul.getCurrentGTPose();
131  curVel = m_simul.getCurrentGTVel();
132  timestamp = mrpt::system::now();
133  curOdometry = m_simul.getCurrentOdometricPose();
134  return true; // ok
135  }
136 
137  bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd& vel_cmd) override
138  {
139  m_simul.sendVelCmd(vel_cmd);
140  return true; // ok
141  }
142 
143  bool stop(bool isEmergencyStop) override
144  {
146  cmd.setToStop();
147  m_simul.sendVelCmd(cmd);
148  return true;
149  }
150 
152  {
155  cmd->setToStop();
156  return cmd;
157  }
159  {
160  return getStopCmd();
161  }
162 
163  /** See CRobot2NavInterface::getNavigationTime(). In this class, simulation
164  * time is returned instead of wall-clock time. */
165  double getNavigationTime() override
166  {
168  }
169  /** See CRobot2NavInterface::resetNavigationTimer() */
170  void resetNavigationTimer() override
171  {
173  }
174 };
175 } // namespace mrpt::nav
mrpt::kinematics::CVehicleVelCmd::Ptr getStopCmd() override
Gets the emergency stop command for the current robot.
bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd) override
Sends a velocity command to the robot.
void resetNavigationTimer() override
See CRobot2NavInterface::resetNavigationTimer()
mrpt::kinematics::CVehicleVelCmd::Ptr getEmergencyStopCmd() override
Gets the emergency stop command for the current robot.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
void resetNavigationTimer() override
See CRobot2NavInterface::resetNavigationTimer()
double getNavigationTime() override
See CRobot2NavInterface::getNavigationTime().
bool stop(bool isEmergencyStop) override
Stop the robot right now.
void sendVelRampCmd(double vel, double dir, double ramp_time, double rot_speed)
Sends a velocity cmd to the holonomic robot.
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)
Definition: TTwist2D.h:19
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
double getNavigationTime() override
See CRobot2NavInterface::getNavigationTime().
void sendVelCmd(const CVehicleVelCmd &cmd_vel) override
Sends a velocity command to the robot.
CRobot2NavInterface implemented for a simulator object based on mrpt::kinematics::CVehicleSimul_DiffD...
CRobot2NavInterfaceForSimulator_DiffDriven(mrpt::kinematics::CVehicleSimul_DiffDriven &simul)
constexpr double DEG2RAD(const double x)
Degrees to radians.
Simulates the kinematics of a differential-driven planar mobile robot/vehicle, including odometry err...
void setToStop() override
Set to a command that means "do not move" / "stop".
double getTime() const
Get the current simulation time.
mrpt::kinematics::CVehicleVelCmd::Ptr getAlignCmd(const double relative_heading_radians) override
Gets a motion command to make the robot to align with a given relative heading, without translating...
bool getCurrentPoseAndSpeeds(mrpt::math::TPose2D &curPose, mrpt::math::TTwist2D &curVel, mrpt::system::TTimeStamp &timestamp, mrpt::math::TPose2D &curOdometry, std::string &frame_id) override
Get the current pose and velocity of the robot.
bool stop(bool isEmergencyStop) override
Stop the robot right now.
Kinematic simulator of a holonomic 2D robot capable of moving in any direction, with "blended" veloci...
Lightweight 2D pose.
Definition: TPose2D.h:22
CRobot2NavInterface implemented for a simulator object based on mrpt::kinematics::CVehicleSimul_Holo...
bool getCurrentPoseAndSpeeds(mrpt::math::TPose2D &curPose, mrpt::math::TTwist2D &curVel, mrpt::system::TTimeStamp &timestamp, mrpt::math::TPose2D &curOdometry, std::string &frame_id) override
Get the current pose and velocity of the robot.
mrpt::kinematics::CVehicleVelCmd::Ptr getStopCmd() override
Gets the emergency stop command for the current robot.
std::shared_ptr< CVehicleVelCmd > Ptr
The pure virtual interface between a real or simulated robot and any CAbstractNavigator-derived class...
mrpt::kinematics::CVehicleVelCmd::Ptr getEmergencyStopCmd() override
Gets the emergency stop command for the current robot.
const mrpt::math::TPose2D & getCurrentOdometricPose() const
Returns the current pose according to (noisy) odometry.
bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd) override
Sends a velocity command to the robot.
Kinematic model for Ackermann-like or differential-driven vehicles.
CRobot2NavInterfaceForSimulator_Holo(mrpt::kinematics::CVehicleSimul_Holo &simul)
int signWithZero(T x)
Returns the sign of X as "0", "1" or "-1".
void sendVelCmd(const CVehicleVelCmd &cmd_vel) override
Sends a velocity command to the robot.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020