emc_system
|
#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 | |
Communication * | comm_ |
bool | odom_set_ = false |
OdometryData | prev_odom_ |
emc::IO::IO | ( | Communication * | comm = nullptr | ) |
emc::IO::IO | ( | std::string | robot_name | ) |
void emc::IO::localization_viz_send_laser_scan | ( | double | angle_min, |
double | angle_max, | ||
double | angle_inc, | ||
int | subsample, | ||
std::vector< float > | prediction | ||
) |
void emc::IO::localization_viz_send_particles | ( | int | N, |
std::vector< std::vector< double >> | particle_poses, | ||
double | mapOrientation | ||
) |
void emc::IO::localization_viz_send_pose | ( | std::vector< double > | pose, |
double | mapOrientation | ||
) |
bool emc::IO::ok | ( | ) |
void emc::IO::play | ( | const std::string & | file | ) |
bool emc::IO::readBackBumperData | ( | BumperData & | bumper | ) |
Receive new BumperData from the rear bumper.
bumper | Bumberdata object to write the new data to, untouched if no data is available |
bool emc::IO::readFrontBumperData | ( | BumperData & | bumper | ) |
Receive new BumperData from the front bumper.
bumper | Bumberdata object to write the new data to, untouched if no data is available |
bool emc::IO::readLaserData | ( | LaserData & | scan | ) |
bool emc::IO::readOdometryData | ( | OdometryData & | odom | ) |
Receive new odometrydata if available.
[out] | odom | reference to an OdometryData object to write the new data to. provides displacement since last time this function or resetOdometry() was called. |
bool emc::IO::readPoseData | ( | PoseData & | pose | ) |
bool emc::IO::resetOdometry | ( | ) |
void emc::IO::sendBaseReference | ( | double | vx, |
double | vy, | ||
double | va | ||
) |
void emc::IO::sendOpendoorRequest | ( | ) |
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.
path | The 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). |
color | The 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. |
id | The 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. |
|
private |
Send an estimate of the current robot pose to be shown in rviz. Rotation is provided as roll pitch and yaw.
px | The x coordinate of the position. |
py | The y coordinate of the position. |
pz | The z coordinate of the position. |
rr | The roll component of the rotation. |
rp | The pitch component of the rotation. |
ry | The yaw component of the rotation. |
|
private |
Send an estimate of the current robot pose to be shown in rviz. Rotation is provided as a quaternion.
px | The x coordinate of the position. |
py | The y coordinate of the position. |
pz | The z coordinate of the position. |
rx | The x component of the rotation. |
ry | The y component of the rotation. |
rz | The z component of the rotation. |
rw | The w component of the rotation. |
bool emc::IO::sendPoseEstimate | ( | double | x, |
double | y, | ||
double | yaw | ||
) |
void emc::IO::speak | ( | const std::string & | text | ) |
|
private |
|
private |