emc_system
Public Member Functions | Private Member Functions | Private Attributes | List of all members
emc::IO Class Reference

#include <io.h>

Public Member Functions

 IO (Communication *comm=nullptr)
 
 IO (std::string robot_name)
 
void localization_viz_send_laser_scan (double angle_min, double angle_max, double angle_inc, int subsample, std::vector< float > prediction)
 
void localization_viz_send_particles (int N, std::vector< std::vector< double >> particle_poses, double mapOrientation)
 
void localization_viz_send_pose (std::vector< double > pose, double mapOrientation)
 
bool ok ()
 Check connection to the robot. More...
 
void play (const std::string &file)
 Play a sound file out loud. The file must be placed in the ~/MRC_audio folder and must be of .mp3 or .wav format. More...
 
bool readBackBumperData (BumperData &bumper)
 Receive new BumperData from the rear bumper. More...
 
bool readFrontBumperData (BumperData &bumper)
 Receive new BumperData from the front bumper. More...
 
bool readLaserData (LaserData &scan)
 Receive new laser data if available. More...
 
bool readOdometryData (OdometryData &odom)
 Receive new odometrydata if available. More...
 
bool readPoseData (PoseData &pose)
 Receive new pose data if available. More...
 
bool resetOdometry ()
 Set the current position as (0,0,0) More...
 
void sendBaseReference (double vx, double vy, double va)
 Send a command velocity to the robot. More...
 
void sendOpendoorRequest ()
 Broadcast a request to open nearby doors. Works in simulator only. More...
 
bool sendPath (std::vector< std::vector< double >> path, std::array< double, 3 > color={0.0, 0.0, 0.0}, double width=0.02, int id=0)
 Send a path to be drawn in rviz. More...
 
bool sendPoseEstimate (double x, double y, double yaw)
 Send an estimate of the current robot pose to be shown in rviz, relative to the centre of the map (same as simulator coordinates). More...
 
void speak (const std::string &text)
 Make the robot speak the input text. This function is non-blocking. More...
 
 ~IO ()
 

Private Member Functions

bool sendPoseEstimate (double px, double py, double pz, double rr, double rp, double ry)
 Send an estimate of the current robot pose to be shown in rviz. Rotation is provided as roll pitch and yaw. More...
 
bool sendPoseEstimate (double px, double py, double pz, double rx, double ry, double rz, double rw)
 Send an estimate of the current robot pose to be shown in rviz. Rotation is provided as a quaternion. More...
 

Private Attributes

Communicationcomm_
 
bool odom_set_ = false
 
OdometryData prev_odom_
 

Detailed Description

Definition at line 18 of file io.h.

Constructor & Destructor Documentation

◆ IO() [1/2]

emc::IO::IO ( Communication comm = nullptr)

Definition at line 18 of file io.cpp.

◆ IO() [2/2]

emc::IO::IO ( std::string  robot_name)

Definition at line 24 of file io.cpp.

◆ ~IO()

emc::IO::~IO ( )

Definition at line 28 of file io.cpp.

Member Function Documentation

◆ localization_viz_send_laser_scan()

void emc::IO::localization_viz_send_laser_scan ( double  angle_min,
double  angle_max,
double  angle_inc,
int  subsample,
std::vector< float >  prediction 
)

Definition at line 199 of file io.cpp.

◆ localization_viz_send_particles()

void emc::IO::localization_viz_send_particles ( int  N,
std::vector< std::vector< double >>  particle_poses,
double  mapOrientation 
)

Definition at line 205 of file io.cpp.

◆ localization_viz_send_pose()

void emc::IO::localization_viz_send_pose ( std::vector< double >  pose,
double  mapOrientation 
)

Definition at line 211 of file io.cpp.

◆ ok()

bool emc::IO::ok ( )

Check connection to the robot.

Returns
true Connected to the robot
false Connection to the robot lost

Definition at line 119 of file io.cpp.

◆ play()

void emc::IO::play ( const std::string file)

Play a sound file out loud. The file must be placed in the ~/MRC_audio folder and must be of .mp3 or .wav format.

Parameters
filefilename, including extension

Definition at line 114 of file io.cpp.

◆ readBackBumperData()

bool emc::IO::readBackBumperData ( BumperData bumper)

Receive new BumperData from the rear bumper.

Parameters
bumperBumberdata object to write the new data to, untouched if no data is available
Returns
true if new data was available
false if not

Definition at line 84 of file io.cpp.

◆ readFrontBumperData()

bool emc::IO::readFrontBumperData ( BumperData bumper)

Receive new BumperData from the front bumper.

Parameters
bumperBumberdata object to write the new data to, untouched if no data is available
Returns
true if new data was available
false if not

Definition at line 79 of file io.cpp.

◆ readLaserData()

bool emc::IO::readLaserData ( LaserData scan)

Receive new laser data if available.

Parameters
scanreference to a LaserData object to write the new data to
Returns
true if new laser data was available
false if not

Definition at line 34 of file io.cpp.

◆ readOdometryData()

bool emc::IO::readOdometryData ( OdometryData odom)

Receive new odometrydata if available.

Parameters
[out]odomreference to an OdometryData object to write the new data to. provides displacement since last time this function or resetOdometry() was called.
Returns
true if new data was available
false if not

Definition at line 44 of file io.cpp.

◆ readPoseData()

bool emc::IO::readPoseData ( PoseData pose)

Receive new pose data if available.

Parameters
posereference to a PoseData object to write the new data to
Returns
true if new pose data was available
false if not

Definition at line 39 of file io.cpp.

◆ resetOdometry()

bool emc::IO::resetOdometry ( )

Set the current position as (0,0,0)

Returns
true if new data was available to reset, false if not

Definition at line 69 of file io.cpp.

◆ sendBaseReference()

void emc::IO::sendBaseReference ( double  vx,
double  vy,
double  va 
)

Send a command velocity to the robot.

Parameters
vx[m/s] Desired velocity in x direction (forward)
vy[m/s] Desired velocity in y direction (left)
va[rad/s] Desired rotational velocity. Turning to the left is positive

Definition at line 89 of file io.cpp.

◆ sendOpendoorRequest()

void emc::IO::sendOpendoorRequest ( )

Broadcast a request to open nearby doors. Works in simulator only.

Definition at line 94 of file io.cpp.

◆ sendPath()

bool emc::IO::sendPath ( std::vector< std::vector< double >>  path,
std::array< double, 3 >  color = {0.0, 0.0, 0.0},
double  width = 0.02,
int  id = 0 
)

Send a path to be drawn in rviz.

Parameters
pathThe sequence of points, between which the path will be drawn. The sequence must be provided as a vector of points, where each point is formatted as {x,y,z} or {x,y}. Coordinates are relative to the centre of the map (same as simulator coordinates).
colorThe color in which the path will be drawn. Format is {r,g,b}, with each value between 0.0 and 1.0.
width[m] Line width.
idThe id of this path. When drawing multiple paths, each sequence must have a unique id. Sending a new path with the same id will overwrite the previous path.
Returns
true Path is sent to rviz.
false Path does not have enough valid points.

Definition at line 124 of file io.cpp.

◆ sendPoseEstimate() [1/3]

bool emc::IO::sendPoseEstimate ( double  px,
double  py,
double  pz,
double  rr,
double  rp,
double  ry 
)
private

Send an estimate of the current robot pose to be shown in rviz. Rotation is provided as roll pitch and yaw.

Parameters
pxThe x coordinate of the position.
pyThe y coordinate of the position.
pzThe z coordinate of the position.
rrThe roll component of the rotation.
rpThe pitch component of the rotation.
ryThe yaw component of the rotation.

Definition at line 183 of file io.cpp.

◆ sendPoseEstimate() [2/3]

bool emc::IO::sendPoseEstimate ( double  px,
double  py,
double  pz,
double  rx,
double  ry,
double  rz,
double  rw 
)
private

Send an estimate of the current robot pose to be shown in rviz. Rotation is provided as a quaternion.

Parameters
pxThe x coordinate of the position.
pyThe y coordinate of the position.
pzThe z coordinate of the position.
rxThe x component of the rotation.
ryThe y component of the rotation.
rzThe z component of the rotation.
rwThe w component of the rotation.

Definition at line 172 of file io.cpp.

◆ sendPoseEstimate() [3/3]

bool emc::IO::sendPoseEstimate ( double  x,
double  y,
double  yaw 
)

Send an estimate of the current robot pose to be shown in rviz, relative to the centre of the map (same as simulator coordinates).

Parameters
xThe x coordinate
yThe y coordinate
yawThe yaw rotation

Definition at line 191 of file io.cpp.

◆ speak()

void emc::IO::speak ( const std::string text)

Make the robot speak the input text. This function is non-blocking.

Parameters
textSentence to be spoken by the robot.

Definition at line 104 of file io.cpp.

Member Data Documentation

◆ comm_

Communication* emc::IO::comm_
private

Definition at line 177 of file io.h.

◆ odom_set_

bool emc::IO::odom_set_ = false
private

Definition at line 175 of file io.h.

◆ prev_odom_

OdometryData emc::IO::prev_odom_
private

Definition at line 174 of file io.h.


The documentation for this class was generated from the following files: