#include <ThirdRobotInterface.h>
Public Member Functions | |
| virtual void | calculateOdometry () |
| Calculate Third robot odometry. Call after reading encoder pulses. | |
| virtual int | closeSerialPort () |
| Close the serial port. | |
| virtual geometry_msgs::Twist | drive (double linear_speed, double angular_speed) |
| Drive. | |
| virtual geometry_msgs::Twist | driveDirect (double front_angular, double rear_speed) |
| Drive direct. | |
| virtual int | getEncoderPacket () |
| Read the encoder pulses from iMCs01. | |
| virtual int | openSerialPort () |
| Open the serial port. | |
| virtual void | resetOdometry () |
| Reset Third robot odometry. | |
| virtual void | setOdometry (double new_x, double new_y, double new_yaw) |
| Set new odometry. | |
| virtual void | setParams (double pulse_rate, double geer_rate, double wheel_diameter_right, double wheel_diameter_left, double tred_width) |
| Setting params. | |
| virtual int | setSerialPort () |
| Setting the serial port. | |
| ThirdRobotInterface (const std::string &new_serial_port_imcs01, int new_baudrate_imcs01) | |
| Constructor. | |
| virtual void | writeCmd (ccmd cmd) |
| write to iMCs01 (ccmd) | |
| ~ThirdRobotInterface () | |
| Destructor. | |
Public Attributes | |
| double | odometry_x_ |
| robot odometry x[m] | |
| double | odometry_y_ |
| robot odometry y[m] | |
| double | odometry_yaw_ |
| robot odometry yaw[rad] | |
| int | stasis_ |
| Robot running status. | |
| double | steer_angle |
| Front steer angle[deg]. | |
Protected Member Functions | |
| geometry_msgs::Twist | fixFrontAngle (double angular_diff) |
| int | parseEncoderPackets () |
| Parse data. | |
| int | parseFrontEncoderCounts () |
| int | parseRearEncoderCounts () |
Protected Attributes | |
| int | baudrate_imcs01 |
| Baudrate. | |
| struct ccmd | cmd_ccmd |
| struct uin | cmd_uin |
| For access to iMCs01. | |
| std::mutex | communication_mutex_ |
| Mutex for communication. | |
| double | delta_rear_encoder_time |
| Delta time. | |
| int | fd_imcs01 |
| File descriptor. | |
| double | GeerRate = 33.0 |
| GEER_RATE. | |
| std::string | imcs01_port_name |
| Serial port to which the robot is connected. | |
| double | last_odometry_yaw = 0.0 |
| [k-1]のyaw | |
| double | last_rear_encoder_time |
| Last time reading encoder. | |
| double | linear_velocity |
| Linear velocity. | |
| termios | newtio_imcs01 |
| termios | oldtio_imcs01 |
| Old and new termios struct. | |
| double | PulseRate = 40.0 |
| num of pulse | |
| int | runmode |
| Forward or Back mode flag. | |
| double | TredWidth = 0.595 |
| Wheel Diameter[m]. | |
Definition at line 67 of file ThirdRobotInterface.h.
| cirkit::ThirdRobotInterface::ThirdRobotInterface | ( | const std::string & | new_serial_port_imcs01, |
| int | new_baudrate_imcs01 | ||
| ) |
Constructor.
Definition at line 31 of file ThirdRobotInterface.cpp.
Destructor.
| virtual void cirkit::ThirdRobotInterface::calculateOdometry | ( | ) | [virtual] |
Calculate Third robot odometry. Call after reading encoder pulses.
| virtual int cirkit::ThirdRobotInterface::closeSerialPort | ( | ) | [virtual] |
Close the serial port.
| virtual geometry_msgs::Twist cirkit::ThirdRobotInterface::drive | ( | double | linear_speed, |
| double | angular_speed | ||
| ) | [virtual] |
Drive.
| virtual geometry_msgs::Twist cirkit::ThirdRobotInterface::driveDirect | ( | double | front_angular, |
| double | rear_speed | ||
| ) | [virtual] |
Drive direct.
| geometry_msgs::Twist cirkit::ThirdRobotInterface::fixFrontAngle | ( | double | angular_diff | ) | [protected] |
| virtual int cirkit::ThirdRobotInterface::getEncoderPacket | ( | ) | [virtual] |
Read the encoder pulses from iMCs01.
| virtual int cirkit::ThirdRobotInterface::openSerialPort | ( | ) | [virtual] |
Open the serial port.
| int cirkit::ThirdRobotInterface::parseEncoderPackets | ( | ) | [protected] |
Parse data.
Data parsing function. Parses data comming from iMCs01.
| buffer | Data to be parsed. |
| int cirkit::ThirdRobotInterface::parseFrontEncoderCounts | ( | ) | [protected] |
| int cirkit::ThirdRobotInterface::parseRearEncoderCounts | ( | ) | [protected] |
| virtual void cirkit::ThirdRobotInterface::resetOdometry | ( | ) | [virtual] |
Reset Third robot odometry.
| virtual void cirkit::ThirdRobotInterface::setOdometry | ( | double | new_x, |
| double | new_y, | ||
| double | new_yaw | ||
| ) | [virtual] |
Set new odometry.
| virtual void cirkit::ThirdRobotInterface::setParams | ( | double | pulse_rate, |
| double | geer_rate, | ||
| double | wheel_diameter_right, | ||
| double | wheel_diameter_left, | ||
| double | tred_width | ||
| ) | [virtual] |
Setting params.
| virtual int cirkit::ThirdRobotInterface::setSerialPort | ( | ) | [virtual] |
Setting the serial port.
| virtual void cirkit::ThirdRobotInterface::writeCmd | ( | ccmd | cmd | ) | [virtual] |
write to iMCs01 (ccmd)
int cirkit::ThirdRobotInterface::baudrate_imcs01 [protected] |
Baudrate.
Definition at line 130 of file ThirdRobotInterface.h.
struct ccmd cirkit::ThirdRobotInterface::cmd_ccmd [protected] |
Definition at line 124 of file ThirdRobotInterface.h.
struct uin cirkit::ThirdRobotInterface::cmd_uin [protected] |
For access to iMCs01.
Definition at line 122 of file ThirdRobotInterface.h.
std::mutex cirkit::ThirdRobotInterface::communication_mutex_ [protected] |
Mutex for communication.
Definition at line 163 of file ThirdRobotInterface.h.
double cirkit::ThirdRobotInterface::delta_rear_encoder_time [protected] |
Delta time.
Definition at line 143 of file ThirdRobotInterface.h.
int cirkit::ThirdRobotInterface::fd_imcs01 [protected] |
File descriptor.
Definition at line 128 of file ThirdRobotInterface.h.
double cirkit::ThirdRobotInterface::GeerRate = 33.0 [protected] |
GEER_RATE.
Definition at line 153 of file ThirdRobotInterface.h.
std::string cirkit::ThirdRobotInterface::imcs01_port_name [protected] |
Serial port to which the robot is connected.
Definition at line 126 of file ThirdRobotInterface.h.
double cirkit::ThirdRobotInterface::last_odometry_yaw = 0.0 [protected] |
double cirkit::ThirdRobotInterface::last_rear_encoder_time [protected] |
Last time reading encoder.
Delta rear encoder counts. 0 is right, 1 is left. Last rear encoder counts reading. For odometry calculation. 0 is right, 1 is left.
Definition at line 136 of file ThirdRobotInterface.h.
double cirkit::ThirdRobotInterface::linear_velocity [protected] |
Linear velocity.
Definition at line 159 of file ThirdRobotInterface.h.
termios cirkit::ThirdRobotInterface::newtio_imcs01 [protected] |
Definition at line 133 of file ThirdRobotInterface.h.
robot odometry x[m]
Definition at line 98 of file ThirdRobotInterface.h.
robot odometry y[m]
Definition at line 100 of file ThirdRobotInterface.h.
robot odometry yaw[rad]
Definition at line 102 of file ThirdRobotInterface.h.
termios cirkit::ThirdRobotInterface::oldtio_imcs01 [protected] |
Old and new termios struct.
Definition at line 132 of file ThirdRobotInterface.h.
double cirkit::ThirdRobotInterface::PulseRate = 40.0 [protected] |
num of pulse
Definition at line 151 of file ThirdRobotInterface.h.
int cirkit::ThirdRobotInterface::runmode [protected] |
Forward or Back mode flag.
Definition at line 161 of file ThirdRobotInterface.h.
Robot running status.
Definition at line 106 of file ThirdRobotInterface.h.
Front steer angle[deg].
Definition at line 104 of file ThirdRobotInterface.h.
double cirkit::ThirdRobotInterface::TredWidth = 0.595 [protected] |