Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

switched to async_read for bad performance with multiple vesc #40

Open
wants to merge 1 commit into
base: ros2
Choose a base branch
from
Open

switched to async_read for bad performance with multiple vesc #40

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
124 changes: 59 additions & 65 deletions vesc_driver/src/vesc_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ class VescInterface::Impl
: owned_ctx{new IoContext(2)},
serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx)}
{}
void packet_creation_thread();
void receive_callback(const std::vector<uint8_t> & buffer, const size_t & bytes_transferred);
void on_configure();
void connect(const std::string & port);

Expand All @@ -78,69 +78,66 @@ class VescInterface::Impl
std::vector<uint8_t> buffer_;
};

void VescInterface::Impl::packet_creation_thread()
void VescInterface::Impl::receive_callback(
const std::vector<uint8_t> & temp_buffer,
const size_t & bytes_transferred
)
{
static auto temp_buffer = Buffer(2048, 0);
while (packet_thread_run_) {
const auto bytes_read = serial_driver_->port()->receive(temp_buffer);
buffer_.reserve(buffer_.size() + temp_buffer.size());
buffer_.insert(buffer_.end(), temp_buffer.begin(), temp_buffer.begin() + bytes_read);
int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE;
if (!buffer_.empty()) {
// search buffer for valid packet(s)
auto iter = buffer_.begin();
auto iter_begin = buffer_.begin();
while (iter != buffer_.end()) {
// check if valid start-of-frame character
if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter ||
VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter)
{
// good start, now attempt to create packet
std::string error;
VescPacketConstPtr packet =
VescPacketFactory::createPacket(iter, buffer_.end(), &bytes_needed, &error);
if (packet) {
// good packet, check if we skipped any data
if (std::distance(iter_begin, iter) > 0) {
std::ostringstream ss;
ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " <<
std::distance(iter_begin, iter) << " bytes.";
error_handler_(ss.str());
}
// call packet handler
packet_handler_(packet);
// update state
iter = iter + packet->frame().size();
iter_begin = iter;
// continue to look for another frame in buffer
continue;
} else if (bytes_needed > 0) {
// need more data, break out of while loop
break; // for (iter_sof...
} else {
// else, this was not a packet, move on to next byte
error_handler_(error);
buffer_.reserve(buffer_.size() + bytes_transferred);
buffer_.insert(buffer_.end(), temp_buffer.begin(), temp_buffer.begin() + bytes_transferred);
int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE;
if (!buffer_.empty()) {
// search buffer for valid packet(s)
auto iter = buffer_.begin();
auto iter_begin = buffer_.begin();
while (iter != buffer_.end()) {
// check if valid start-of-frame character
if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter ||
VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter)
{
// good start, now attempt to create packet
std::string error;
VescPacketConstPtr packet =
VescPacketFactory::createPacket(iter, buffer_.end(), &bytes_needed, &error);
if (packet) {
// good packet, check if we skipped any data
if (std::distance(iter_begin, iter) > 0) {
std::ostringstream ss;
ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " <<
std::distance(iter_begin, iter) << " bytes.";
error_handler_(ss.str());
}
// call packet handler
packet_handler_(packet);
// update state
iter = iter + packet->frame().size();
iter_begin = iter;
// continue to look for another frame in buffer
continue;
} else if (bytes_needed > 0) {
// need more data, break out of while loop
break; // for (iter_sof...
} else {
// else, this was not a packet, move on to next byte
error_handler_(error);
}

iter++;
}

// if iter is at the end of the buffer, more bytes are needed
if (iter == buffer_.end()) {
bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE;
}
iter++;
}

// erase "used" buffer
if (std::distance(iter_begin, iter) > 0) {
std::ostringstream ss;
ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes.";
error_handler_(ss.str());
}
buffer_.erase(buffer_.begin(), iter);
// if iter is at the end of the buffer, more bytes are needed
if (iter == buffer_.end()) {
bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE;
}
// Only attempt to read every 5 ms
std::this_thread::sleep_for(std::chrono::milliseconds(5));

// erase "used" buffer
if (std::distance(iter_begin, iter) > 0) {
std::ostringstream ss;
ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes.";
error_handler_(ss.str());
}
buffer_.erase(buffer_.begin(), iter);
}
}

Expand All @@ -155,9 +152,14 @@ void VescInterface::Impl::connect(const std::string & port)
serial_driver_->init_port(port, *device_config_);
if (!serial_driver_->port()->is_open()) {
serial_driver_->port()->open();
serial_driver_->port()->async_receive(
std::bind(
&VescInterface::Impl::receive_callback, this, std::placeholders::_1,
std::placeholders::_2));
}
}


VescInterface::VescInterface(
const std::string & port,
const PacketHandlerFunction & packet_handler,
Expand Down Expand Up @@ -205,12 +207,6 @@ void VescInterface::connect(const std::string & port)
ss << "Failed to open the serial port " << port << " to the VESC. " << e.what();
throw SerialException(ss.str().c_str());
}

// start up a monitoring thread
impl_->packet_thread_run_ = true;
impl_->packet_thread_ = std::unique_ptr<std::thread>(
new std::thread(
&VescInterface::Impl::packet_creation_thread, impl_.get()));
}

void VescInterface::disconnect()
Expand All @@ -219,9 +215,7 @@ void VescInterface::disconnect()

if (isConnected()) {
// bring down read thread
impl_->packet_thread_run_ = false;
requestFWVersion();
impl_->packet_thread_->join();
impl_->serial_driver_->port()->close();
}
}
Expand Down