diff --git a/.gitignore b/.gitignore index 9247c7f..106f8bc 100644 --- a/.gitignore +++ b/.gitignore @@ -7,7 +7,6 @@ *.pyc *.pyo *.zip -*.cc */files/* */tmp/* *.hwm* diff --git a/examples/segwayrmp_example.cc b/examples/segwayrmp_example.cc new file mode 100644 index 0000000..a9fa474 --- /dev/null +++ b/examples/segwayrmp_example.cc @@ -0,0 +1,106 @@ +#include +#include +#include + +#include "segwayrmp/segwayrmp.h" + +void handleSegwayStatus(segwayrmp::SegwayStatus::Ptr &ss) { + std::cout << ss->str() << std::endl << std::endl; +} + +int run_segway(segwayrmp::InterfaceType interface_type, std::string configuration, int config_type = 0) { + std::cout << "!!!!!!!!!!!!WARNING!!!!!!!!!!!!" << std::endl; + std::cout << "This example moves the segway!!" << std::endl; + std::cout << "(If you continue, ctrl-c quits)" << std::endl; + std::cout << "Do you want to continue? [No/yes] "; + std::string response; + std::getline(std::cin, response); + std::transform(response.begin(), response.end(), + response.begin(), ::tolower); + if (response != std::string("yes") and response != std::string("y")) { + std::cout << "Aborting." << std::endl; + return 1; + } + try { + // segwayrmp::SegwayRMP rmp(interface_type, segwayrmp::rmp100); + segwayrmp::SegwayRMP rmp(interface_type); + if (interface_type == segwayrmp::serial) { + rmp.configureSerial(configuration); + } else if (interface_type == segwayrmp::usb) { + switch (config_type) { + case 1: + rmp.configureUSBBySerial(configuration); + break; + case 2: + rmp.configureUSBByDescription(configuration); + break; + case 3: + rmp.configureUSBByIndex(atoi(configuration.c_str())); + break; + case 0: + default: + std::cout << "Invalid interface type provided." + << std::endl; + return 0; + } + } + rmp.setStatusCallback(handleSegwayStatus); + rmp.connect(); + rmp.setOperationalMode(segwayrmp::balanced); + while(true) { + rmp.move(0.1, 0); + usleep(100000); + } + } catch (std::exception &e) { + std::cerr << "Error: " << e.what() << std::endl; + } + return 0; +} + +void print_usage() { + std::cout << "Usage: " << std::endl; + std::cout << " segwayrmp_example usb <\"00000056\" | \"Robotic Mobile Platform\" | 0>" + << std::endl; + std::cout << " or" << std::endl; + std::cout << " segwayrmp_example serial " << std::endl; + std::cout << "Examples:" << std::endl; + std::cout << " segwayrmp_example usb index 0" << std::endl; + std::cout << " segwayrmp_example usb serial_number \"00000056\"" + << std::endl; + std::cout << " segwayrmp_example serial /dev/ttyUSB0" << std::endl; + std::cout << " segwayrmp_example serial COM0" << std::endl; +} + +int main(int argc, char *argv[]) { + if (argc < 2) { + print_usage(); + return 0; + } + if (std::strcmp(argv[1], "serial") == 0) { + if (argc < 3) { + print_usage(); + return 0; + } + run_segway(segwayrmp::serial, std::string(argv[2])); + } else if (std::strcmp(argv[1], "usb") == 0) { + if (argc < 4) { + print_usage(); + return 0; + } + + if (std::strcmp(argv[2], "serial_number") == 0) { + run_segway(segwayrmp::usb, std::string(argv[3]), 1); + } else if (std::strcmp(argv[2], "description") == 0) { + run_segway(segwayrmp::usb, std::string(argv[3]), 2); + } else if (std::strcmp(argv[2], "index") == 0) { + run_segway(segwayrmp::usb, std::string(argv[3]), 3); + } else { + print_usage(); + return 0; + } + } else { + print_usage(); + return 0; + } +} diff --git a/manifest.xml b/manifest.xml new file mode 100644 index 0000000..b3cfb0a --- /dev/null +++ b/manifest.xml @@ -0,0 +1,15 @@ + + + + libsegwayrmp + + + William Woodall + BSD + + http://ros.org/wiki/libsegwayrmp + + + + + diff --git a/segway.sublime-project b/segway.sublime-project index f21c926..d19fda8 100644 --- a/segway.sublime-project +++ b/segway.sublime-project @@ -15,8 +15,8 @@ { "sublimeclang_options": [ - "/usr/include", - "/usr/local/include", + "-I/usr/include", + "-I/usr/local/include", "-I${folder:${project_path:segway.sublime-project}}/include", "-I${folder:${project_path:segway.sublime-project}}/ftd2xx/include", "-DSEGWAYRMP_USE_SERIAL", diff --git a/src/impl/rmp_ftd2xx.cc b/src/impl/rmp_ftd2xx.cc new file mode 100644 index 0000000..621c521 --- /dev/null +++ b/src/impl/rmp_ftd2xx.cc @@ -0,0 +1,373 @@ +#include "segwayrmp/segwayrmp.h" +#include "segwayrmp/impl/rmp_ftd2xx.h" + +#include + +using namespace segwayrmp; + +static const bool ftd2xx_devices_debug = false; + +inline std::string +getErrorMessageByFT_STATUS(FT_STATUS result, std::string what) +{ + std::stringstream msg; + msg << "FTD2XX error while " << what << ": "; + switch (result) { + case FT_INVALID_HANDLE: + msg << "FT_INVALID_HANDLE"; + break; + case FT_DEVICE_NOT_FOUND: + msg << "FT_DEVICE_NOT_FOUND"; + break; + case FT_DEVICE_NOT_OPENED: + msg << "FT_DEVICE_NOT_OPENED"; + break; + case FT_IO_ERROR: + msg << "FT_IO_ERROR"; + break; + case FT_INSUFFICIENT_RESOURCES: + msg << "FT_INSUFFICIENT_RESOURCES"; + break; + case FT_INVALID_PARAMETER: + msg << "FT_INVALID_PARAMETER"; + break; + case FT_INVALID_BAUD_RATE: + msg << "FT_INVALID_BAUD_RATE"; + break; + case FT_DEVICE_NOT_OPENED_FOR_ERASE: + msg << "FT_DEVICE_NOT_OPENED_FOR_ERASE"; + break; + case FT_DEVICE_NOT_OPENED_FOR_WRITE: + msg << "FT_DEVICE_NOT_OPENED_FOR_WRITE"; + break; + case FT_FAILED_TO_WRITE_DEVICE: + msg << "FT_FAILED_TO_WRITE_DEVICE"; + break; + case FT_EEPROM_READ_FAILED: + msg << "FT_EEPROM_READ_FAILED"; + break; + case FT_EEPROM_WRITE_FAILED: + msg << "FT_EEPROM_WRITE_FAILED"; + break; + case FT_EEPROM_ERASE_FAILED: + msg << "FT_EEPROM_ERASE_FAILED"; + break; + case FT_EEPROM_NOT_PRESENT: + msg << "FT_EEPROM_NOT_PRESENT"; + break; + case FT_EEPROM_NOT_PROGRAMMED: + msg << "FT_EEPROM_NOT_PROGRAMMED"; + break; + case FT_INVALID_ARGS: + msg << "FT_INVALID_ARGS"; + break; + case FT_NOT_SUPPORTED: + msg << "FT_NOT_SUPPORTED"; + break; + case FT_OTHER_ERROR: + msg << "FT_OTHER_ERROR"; + break; + default: + msg << "Unknown FTD2XX Error."; + break; + } + return msg.str(); +} + +std::vector segwayrmp::enumerateUSBDevices() { + FT_STATUS result; + FT_DEVICE_LIST_INFO_NODE *device_info; + DWORD number_of_devices; + + // If mac or linux you must set VID/PID + #ifndef _WIN32 + DWORD FTDI_VID = 0x403; + DWORD SEGWAY_PID = 0xe729; + + try { + result = FT_SetVIDPID(FTDI_VID, SEGWAY_PID); + } catch(std::exception &e) { + RMP_THROW_MSG(ReadFailedException, e.what()); + } + if (result != FT_OK) { + RMP_THROW_MSG(ReadFailedException, + getErrorMessageByFT_STATUS(result, "setting vid and pid").c_str()); + } + #endif + + try { + result = FT_CreateDeviceInfoList(&number_of_devices); + } catch(std::exception &e) { + RMP_THROW_MSG(ReadFailedException, e.what()); + } + if (result != FT_OK) { + RMP_THROW_MSG(ReadFailedException, + getErrorMessageByFT_STATUS(result, "enumerating devices").c_str()); + } + + if (ftd2xx_devices_debug) + std::cout << "Number of devices is: " << number_of_devices << std::endl; + + std::vector devices; + if (number_of_devices > 0) { + device_info = (FT_DEVICE_LIST_INFO_NODE*)malloc( + sizeof(FT_DEVICE_LIST_INFO_NODE)*number_of_devices + ); + try { + result = FT_GetDeviceInfoList(device_info, &number_of_devices); + } catch(std::exception &e) { + RMP_THROW_MSG(ReadFailedException, e.what()); + } + if (result != FT_OK) { + RMP_THROW_MSG(ReadFailedException, + getErrorMessageByFT_STATUS(result, "enumerating devices").c_str()); + } + for (int i = 0; i < number_of_devices; i++) { + devices.push_back(device_info[i]); + if (ftd2xx_devices_debug) { + printf("Dev %d:\n", i); + printf(" Flags=0x%x\n", device_info[i].Flags); + printf(" Type=0x%x\n", device_info[i].Type); + printf(" ID=0x%x\n", device_info[i].ID); + printf(" LocId=0x%x\n", device_info[i].LocId); + printf(" SerialNumber=%s\n", device_info[i].SerialNumber); + printf(" Description=%s\n", device_info[i].Description); + } + } + } + return devices; +} + +///////////////////////////////////////////////////////////////////////////// +// FTD2XXRMPIO + +FTD2XXRMPIO::FTD2XXRMPIO() +: configured(false), config_type(by_none), baudrate(460800), is_open(false) +{ + this->port_serial_number = ""; + this->port_description = ""; + this->port_index = 0; + this->connected = false; + this->usb_port_handle = NULL; +} + +FTD2XXRMPIO::~FTD2XXRMPIO() { + this->disconnect(); +} + +void FTD2XXRMPIO::connect() { + if(!this->configured) { + RMP_THROW_MSG(ConnectionFailedException, "The usb port must be " + "configured before connecting."); + } + + FT_STATUS result; + + this->enumerateUSBDevices_(); + + // Select connection method and open + switch (this->config_type) { + case by_serial_number: + this->connectBySerial(); + break; + case by_description: + this->connectByDescription(); + break; + case by_index: + this->connectByIndex(); + break; + case by_none: + default: + RMP_THROW_MSG(ConnectionFailedException, "The usb port must be " + "configured before connecting."); + break; + } + + this->is_open = true; + + // Configure the Baudrate + try { + result = FT_SetBaudRate(this->usb_port_handle, this->baudrate); + } catch(std::exception &e) { + RMP_THROW_MSG(ConnectionFailedException, e.what()); + } + if (result != FT_OK) { + RMP_THROW_MSG(ConnectionFailedException, + getErrorMessageByFT_STATUS(result, "setting the baudrate").c_str()); + } + + // Set default timeouts + try { + // 1 sec read and 1 sec write + result = FT_SetTimeouts(this->usb_port_handle, 1000, 1000); + } catch(std::exception &e) { + RMP_THROW_MSG(ConnectionFailedException, e.what()); + } + if (result != FT_OK) { + RMP_THROW_MSG(ConnectionFailedException, + getErrorMessageByFT_STATUS(result, "setting timeouts").c_str()); + } + + // Set Latency Timer + try { + result = FT_SetLatencyTimer(this->usb_port_handle, 1); + } catch(std::exception &e) { + RMP_THROW_MSG(ConnectionFailedException, e.what()); + } + if (result != FT_OK) { + RMP_THROW_MSG(ConnectionFailedException, + getErrorMessageByFT_STATUS(result, "setting latency timer").c_str()); + } + + // Set flowcontrol + try { + result = FT_SetFlowControl(this->usb_port_handle, FT_FLOW_NONE, 0, 0); + } catch(std::exception &e) { + RMP_THROW_MSG(ConnectionFailedException, e.what()); + } + if (result != FT_OK) { + RMP_THROW_MSG(ConnectionFailedException, + getErrorMessageByFT_STATUS(result, "setting flowcontrol").c_str()); + } + + // Purge the I/O buffers of the usb device + try { + result = FT_Purge(this->usb_port_handle, FT_PURGE_RX | FT_PURGE_TX); + } catch(std::exception &e) { + RMP_THROW_MSG(ConnectionFailedException, e.what()); + } + if (result != FT_OK) { + RMP_THROW_MSG(ConnectionFailedException, + getErrorMessageByFT_STATUS(result, "purging the buffers").c_str()); + } + + this->connected = true; +} + +void FTD2XXRMPIO::disconnect() { + if(this->connected) { + if (this->is_open) { + FT_Close(this->usb_port_handle); + } + this->connected = false; + } +} + +int FTD2XXRMPIO::read(unsigned char* buffer, int size) { + FT_STATUS result; + DWORD bytes_read; + + try { + result = FT_Read(this->usb_port_handle, buffer, size, &bytes_read); + } catch(std::exception &e) { + RMP_THROW_MSG(ReadFailedException, e.what()); + } + if (result != FT_OK) { + RMP_THROW_MSG(ReadFailedException, + getErrorMessageByFT_STATUS(result, "reading").c_str()); + } + + return bytes_read; +} + +int FTD2XXRMPIO::write(unsigned char* buffer, int size) { + FT_STATUS result; + DWORD bytes_written; + + try { + result = FT_Write(this->usb_port_handle, buffer, size, &bytes_written); + } catch(std::exception &e) { + RMP_THROW_MSG(ReadFailedException, e.what()); + } + if (result != FT_OK) { + RMP_THROW_MSG(ReadFailedException, + getErrorMessageByFT_STATUS(result, "reading").c_str()); + } + + return bytes_written; +} + +std::vector FTD2XXRMPIO::enumerateUSBDevices_() { + return enumerateUSBDevices(); +} + +void +FTD2XXRMPIO::configureUSBBySerial(std::string serial_number, int baudrate) +{ + this->config_type = by_serial_number; + this->port_serial_number = serial_number; + this->configured = true; + this->baudrate = baudrate; +} + +void FTD2XXRMPIO::connectBySerial() { + FT_STATUS result; + DWORD number_of_devices; + + try { + // Open the usb port + result = FT_OpenEx((PVOID)this->port_serial_number.c_str(), + (DWORD)FT_OPEN_BY_SERIAL_NUMBER, + &(this->usb_port_handle)); + } catch(std::exception &e) { + RMP_THROW_MSG(ConnectionFailedException, e.what()); + } + if (this->usb_port_handle == NULL) { + // Failed to open port + RMP_THROW_MSG(ConnectionFailedException, + getErrorMessageByFT_STATUS(result, "opening the usb port").c_str()); + } +} + +void +FTD2XXRMPIO::configureUSBByDescription(std::string description, int baudrate) +{ + this->config_type = by_description; + this->port_description = description; + this->configured = true; + this->baudrate = baudrate; +} + +void FTD2XXRMPIO::connectByDescription() { + FT_STATUS result; + + try { + // Open the usb port + result = FT_OpenEx(const_cast(this->port_description.c_str()), + FT_OPEN_BY_DESCRIPTION, + &(this->usb_port_handle)); + } catch(std::exception &e) { + RMP_THROW_MSG(ConnectionFailedException, e.what()); + } + if (result != FT_OK) { + // Failed to open port + RMP_THROW_MSG(ConnectionFailedException, + getErrorMessageByFT_STATUS(result, "opening the usb port").c_str()); + } +} + +void +FTD2XXRMPIO::configureUSBByIndex(unsigned int device_index, int baudrate) +{ + this->config_type = by_index; + this->port_index = device_index; + this->configured = true; + this->baudrate = baudrate; +} + +void FTD2XXRMPIO::connectByIndex() { + FT_STATUS result; + + try { + // Open the usb port + result = FT_Open(this->port_index, &(this->usb_port_handle)); + } catch(std::exception &e) { + RMP_THROW_MSG(ConnectionFailedException, e.what()); + } + if (this->usb_port_handle == NULL) { + // Failed to open port + RMP_THROW_MSG(ConnectionFailedException, + getErrorMessageByFT_STATUS(result, "opening the usb port").c_str()); + } +} + diff --git a/src/impl/rmp_io.cc b/src/impl/rmp_io.cc new file mode 100755 index 0000000..77095ac --- /dev/null +++ b/src/impl/rmp_io.cc @@ -0,0 +1,154 @@ +#include "segwayrmp/segwayrmp.h" +#include "segwayrmp/impl/rmp_io.h" + +inline void printHex(char * data, int length) { + for(int i = 0; i < length; ++i) { + printf("0x%.2X ", (unsigned)(unsigned char)data[i]); + } + printf("\n"); +} + +using namespace segwayrmp; + +static unsigned int BUFFER_SIZE = 256; + +///////////////////////////////////////////////////////////////////////////// +// RMPIO + +void RMPIO::getPacket(Packet &packet) { + if(!this->connected) + RMP_THROW_MSG_AND_ID(PacketRetrievalException, "Not connected.", 1); + + unsigned char usb_packet[18]; + bool packet_complete = false; + int packet_index = 0; + + while(!packet_complete) { + // Top the buffer off + size_t prev_size = this->data_buffer.size(); + if(prev_size < 18) { + this->fillBuffer(); + // Ensure that data was read into the buffer + if(prev_size == this->data_buffer.size()) { + RMP_THROW_MSG_AND_ID(PacketRetrievalException, "No data received " + "from Segway.", 3); + } + } + + // If looking for start of packet and start of packet + if(packet_index == 0 && this->data_buffer[0] == 0xF0) { + // Put the 0xF0 in the packet + usb_packet[packet_index] = this->data_buffer[0]; + // Remove the 0xF0 from the buffer + this->data_buffer.erase(this->data_buffer.begin()); + // Look for next packet byte + packet_index += 1; + } else if(packet_index == 0) { // If we were looking for the first byte, but didn't find it + // Remove the invalid byte from the buffer + this->data_buffer.erase(this->data_buffer.begin()); + } + + // If looking for second byte of packet and second byte of packet + if(packet_index == 1 && this->data_buffer[0] == 0x55) { + // Put the 0x55 in the packet + usb_packet[packet_index] = this->data_buffer[0]; + // Remove the 0x55 from the buffer + this->data_buffer.erase(this->data_buffer.begin()); + // Look for next packet byte + packet_index += 1; + } else if(packet_index == 1) { // Else were looking for second byte but didn't find it + // Reset the packet index to start search for packet over + packet_index = 0; + } + + // If looking for channel byte and channel A or B + if(packet_index == 2 && (this->data_buffer[0] == 0xAA || this->data_buffer[0] == 0xBB)) { + // Put the channel in the packet + usb_packet[packet_index] = this->data_buffer[0]; + // Remove the channel from the buffer + this->data_buffer.erase(this->data_buffer.begin()); + // Look for next packet byte + packet_index += 1; + } else if(packet_index == 2) { // Else were looking for channel byte but didn't find it + // Reset the packet index to start search for packet over + packet_index = 0; + } + + // If packet_index >= 3 then we just need to collect the rest of the bytes + // (we assume that if the previous three bytes were recieved then this is a valid packet, + // if it isn't the checksum will fail) + if(packet_index >= 3) { + // Put the next btye in the packet + usb_packet[packet_index] = this->data_buffer[0]; + // Remove the byte from the buffer + this->data_buffer.erase(this->data_buffer.begin()); + // Look for next packet byte + packet_index += 1; + } + + // If packet_index is 18 then we have a full packet + if(packet_index == 18) + packet_complete = true; + } + + // Check the Checksum + if(usb_packet[17] != this->computeChecksum(usb_packet)) { + RMP_THROW_MSG_AND_ID(PacketRetrievalException, "Checksum mismatch.", 2); + } + + // Convert to the packet type + packet.channel = usb_packet[2]; + packet.id = ((usb_packet[4] << 3) | ((usb_packet[5] >> 5) & 7)) & 0x0fff; + for (int i = 0; i < 8; i++) { + packet.data[i] = usb_packet[i + 9]; + } + + return; +} + +void RMPIO::sendPacket(Packet &packet) { + unsigned char usb_packet[18] = {0xF0, 0x55, 0x00, 0x00, 0x00, 0x00, 0x04, + 0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00}; + // Set the id + usb_packet[6] = (packet.id & 0xFF00) >> 8; + usb_packet[7] = packet.id & 0x00FF; + // Set the desitnation channel, 0x01 for 0xAA and 0x02 for 0xBB + usb_packet[2] = packet.channel; + // Copy movement and configuration commands + for(int i = 0; i < 8; ++i) { + usb_packet[9+i] = packet.data[i]; + } + // Compute and set the checksum + usb_packet[17] = this->computeChecksum(usb_packet); + + // Write the data + this->write(usb_packet, 18); +} + +void RMPIO::fillBuffer() { + unsigned char buffer[BUFFER_SIZE]; + // Read up to BUFFER_SIZE what ever is needed to fill the vector + // to BUFFER_SIZE + int bytes_read = this->read(buffer, BUFFER_SIZE-this->data_buffer.size()); + // Append the buffered data to the vector + this->data_buffer.insert(this->data_buffer.end(), buffer, buffer+bytes_read); +} + +unsigned char RMPIO::computeChecksum(unsigned char* usb_packet) { + unsigned short checksum = 0; + unsigned short checksum_hi = 0; + + for(int i = 0; i < 17; i++) { + checksum += (short)usb_packet[i]; + } + + checksum_hi = (unsigned short)(checksum >> 8); + checksum &= 0xff; + checksum += checksum_hi; + checksum_hi = (unsigned short)(checksum >> 8); + checksum &= 0xff; + checksum += checksum_hi; + checksum = (~checksum + 1) & 0xff; + return (unsigned char)checksum; +} diff --git a/src/impl/rmp_serial.cc b/src/impl/rmp_serial.cc new file mode 100644 index 0000000..9443614 --- /dev/null +++ b/src/impl/rmp_serial.cc @@ -0,0 +1,55 @@ +#include "segwayrmp/segwayrmp.h" +#include "segwayrmp/impl/rmp_serial.h" + +using namespace segwayrmp; + +///////////////////////////////////////////////////////////////////////////// +// SerialRMPIO + +SerialRMPIO::SerialRMPIO() : configured(false), baudrate(460800), port("") { + this->connected = false; +} + +SerialRMPIO::~SerialRMPIO() { + this->disconnect(); +} + +void SerialRMPIO::configure(std::string port, int baudrate) { + this->port = port; + this->baudrate = baudrate; + this->configured = true; +} + +void SerialRMPIO::connect() { + if(!this->configured) { + RMP_THROW_MSG(ConnectionFailedException, "The serial port must be " + "configured before connecting."); + } + try { + // Configure and open the serial port + this->serial_port.setPort(this->port); + this->serial_port.setBaudrate(this->baudrate); + serial::Timeout timeout = serial::Timeout::simpleTimeout(1000); + this->serial_port.setTimeout(timeout); + this->serial_port.open(); + } catch(std::exception &e) { + RMP_THROW_MSG(ConnectionFailedException, e.what()); + } + this->connected = true; +} + +void SerialRMPIO::disconnect() { + if(this->connected) { + if(this->serial_port.isOpen()) + this->serial_port.close(); + this->connected = false; + } +} + +int SerialRMPIO::read(unsigned char* buffer, int size) { + return this->serial_port.read(buffer, size); +} + +int SerialRMPIO::write(unsigned char* buffer, int size) { + return this->serial_port.write(buffer, size); +} diff --git a/src/segwayrmp.cc b/src/segwayrmp.cc new file mode 100755 index 0000000..63fa193 --- /dev/null +++ b/src/segwayrmp.cc @@ -0,0 +1,920 @@ +#include + +#include +#include +#include +#if defined(SEGWAYRMP_USE_SERIAL) +# include +#endif + +#if !HAS_CLOCK_GETTIME +# include +#endif + +inline void +defaultSegwayStatusCallback(segwayrmp::SegwayStatus::Ptr &segway_status) +{ + std::cout << "Segway Status:" << std::endl << std::endl + << segway_status->str() << std::endl << std::endl; +} + +inline void defaultDebugMsgCallback(const std::string &msg) +{ + std::cerr << "SegwayRMP Debug: " << msg << std::endl; +} + +inline void defaultInfoMsgCallback(const std::string &msg) +{ + std::cerr << "SegwayRMP Info: " << msg << std::endl; +} + +inline void defaultErrorMsgCallback(const std::string &msg) +{ + std::cerr << "SegwayRMP Error: " << msg << std::endl; +} + +// This is from ROS's walltime function +// http://www.ros.org/doc/api/rostime/html/time_8cpp_source.html +inline segwayrmp::SegwayTime defaultTimestampCallback() +#ifndef WIN32 +throw(segwayrmp::NoHighPerformanceTimersException) +#endif +{ + segwayrmp::SegwayTime st; +#ifndef WIN32 +# if HAS_CLOCK_GETTIME + timespec start; + clock_gettime(CLOCK_REALTIME, &start); + st.sec = start.tv_sec; + st.nsec = start.tv_nsec; +# else + struct timeval timeofday; + gettimeofday(&timeofday, NULL); + st.sec = timeofday.tv_sec; + st.nsec = timeofday.tv_usec * 1000; +# endif +#else + // Win32 implementation + // unless I've missed something obvious, the only way to get high-precision + // time on Windows is via the QueryPerformanceCounter() call. However, + // this is somewhat problematic in Windows XP on some processors, + // especially AMD, because the Windows implementation can freak out when + // the CPU clocks down to save power. Time can jump or even go backwards. + // Microsoft has fixed this bug for most systems now, but it can still + // show up if you have not installed the latest CPU drivers (an oxymoron). + // They fixed all these problems in Windows Vista, and this API is by far + // the most accurate that I know of in Windows, so I'll use it here + // despite all these caveats + static LARGE_INTEGER cpu_freq, init_cpu_time; + uint32_t start_sec = 0; + uint32_t start_nsec = 0; + if ( ( start_sec == 0 ) && ( start_nsec == 0 ) ) { + QueryPerformanceFrequency(&cpu_freq); + if (cpu_freq.QuadPart == 0) { + throw segwayrmp::NoHighPerformanceTimersException(); + } + QueryPerformanceCounter(&init_cpu_time); + // compute an offset from the Epoch using the lower-performance + // timer API + FILETIME ft; + GetSystemTimeAsFileTime(&ft); + LARGE_INTEGER start_li; + start_li.LowPart = ft.dwLowDateTime; + start_li.HighPart = ft.dwHighDateTime; + // why did they choose 1601 as the time zero, instead of 1970? + // there were no outstanding hard rock bands in 1601. +# ifdef _MSC_VER + start_li.QuadPart -= 116444736000000000Ui64; +# else + start_li.QuadPart -= 116444736000000000ULL; +# endif + // 100-ns units. odd. + start_sec = (uint32_t)(start_li.QuadPart / 10000000); + start_nsec = (start_li.LowPart % 10000000) * 100; + } + LARGE_INTEGER cur_time; + QueryPerformanceCounter(&cur_time); + LARGE_INTEGER delta_cpu_time; + delta_cpu_time.QuadPart = cur_time.QuadPart - init_cpu_time.QuadPart; + // todo: how to handle cpu clock drift. not sure it's a big deal for us. + // also, think about clock wraparound. seems extremely unlikey, + // but possible + double d_delta_cpu_time = + delta_cpu_time.QuadPart / (double) cpu_freq.QuadPart; + uint32_t delta_sec = (uint32_t) floor(d_delta_cpu_time); + uint32_t delta_nsec = + (uint32_t) boost::math::round((d_delta_cpu_time - delta_sec) * 1e9); + + int64_t sec_sum = (int64_t)start_sec + (int64_t)delta_sec; + int64_t nsec_sum = (int64_t)start_nsec + (int64_t)delta_nsec; + + // Throws an exception if we go out of 32-bit range + normalizeSecNSecUnsigned(sec_sum, nsec_sum); + + st.sec = sec_sum; + st.nsec = nsec_sum; +#endif + return st; +} + +inline void defaultExceptionCallback(const std::exception &error) +{ + std::cerr << "SegwayRMP Unhandled Exception: " << error.what() + << std::endl; +} + +inline void printHex(char *data, int length) +{ + for (int i = 0; i < length; ++i) { + printf("0x%.2X ", (unsigned)(unsigned char)data[i]); + } + printf("\n"); +} + +inline void printHexFromString(std::string str) +{ + printHex(const_cast(str.c_str()), str.length()); +} + +using namespace segwayrmp; + +SegwayStatus::SegwayStatus() + : timestamp(SegwayTime(0, 0)), pitch(0.0f), pitch_rate(0.0f), roll(0.0f), + roll_rate(0.0f), left_wheel_speed(0.0f), right_wheel_speed(0.0f), + yaw_rate(0.0f), servo_frames(0.0f), integrated_left_wheel_position(0.0f), + integrated_right_wheel_position(0.0f), integrated_forward_position(0.0f), + integrated_turn_position(0.0f), left_motor_torque(0.0f), + right_motor_torque(0.0f), ui_battery_voltage(0.0f), + powerbase_battery_voltage(0.0f), commanded_velocity(0.0f), + commanded_yaw_rate(0.0f), operational_mode(disabled), + controller_gain_schedule(light), motor_status(0), touched(false) +{} + +std::string SegwayStatus::str() +{ + std::stringstream ss; + ss << "Time Stamp: " + << "\n Seconds: " << timestamp.sec + << "\n Nanoseconds: " << timestamp.nsec + << "\nPitch: " << pitch << "\nPitch Rate: " << pitch_rate + << "\nRoll: " << roll << "\nRoll Rate: " << roll_rate + << "\nLeft Wheel Speed: " << left_wheel_speed + << "\nRight Wheel Speed: " << right_wheel_speed + << "\nYaw Rate: " << yaw_rate + << "\nServo Frames: " << servo_frames + << "\nIntegrated Left Wheel Position: " << integrated_left_wheel_position + << "\nIntegrated Right Wheel Position: " + << integrated_right_wheel_position + << "\nIntegrated Forward Displacement: " << integrated_forward_position + << "\nIntegrated Turn Position: " << integrated_turn_position + << "\nLeft Motor Torque: " << left_motor_torque + << "\nRight Motor Torque: " << right_motor_torque + << "\nUI Battery Voltage: " << ui_battery_voltage + << "\nPowerbase Battery Voltage: " << powerbase_battery_voltage + << "\nOperational Mode: "; + switch (operational_mode) { + case disabled: + ss << "disabled"; + break; + case tractor: + ss << "tractor"; + break; + case balanced: + ss << "balanced"; + break; + case power_down: + ss << "power down"; + break; + default: + ss << "unknown"; + break; + } + ss << "\nController Gain Schedule: "; + switch (controller_gain_schedule) { + case light: + ss << "light"; + break; + case tall: + ss << "tall"; + break; + case heavy: + ss << "heavy"; + break; + default: + ss << "unknown"; + break; + } + ss << "\nCommanded Velocity: " << commanded_velocity + << "\nCommanded Yaw Rate: " << commanded_yaw_rate + << "\nMotor Status: "; + if (motor_status) { + ss << "Motors Enabled"; + } else { + ss << "E-Stopped"; + } + return ss.str(); +} + +SegwayRMP::SegwayRMP(InterfaceType interface_type, + SegwayRMPType segway_rmp_type) +: interface_type_(no_interface), segway_rmp_type_(segway_rmp_type), + connected_(false), + continuously_reading_(false), + status_callback_(defaultSegwayStatusCallback), + get_time_(defaultTimestampCallback), + debug_(defaultDebugMsgCallback), + info_(defaultInfoMsgCallback), + error_(defaultErrorMsgCallback), + handle_exception_(defaultExceptionCallback) +{ + this->segway_status_ = SegwayStatus::Ptr(new SegwayStatus()); + this->interface_type_ = interface_type; + switch (interface_type) { + case can: + RMP_THROW_MSG(ConfigurationException, "CAN is not supported currently"); + break; + case usb: + this->rmp_io_ = new FTD2XXRMPIO(); + break; + case serial: +#if defined(SEGWAYRMP_USE_SERIAL) + this->rmp_io_ = new SerialRMPIO(); +#else + RMP_THROW_MSG(ConfigurationException, "Library is not built with Serial " + "support"); +#endif + break; + case ethernet: + RMP_THROW_MSG(ConfigurationException, "Ethernet is not currently " + "supported"); + break; + case no_interface: + // do nothing + break; + default: + RMP_THROW_MSG(ConfigurationException, "Invalid InterfaceType specified"); + break; + } + + // Set the constants based on the segway type + this->SetConstantsBySegwayType_(this->segway_rmp_type_); +} + +SegwayRMP::~SegwayRMP() +{ + if (this->continuously_reading_) { + this->StopReadingContinuously_(); + } + if (this->interface_type_ == serial) { +#if defined(SEGWAYRMP_USE_SERIAL) + SerialRMPIO * ptr = (SerialRMPIO *)(this->rmp_io_); + delete ptr; +#endif + } + if (this->interface_type_ == usb) { + FTD2XXRMPIO * ptr = (FTD2XXRMPIO *)(this->rmp_io_); + delete ptr; + } +} + +void SegwayRMP::configureSerial(std::string port, int baudrate) +{ +#if SEGWAYRMP_USE_SERIAL + if (this->interface_type_ == serial) { + SerialRMPIO *serial_rmp = (SerialRMPIO *)(this->rmp_io_); + serial_rmp->configure(port, baudrate); + } else { + RMP_THROW_MSG(ConfigurationException, "configureSerial: Cannot configure " + "serial when the InterfaceType is not serial."); + } +#else + RMP_THROW_MSG(ConfigurationException, "configureSerial: The segwayrmp " + "library is not build with serial support, not implemented."); +#endif +} + +void SegwayRMP::configureUSBBySerial(std::string serial_number, int baudrate) +{ + if (this->interface_type_ == usb) { + FTD2XXRMPIO *ftd2xx_rmp = (FTD2XXRMPIO *)(this->rmp_io_); + ftd2xx_rmp->configureUSBBySerial(serial_number, baudrate); + } else { + RMP_THROW_MSG(ConfigurationException, "configureUSBBySerial: Cannot " + "configure ftd2xx usb when the InterfaceType is not usb."); + } +} + +void SegwayRMP::configureUSBByDescription(std::string description, + int baudrate) +{ + if (this->interface_type_ == usb) { + FTD2XXRMPIO *ftd2xx_rmp = (FTD2XXRMPIO *)(this->rmp_io_); + ftd2xx_rmp->configureUSBByDescription(description, baudrate); + } else { + RMP_THROW_MSG(ConfigurationException, "configureUSBByDescription: " + "Cannot configure ftd2xx usb when the InterfaceType is not usb."); + } +} + +void SegwayRMP::configureUSBByIndex(int device_index, int baudrate) +{ + if (this->interface_type_ == usb) { + FTD2XXRMPIO *ftd2xx_rmp = (FTD2XXRMPIO *)(this->rmp_io_); + ftd2xx_rmp->configureUSBByIndex(device_index, baudrate); + } else { + RMP_THROW_MSG(ConfigurationException, "configureUSBByIndex: " + "Cannot configure ftd2xx usb when the InterfaceType is not usb."); + } +} + +void SegwayRMP::connect(bool reset_integrators) +{ + // Connect to the interface + this->rmp_io_->connect(); + + this->connected_ = true; + + // Kick off the read thread + this->StartReadingContinuously_(); + + if (reset_integrators) { + // Reset all the integrators + this->resetAllIntegrators(); + } +} + +void SegwayRMP::shutdown() +{ + // Ensure we are connected + if (!this->connected_) + RMP_THROW_MSG(ConfigurationException, "Cannot send shutdown: " + "Not Connected."); + try { + Packet packet; + + packet.id = 0x0412; + + this->rmp_io_->sendPacket(packet); + } catch (std::exception &e) { + std::stringstream ss; + ss << "Cannot send shutdown: " << e.what(); + RMP_THROW_MSG(ConfigurationException, ss.str().c_str()); + } +} + +void SegwayRMP::move(float linear_velocity, float angular_velocity) +{ + // Ensure we are connected + if (!this->connected_) + RMP_THROW_MSG(MoveFailedException, "Not Connected."); + try { + short int lv = (short int)(linear_velocity * this->mps_to_counts_); + short int av = (short int)(angular_velocity * this->dps_to_counts_); + + Packet packet; + + packet.id = 0x0413; + + packet.data[0] = (unsigned char)((lv & 0xFF00) >> 8); + packet.data[1] = (unsigned char)(lv & 0x00FF); + packet.data[2] = (unsigned char)((av & 0xFF00) >> 8); + packet.data[3] = (unsigned char)(av & 0x00FF); + packet.data[4] = 0x00; + packet.data[5] = 0x00; + packet.data[6] = 0x00; + packet.data[7] = 0x00; + + this->rmp_io_->sendPacket(packet); + } catch (std::exception &e) { + RMP_THROW_MSG(MoveFailedException, e.what()); + } +} + +void SegwayRMP::setOperationalMode(OperationalMode operational_mode) +{ + // Ensure we are connected + if (!this->connected_) + RMP_THROW_MSG(ConfigurationException, "Cannot set operational mode: " + "Not Connected."); + try { + Packet packet; + + packet.id = 0x0413; + + packet.data[0] = 0x00; + packet.data[1] = 0x00; + packet.data[2] = 0x00; + packet.data[3] = 0x00; + packet.data[4] = 0x00; + packet.data[5] = 0x10; + packet.data[6] = 0x00; + packet.data[7] = (unsigned char)operational_mode; + + this->rmp_io_->sendPacket(packet); + +// while(this->segway_status_->operational_mode != operational_mode) { +// // Check again in 10 ms +// boost::this_thread::sleep(boost::posix_time::milliseconds(10)); +// } + } catch (std::exception &e) { + std::stringstream ss; + ss << "Cannot set operational mode: " << e.what(); + RMP_THROW_MSG(ConfigurationException, ss.str().c_str()); + } +} + +void SegwayRMP::setControllerGainSchedule( + ControllerGainSchedule controller_gain_schedule) +{ + // Ensure we are connected + if (!this->connected_) + RMP_THROW_MSG(ConfigurationException, "Cannot set controller gain " + "schedule: Not Connected."); + try { + Packet packet; + + packet.id = 0x0413; + + packet.data[0] = 0x00; + packet.data[1] = 0x00; + packet.data[2] = 0x00; + packet.data[3] = 0x00; + packet.data[4] = 0x00; + packet.data[5] = 0x0D; + packet.data[6] = 0x00; + packet.data[7] = (unsigned char)controller_gain_schedule; + + this->rmp_io_->sendPacket(packet); + +// while(this->segway_status_->controller_gain_schedule +// != controller_gain_schedule) +// { +// // Check again in 10 ms +// boost::this_thread::sleep(boost::posix_time::milliseconds(10)); +// } + } catch (std::exception &e) { + std::stringstream ss; + ss << "Cannot set controller gain schedule: " << e.what(); + RMP_THROW_MSG(ConfigurationException, ss.str().c_str()); + } +} + +void SegwayRMP::setBalanceModeLocking(bool state) +{ + // Ensure we are connected + if (!this->connected_) + RMP_THROW_MSG(ConfigurationException, "Cannot set balance mode lock: " + "Not Connected."); + try { + Packet packet; + + packet.id = 0x0413; + + packet.data[0] = 0x00; + packet.data[1] = 0x00; + packet.data[2] = 0x00; + packet.data[3] = 0x00; + packet.data[4] = 0x00; + packet.data[5] = 0x0F; + packet.data[6] = 0x00; + if (state) + packet.data[7] = 0x01; + else + packet.data[7] = 0x00; + + this->rmp_io_->sendPacket(packet); + } catch (std::exception &e) { + std::stringstream ss; + ss << "Cannot set balance mode lock: " << e.what(); + RMP_THROW_MSG(ConfigurationException, ss.str().c_str()); + } +} + +void SegwayRMP::resetAllIntegrators() +{ + // Ensure we are connected + if (!this->connected_) + RMP_THROW_MSG(ConfigurationException, "Cannot reset Integrators: Not " + "Connected."); + try { + Packet packet; + + packet.id = 0x0413; + + packet.data[0] = 0x00; + packet.data[1] = 0x00; + packet.data[2] = 0x00; + packet.data[3] = 0x00; + packet.data[4] = 0x00; + packet.data[5] = 0x32; + packet.data[6] = 0x00; + packet.data[7] = 0x01; + + this->rmp_io_->sendPacket(packet); + + packet.data[7] = 0x02; + + this->rmp_io_->sendPacket(packet); + + packet.data[7] = 0x04; + + this->rmp_io_->sendPacket(packet); + + packet.data[7] = 0x08; + + this->rmp_io_->sendPacket(packet); + + } catch (std::exception &e) { + std::stringstream ss; + ss << "Cannot reset Integrators: " << e.what(); + RMP_THROW_MSG(ConfigurationException, ss.str().c_str()); + } +} + +void SegwayRMP::setMaxVelocityScaleFactor(double scalar) +{ + // Ensure we are connected + if (!this->connected_) + RMP_THROW_MSG(ConfigurationException, "Cannot set max velocity scale " + "factor: Not Connected."); + try { + Packet packet; + + packet.id = 0x0413; + + packet.data[0] = 0x00; + packet.data[1] = 0x00; + packet.data[2] = 0x00; + packet.data[3] = 0x00; + packet.data[4] = 0x00; + packet.data[5] = 0x0A; + packet.data[6] = 0x00; + + if (scalar < 0.0) + scalar = 0.0; + if (scalar > 1.0) + scalar = 1.0; + scalar *= 16.0; + scalar = floor(scalar); + + short int scalar_int = (short int)scalar; + + packet.data[7] = (unsigned char)(scalar_int & 0x00FF); + + this->rmp_io_->sendPacket(packet); + } catch (std::exception &e) { + std::stringstream ss; + ss << "Cannot set max velocity scale factor: " << e.what(); + RMP_THROW_MSG(ConfigurationException, ss.str().c_str()); + } +} + +void SegwayRMP::setMaxAccelerationScaleFactor(double scalar) +{ + // Ensure we are connected + if (!this->connected_) + RMP_THROW_MSG(ConfigurationException, "Cannot set max acceleration scale " + "factor: Not Connected."); + try { + Packet packet; + + packet.id = 0x0413; + + packet.data[0] = 0x00; + packet.data[1] = 0x00; + packet.data[2] = 0x00; + packet.data[3] = 0x00; + packet.data[4] = 0x00; + packet.data[5] = 0x0B; + packet.data[6] = 0x00; + + if (scalar < 0.0) + scalar = 0.0; + if (scalar > 1.0) + scalar = 1.0; + scalar *= 16.0; + scalar = floor(scalar); + + short int scalar_int = (short int)scalar; + + packet.data[7] = (unsigned char)(scalar_int & 0x00FF); + + this->rmp_io_->sendPacket(packet); + } catch (std::exception &e) { + std::stringstream ss; + ss << "Cannot set max acceleration scale factor: " << e.what(); + RMP_THROW_MSG(ConfigurationException, ss.str().c_str()); + } +} + +void SegwayRMP::setMaxTurnScaleFactor(double scalar) +{ + // Ensure we are connected + if (!this->connected_) + RMP_THROW_MSG(ConfigurationException, "Cannot set max turn scale factor: " + "Not Connected."); + try { + Packet packet; + + packet.id = 0x0413; + + packet.data[0] = 0x00; + packet.data[1] = 0x00; + packet.data[2] = 0x00; + packet.data[3] = 0x00; + packet.data[4] = 0x00; + packet.data[5] = 0x0C; + packet.data[6] = 0x00; + + if (scalar < 0.0) + scalar = 0.0; + if (scalar > 1.0) + scalar = 1.0; + scalar *= 16.0; + scalar = floor(scalar); + + short int scalar_int = (short int)scalar; + + packet.data[7] = (unsigned char)(scalar_int & 0x00FF); + + this->rmp_io_->sendPacket(packet); + } catch (std::exception &e) { + std::stringstream ss; + ss << "Cannot set max turn scale factor: " << e.what(); + RMP_THROW_MSG(ConfigurationException, ss.str().c_str()); + } +} + +void SegwayRMP::setCurrentLimitScaleFactor(double scalar) +{ + // Ensure we are connected + if (!this->connected_) + RMP_THROW_MSG(ConfigurationException, "Cannot set current limit scale " + "factor: Not Connected."); + try { + Packet packet; + + packet.id = 0x0413; + + packet.data[0] = 0x00; + packet.data[1] = 0x00; + packet.data[2] = 0x00; + packet.data[3] = 0x00; + packet.data[4] = 0x00; + packet.data[5] = 0x0E; + packet.data[6] = 0x00; + + if (scalar < 0.0) + scalar = 0.0; + if (scalar > 1.0) + scalar = 1.0; + scalar *= 256.0; + scalar = floor(scalar); + + short int scalar_int = (short int)scalar; + + packet.data[7] = (unsigned char)(scalar_int & 0x00FF); + + this->rmp_io_->sendPacket(packet); + } catch (std::exception &e) { + std::stringstream ss; + ss << "Cannot set current limit scale factor: " << e.what(); + RMP_THROW_MSG(ConfigurationException, ss.str().c_str()); + } +} + +void SegwayRMP::setStatusCallback(SegwayStatusCallback callback) { + this->status_callback_ = callback; +} + +void SegwayRMP::setLogMsgCallback(std::string log_level, + LogMsgCallback callback) +{ + // Convert to lower case + std::transform(log_level.begin(), log_level.end(), + log_level.begin(), ::tolower); + if (log_level == "debug") { + this->debug_ = callback; + } + if (log_level == "info") { + this->info_ = callback; + } + if (log_level == "error") { + this->error_ = callback; + } +} + +void SegwayRMP::setTimestampCallback(GetTimeCallback callback) { + this->get_time_ = callback; +} + +void SegwayRMP::setExceptionCallback(ExceptionCallback exception_callback) { + this->handle_exception_ = exception_callback; +} + +void SegwayRMP::ReadContinuously_() { + Packet packet; + while (this->continuously_reading_) { + try { + this->rmp_io_->getPacket(packet); + this->ProcessPacket_(packet); + } catch (PacketRetrievalException &e) { + if (e.error_number() == 2) // Failed Checksum + this->error_("Checksum mismatch..."); + else if (e.error_number() == 3) // No packet received + this->error_("No data from Segway..."); + else + this->handle_exception_(e); + } + } +} + +void SegwayRMP::ExecuteCallbacks_() { + while (this->continuously_reading_) { + SegwayStatus::Ptr ss = this->ss_queue_.dequeue(); + if (this->continuously_reading_) { + try { + if (ss) { + if (this->status_callback_) { + this->status_callback_(ss); + } // if this->status_callback_ + } // if ss + } catch (std::exception &e) { + this->handle_exception_(e); + }// try callback + }// if continuous + }// while continuous +} + +void SegwayRMP::StartReadingContinuously_() { + this->continuously_reading_ = true; + this->read_thread_ = + boost::thread(&SegwayRMP::ReadContinuously_, this); + this->callback_execution_thread_ = + boost::thread(&SegwayRMP::ExecuteCallbacks_, this); +} + +void SegwayRMP::StopReadingContinuously_() +{ + this->continuously_reading_ = false; + this->read_thread_.join(); + this->ss_queue_.cancel(); + this->callback_execution_thread_.join(); +} + +void SegwayRMP::SetConstantsBySegwayType_(SegwayRMPType &rmp_type) { + if (rmp_type == rmp200 || rmp_type == rmp400) { + this->dps_to_counts_ = 7.8; + this->mps_to_counts_ = 332.0; + this->meters_to_counts_ = 33215.0; + this->rev_to_counts_ = 112644.0; + this->torque_to_counts_ = 1094.0; + } else + if (rmp_type == rmp50 || rmp_type == rmp100) { + this->dps_to_counts_ = 7.8; + this->mps_to_counts_ = 401.0; + this->meters_to_counts_ = 40181.0; + this->rev_to_counts_ = 117031.0; + this->torque_to_counts_ = 1463.0; + } else { + RMP_THROW_MSG(ConfigurationException, "Invalid Segway RMP Type"); + } +} + +inline short int getShortInt(unsigned char high, unsigned char low) +{ + return (short int)(((unsigned short int)high << 8) + | (unsigned short int)low); +} + +inline int +getInt(unsigned char lhigh, unsigned char llow, + unsigned char hhigh, unsigned char hlow) +{ + int result = 0; + char data[4] = {llow, lhigh, hlow, hhigh}; + memcpy(&result, data, 4); + return result; +} + +bool SegwayRMP::ParsePacket_(Packet &packet, SegwayStatus::Ptr &ss_ptr) +{ + bool status_updated = false; + if (packet.channel == 0xBB) // Ignore Channel B messages + return status_updated; + + // This section comes largerly from the Segway example code + switch (packet.id) { + case 0x0400: // COMMAND REQUEST + // This is the first packet of a msg series, timestamp here. + ss_ptr->timestamp = this->get_time_(); + break; + case 0x0401: + ss_ptr->pitch = getShortInt(packet.data[0], packet.data[1]) + / this->dps_to_counts_; + ss_ptr->pitch_rate = getShortInt(packet.data[2], packet.data[3]) + / this->dps_to_counts_; + ss_ptr->roll = getShortInt(packet.data[4], packet.data[5]) + / this->dps_to_counts_; + ss_ptr->roll_rate = getShortInt(packet.data[6], packet.data[7]) + / this->dps_to_counts_; + ss_ptr->touched = true; + break; + case 0x0402: + ss_ptr->left_wheel_speed = getShortInt(packet.data[0], packet.data[1]) + / this->mps_to_counts_; + ss_ptr->right_wheel_speed = getShortInt(packet.data[2], packet.data[3]) + / this->mps_to_counts_; + ss_ptr->yaw_rate = getShortInt(packet.data[4], packet.data[5]) + / this->dps_to_counts_; + ss_ptr->servo_frames = ( + (((short unsigned int)packet.data[6]) << 8) + | ((short unsigned int)packet.data[7]) + ) * 0.01; + ss_ptr->touched = true; + break; + case 0x0403: + ss_ptr->integrated_left_wheel_position = + getInt(packet.data[0], packet.data[1], packet.data[2], packet.data[3]) + / this->meters_to_counts_; + ss_ptr->integrated_right_wheel_position = + getInt(packet.data[4], packet.data[5], packet.data[6], packet.data[7]) + / this->meters_to_counts_; + ss_ptr->touched = true; + break; + case 0x0404: + ss_ptr->integrated_forward_position = + getInt(packet.data[0], packet.data[1], packet.data[2], packet.data[3]) + / this->meters_to_counts_; + ss_ptr->integrated_turn_position = + getInt(packet.data[4], packet.data[5], packet.data[6], packet.data[7]) + / this->rev_to_counts_; + // convert from revolutions to degrees + ss_ptr->integrated_turn_position *= 360.0; + ss_ptr->touched = true; + break; + case 0x0405: + ss_ptr->left_motor_torque = getShortInt(packet.data[0], packet.data[1]) + / this->torque_to_counts_; + ss_ptr->right_motor_torque = getShortInt(packet.data[2], packet.data[3]) + / this->torque_to_counts_; + ss_ptr->touched = true; + break; + case 0x0406: + ss_ptr->operational_mode = + OperationalMode(getShortInt(packet.data[0], packet.data[1])); + ss_ptr->controller_gain_schedule = + ControllerGainSchedule(getShortInt(packet.data[2], packet.data[3])); + ss_ptr->ui_battery_voltage = + ( + (((short unsigned int)packet.data[4]) << 8) + | ((short unsigned int)packet.data[5]) + ) * 0.0125 + 1.4; + ss_ptr->powerbase_battery_voltage = + ( + (((short unsigned int)packet.data[6]) << 8) + | ((short unsigned int)packet.data[7]) + ) / 4.0; + ss_ptr->touched = true; + break; + case 0x0407: + ss_ptr->commanded_velocity = + (float)getShortInt(packet.data[0], packet.data[1]) + / this->mps_to_counts_; + ss_ptr->commanded_yaw_rate = + (float)getShortInt(packet.data[2], packet.data[3]) + / 1024.0; + status_updated = true; + ss_ptr->touched = true; + break; + case 0x0680: + if (packet.data[3] == 0x80) // Motors Enabled + ss_ptr->motor_status = 1; + else // E-Stopped + ss_ptr->motor_status = 0; + ss_ptr->touched = true; + break; + default: // Unknown/Unhandled Message + break; + }; + return status_updated; +} + +void SegwayRMP::ProcessPacket_(Packet &packet) +{ + bool status_updated = false; + + status_updated = this->ParsePacket_(packet, this->segway_status_); + + // Messages come in order 0x0400, 0x0401, ... 0x0407 so a + // complete "cycle" of information has been sent every + // time we get an 0x0407 + if (status_updated) { + if (this->ss_queue_.enqueue(this->segway_status_)) { + this->error_("Falling behind, SegwayStatus Queue Full, skipping packet " + "report..."); + } + this->segway_status_ = SegwayStatus::Ptr(new SegwayStatus()); + } +} + diff --git a/stack.xml b/stack.xml new file mode 100644 index 0000000..2a8233b --- /dev/null +++ b/stack.xml @@ -0,0 +1,9 @@ + + libsegwayrmp + Maintained by William Woodall + BSD + + http://ros.org/wiki/libsegwayrmp + + + diff --git a/tests/segwayrmp_tests.cc b/tests/segwayrmp_tests.cc new file mode 100644 index 0000000..f0aaffa --- /dev/null +++ b/tests/segwayrmp_tests.cc @@ -0,0 +1,172 @@ +#include "gtest/gtest.h" + +// OMG this is so nasty... +#define private public +#define protected public +#include "segwayrmp/segwayrmp.h" +#include "segwayrmp/impl/rmp_io.h" + +using namespace segwayrmp; + +namespace { + +class PacketTests : public ::testing::Test { +protected: + virtual void SetUp() { + segway_rmp = new SegwayRMP(no_interface); + ss = SegwayStatus::Ptr(new SegwayStatus); + + pck.channel = 0xAA; + pck.id = 0x0400; + pck.data[0] = 0x00; + pck.data[1] = 0x00; + pck.data[2] = 0x00; + pck.data[3] = 0x00; + pck.data[4] = 0x00; + pck.data[5] = 0x00; + pck.data[6] = 0x00; + pck.data[7] = 0x00; + } + + SegwayRMP *segway_rmp; + Packet pck; + SegwayStatus::Ptr ss; +}; + +TEST_F(PacketTests, IgnoresChannelB) { + pck.channel = 0xBB; + pck.id = 0x0401; + segway_rmp->ParsePacket_(pck, ss); + + ASSERT_FALSE(ss->touched); +} + +TEST_F(PacketTests, IgnoresCommandRequest) { + segway_rmp->ParsePacket_(pck,ss); + + ASSERT_FALSE(ss->touched); +} + +TEST_F(PacketTests, ParsesPitchRollCorrectly) { + pck.id = 0x0401; + pck.data[0] = 0xFF; + pck.data[1] = 0xF9; + pck.data[2] = 0x00; + pck.data[3] = 0x0F; + pck.data[4] = 0xFF; + pck.data[5] = 0xFF; + pck.data[6] = 0x00; + pck.data[7] = 0x09; + + segway_rmp->ParsePacket_(pck,ss); + EXPECT_TRUE(ss->touched); + EXPECT_NEAR(-0.897,ss->pitch,0.128); + EXPECT_NEAR(1.923,ss->pitch_rate,0.128); + EXPECT_NEAR(-0.128,ss->roll,0.128); + EXPECT_NEAR(1.154,ss->roll_rate,0.128); +} + +TEST_F(PacketTests, ParsesWheelSpeedsYawRatesServoFramesCorrectly) { + pck.id = 0x0402; + pck.data[0] = 0x00; + pck.data[1] = 0x0C; + pck.data[2] = 0xFF; + pck.data[3] = 0xF2; + pck.data[4] = 0x00; + pck.data[5] = 0x4B; + pck.data[6] = 0x18; + pck.data[7] = 0xD4; + + segway_rmp->ParsePacket_(pck,ss); + EXPECT_TRUE(ss->touched); + EXPECT_NEAR(0.0361,ss->left_wheel_speed,0.003); + EXPECT_NEAR(-0.0422,ss->right_wheel_speed,0.003); + EXPECT_NEAR(9.6154,ss->yaw_rate,0.128); + EXPECT_NEAR(63.56,ss->servo_frames,0.01); +} + +TEST_F(PacketTests, ParsesIntegratedWheelsCorrectly) { + pck.id = 0x0403; + pck.data[0] = 0xFE; + pck.data[1] = 0x06; + pck.data[2] = 0xFF; + pck.data[3] = 0xFF; + pck.data[4] = 0x00; + pck.data[5] = 0x4F; + pck.data[6] = 0x00; + pck.data[7] = 0x00; + + segway_rmp->ParsePacket_(pck,ss); + EXPECT_TRUE(ss->touched); + EXPECT_NEAR(-0.0152341,ss->integrated_left_wheel_position,0.00003); + EXPECT_NEAR(0.0023784,ss->integrated_right_wheel_position,0.00003); +} + +TEST_F(PacketTests, ParsesIntegratedDistanceTurnCorrectly) { + pck.id = 0x0404; + pck.channel = 0xAA; + // Packet id: 404, Packet Channel: AA, + // Packet Data: 0xFC 0x19 0xFF 0xFF + // 0xFF 0xAC 0xFF 0xFF + pck.data[0] = 0xFC; + pck.data[1] = 0x19; + pck.data[2] = 0xFF; + pck.data[3] = 0xFF; + pck.data[4] = 0xFF; + pck.data[5] = 0xAC; + pck.data[6] = 0xFF; + pck.data[7] = 0xFF; + + segway_rmp->ParsePacket_(pck,ss); + EXPECT_TRUE(ss->touched); + EXPECT_NEAR(-0.0300768,ss->integrated_forward_position,0.00003); + EXPECT_NEAR(-0.268452,ss->integrated_turn_position,0.00001); +} + +TEST_F(PacketTests, ParsesMotorTorqueCorrectly) { + pck.id = 0x0405; + // Packet id: 405, Packet Channel: AA, Packet Data: 0xFF 0xF9 0x00 0xA7 0x00 0x80 0x00 0x00 + // 1094 + pck.data[0] = 0xFF; + pck.data[1] = 0xF9; + pck.data[2] = 0x00; + pck.data[3] = 0xA7; + pck.data[4] = 0x00; + pck.data[5] = 0x80; + pck.data[6] = 0x00; + pck.data[7] = 0x00; + + segway_rmp->ParsePacket_(pck,ss); + EXPECT_TRUE(ss->touched); + EXPECT_NEAR(-0.006399,ss->left_motor_torque,0.0009); + EXPECT_NEAR(0.152651,ss->right_motor_torque,0.0009); +} + +TEST_F(PacketTests, ParsesStatusesCorrectly) { + pck.id = 0x0406; + // Packet id: 406, Packet Channel: AA, Packet Data: 0x00 0x01 0x00 0x00 0x02 0x7A 0x01 0x38 + pck.data[0] = 0x00; + pck.data[1] = 0x01; + pck.data[2] = 0x00; + pck.data[3] = 0x00; + pck.data[4] = 0x02; + pck.data[5] = 0x7A; + pck.data[6] = 0x01; + pck.data[7] = 0x38; + + segway_rmp->ParsePacket_(pck,ss); + EXPECT_TRUE(ss->touched); + // EXPECT_EQ(1,(int)ss.operational_mode); + // EXPECT_EQ(0,(int)ss.controller_gain_schedule); + EXPECT_NEAR(9.325,ss->ui_battery_voltage,0.25); + EXPECT_NEAR(78.0,ss->powerbase_battery_voltage,0.25); +} + +// TODO: Add tests for motor enabled/disabled and commanded velocity and yaw rate + +} // namespace + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}