void Configure(SerialSettings& arSettings, boost::asio::serial_port& arPort, error_code& ec)
{
//Set all the various options
arPort.set_option(ConvertBaud(arSettings.mBaud), ec); if(ec) return;
arPort.set_option(ConvertDataBits(arSettings.mDataBits), ec); if(ec) return;
arPort.set_option(ConvertParity(arSettings.mParity), ec); if(ec) return;
arPort.set_option(ConvertStopBits(arSettings.mStopBits), ec); if(ec) return;
arPort.set_option(ConvertFlow(arSettings.mFlowType), ec); if(ec) return;
}
serial(const char* dev_name):ios(),port(ios,dev_name)
{
port.set_option(boost::asio::serial_port_base::baud_rate(9600));
port.set_option(boost::asio::serial_port_base::character_size(8));
port.set_option(boost::asio::serial_port_base::stop_bits(serial_port_base::stop_bits::one));
port.set_option(boost::asio::serial_port_base::parity(serial_port_base::parity::none));
port.set_option(boost::asio::serial_port_base::flow_control(serial_port_base::flow_control::none));
read_some();
boost::thread td(boost::bind(&boost::asio::io_service::run,&ios));
}
ComClient(boost::asio::io_service& io_service, unsigned int baud, const string& device)
: active_(true), io_service_(io_service), serialPort(io_service, device)
{
if (!serialPort.is_open())
{
cerr << "Failed to open serial port\n";
return;
}
cout << "Open " << device << " successfully.\n";
boost::asio::serial_port_base::baud_rate baud_option(baud);
serialPort.set_option(baud_option); // set the baud rate after the port has been opened
read_start();
}
/*
Attempts to close and reopen the serial port
Sleeps for one second before returning if an exception is caught
*/
void open()
{
try
{
p.close();
p.open(port);
p.set_option(boost::asio::serial_port::baud_rate(baudrate));
return;
}
catch (const std::exception &exc)
{
ROS_ERROR("DVL: error on open(port=%s): %s; reopening after delay", port.c_str(), exc.what());
boost::this_thread::sleep(boost::posix_time::seconds(1));
}
}
void read_start(void)
{ // Start an asynchronous read and call read_complete when it completes or fails
serialPort.async_read_some(boost::asio::buffer(read_msg_, max_read_length),
boost::bind(&ComClient::read_complete, this,
boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
}
void send_heartbeat()
{
double maxdepth = 15;
std::stringstream buf;
buf << "CR0\r"; // load factory settings (won't change baud rate)
buf << "#BJ 100 110 000\r"; // enable only bottom track high res velocity and bottom track
// range
// buf << "#BK2\r"; // send water mass pings when bottom track pings fail
// buf << "#BL7,36,46\r"; // configure near layer and far layer to 12 and 15 feet
buf << "ES0\r"; // 0 salinity
buf << "EX00000\r"; // no transformation
buf << "EZ10000010\r"; // configure sensor sources. Provide manual data for everything except
// speed of sound and temperature
buf << "BX" << std::setw(5) << std::setfill('0') << (int)(maxdepth * 10 + 0.5) << '\r'; // configure max depth
buf << "TT2012/03/04, 05:06:07\r"; // set RTC
buf << "CS\r"; // start pinging
std::string str = buf.str();
try // Write heartbeat to serial port
{
size_t written = 0;
while (written < str.size())
{
written += p.write_some(boost::asio::buffer(str.data() + written, str.size() - written));
}
}
catch (const std::exception &exc)
{
ROS_ERROR_THROTTLE(0.5, "DVL: error on write: %s; dropping heartbeat", exc.what());
}
}
void read_some()
{
port.async_read_some(boost::asio::buffer(read_msg_,sizeof(read_msg_)),
boost::bind(&serial::handler,this,
boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
}
void do_close(const boost::system::error_code& error)
{ // something has gone wrong, so close the socket & make this object inactive
if (error == boost::asio::error::operation_aborted) // if this call is the result of a timer cancel()
return; // ignore it because the connection cancelled the timer
if (error)
cerr << "Error: " << error.message() << endl; // show the error message
else
cout << "Error: Connection did not succeed.\n";
cout << "Press Enter to exit\n";
serialPort.close();
active_ = false;
}
void Gps::initialize(boost::asio::serial_port& serial_port, boost::asio::io_service& io_service)
{
if (worker_) return;
initialize(boost::shared_ptr<Worker>(new AsyncWorker<boost::asio::serial_port>(serial_port, io_service)));
configured_ = false;
boost::asio::serial_port_base::baud_rate current_baudrate;
serial_port.set_option(boost::asio::serial_port_base::baud_rate(4800));
boost::this_thread::sleep(boost::posix_time::milliseconds(500));
if (debug) { serial_port.get_option(current_baudrate); std::cout << "Set baudrate " << current_baudrate.value() << std::endl; }
configured_ = setBaudrate(baudrate_);
if (configured_) return;
serial_port.set_option(boost::asio::serial_port_base::baud_rate(9600));
boost::this_thread::sleep(boost::posix_time::milliseconds(500));
if (debug) { serial_port.get_option(current_baudrate); std::cout << "Set baudrate " << current_baudrate.value() << std::endl; }
configured_ = setBaudrate(baudrate_);
if (configured_) return;
serial_port.set_option(boost::asio::serial_port_base::baud_rate(19200));
boost::this_thread::sleep(boost::posix_time::milliseconds(500));
if (debug) { serial_port.get_option(current_baudrate); std::cout << "Set baudrate " << current_baudrate.value() << std::endl; }
configured_ = setBaudrate(baudrate_);
if (configured_) return;
serial_port.set_option(boost::asio::serial_port_base::baud_rate(38400));
boost::this_thread::sleep(boost::posix_time::milliseconds(500));
if (debug) { serial_port.get_option(current_baudrate); std::cout << "Set baudrate " << current_baudrate.value() << std::endl; }
configured_ = setBaudrate(baudrate_);
if (configured_) return;
serial_port.set_option(boost::asio::serial_port_base::baud_rate(baudrate_));
boost::this_thread::sleep(boost::posix_time::milliseconds(500));
if (debug) { serial_port.get_option(current_baudrate); std::cout << "Set baudrate " << current_baudrate.value() << std::endl; }
configured_ = setBaudrate(baudrate_);
if (configured_) return;
}
/// @brief tcflush - flush non-transmitted output data, non-read input data or both
/// Flush a serial port's buffers. Oll data in buffer is
///
/// @param serial_port Port to flush.
/// @param what Determines the buffers to flush.
/// @param error Set to indicate what error occurred, if any.
void SerialConnection::flush_serial_port(boost::asio::serial_port& serial_port,
flush_type what,
boost::system::error_code& error)
{
if (0 == ::tcflush(serial_port.lowest_layer().native_handle(), what))
{
error = boost::system::error_code();
}
else
{
error = boost::system::error_code(errno,
boost::asio::error::get_system_category());
}
}
/*
Reads byte into a uint8_t
res - uint8_t reference to write byte into
*/
bool read_byte(uint8_t &res)
{
while (true)
{
try
{
p.read_some(boost::asio::buffer(&res, sizeof(res)));
return true;
}
catch (const std::exception &exc)
{
ROS_ERROR_THROTTLE(0.5, "DVL: error on read: %s; reopening serial port", exc.what());
open();
return false;
}
}
}
void Controls::joy_callback(const sensor_msgs::Joy::ConstPtr& joy)
{
int leftServoAngle = 0;
int rightServoAngle = 0;
int rightMultiplier = 1;
int leftMultiplier = 1;
int leftPower = 0;
int rightPower = 0;
if(joy->buttons[rightTrigger] > 0){
rightMultiplier = 0;
}
if(joy->buttons[leftTrigger] > 0){
leftMultiplier = 0;
}
/* No Servos, because I said so */
if(joy->buttons[xButton] > 0){
leftServoAngle = 90;
rightServoAngle = 90;
}
leftPower = joy->axes[leftStickY] * (100.0 / 127.0) * leftMultiplier;
rightPower = joy->axes[leftStickY] * (100.0 / 127.0) * rightMultiplier;
leftPower = 1500 + leftPower;
rightPower = 1500 + rightPower;
const int SIZE = 11;
unsigned char packet[SIZE];
packet[0] = '-';
packet[1] = leftServoAngle >> 8;
packet[2] = leftServoAngle;
packet[3] = rightServoAngle >> 8;
packet[4] = rightServoAngle;
packet[5] = 0 >> 8;
packet[6] = 0;
packet[7] = leftPower >> 8;
packet[8] = leftPower;
packet[9] = rightPower >> 8;
packet[10] = rightPower;
s_p.write_some(boost::asio::buffer(&packet, SIZE));
}
/// @brief Flush a serial port's buffers.
///
/// @param serial_port Port to flush.
/// @param what Determines the buffers to flush.
/// @param error Set to indicate what error occurred, if any.
void flush_serial_port(
boost::asio::serial_port& serial_port,
flush_type what,
boost::system::error_code& error)
{
#if defined(BOOST_ASIO_WINDOWS)
// On windows it's a noop for now.
error = boost::system::error_code();
#else
if (0 == ::tcflush(serial_port.native_handle(), what))
{
error = boost::system::error_code();
}
else
{
error = boost::system::error_code(errno,
boost::asio::error::get_system_category());
}
#endif
}
void read(boost::optional<mil_msgs::VelocityMeasurements> &res, boost::optional<mil_msgs::RangeStamped> &height_res)
{
res = boost::none;
height_res = boost::none;
if (!p.is_open()) // Open serial port if closed
{
open();
return;
}
ByteVec ensemble;
ensemble.resize(4);
// Header ID
if (!read_byte(ensemble[0]))
return;
if (ensemble[0] != 0x7F)
return;
ros::Time stamp = ros::Time::now();
// Data Source ID
if (!read_byte(ensemble[1]))
return;
if (ensemble[1] != 0x7F)
return;
// Size low
if (!read_byte(ensemble[2]))
return;
// Size high
if (!read_byte(ensemble[3]))
return;
uint16_t ensemble_size = getu16le(ensemble.data() + 2);
ensemble.resize(ensemble_size);
for (int i = 4; i < ensemble_size; i++)
{
if (!read_byte(ensemble[i]))
return;
}
uint16_t checksum = 0;
BOOST_FOREACH (uint16_t b, ensemble)
checksum += b;
uint16_t received_checksum;
if (!read_short(received_checksum))
return;
if (received_checksum != checksum)
{
ROS_ERROR_THROTTLE(0.5, "DVL: invalid ensemble checksum. received: %i calculated: %i size: %i", received_checksum,
checksum, ensemble_size);
return;
}
if (ensemble.size() < 6)
return;
for (int dt = 0; dt < ensemble[5]; dt++)
{
int offset = getu16le(ensemble.data() + 6 + 2 * dt);
if (ensemble.size() - offset < 2)
continue;
// Three modes, encoded by the section_id: Bottom Track High Resolution Velocity
// Bottom Track, Bottom Track Range
uint16_t section_id = getu16le(ensemble.data() + offset);
std::vector<double> correlations(4, nan(""));
if (section_id == 0x5803) // Bottom Track High Resolution Velocity
{
if (ensemble.size() - offset < 2 + 4 * 4)
continue;
res = boost::make_optional(mil_msgs::VelocityMeasurements());
res->header.stamp = stamp;
std::vector<geometry_msgs::Vector3> dirs;
{
double tilt = 30 * boost::math::constants::pi<double>() / 180;
double x = sin(tilt);
double z = cos(tilt);
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(-x, 0, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(+x, 0, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, +x, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, -x, -z));
}
// Keep track of which beams didn't return for logging
std::vector<size_t> invalid_beams;
invalid_beams.reserve(4);
for (int i = 0; i < 4; i++)
{
mil_msgs::VelocityMeasurement m;
m.direction = dirs[i];
int32_t vel = gets32le(ensemble.data() + offset + 2 + 4 * i);
m.velocity = -vel * .01e-3;
if (vel == -3276801) // -3276801 indicates no data
{
invalid_beams.push_back(i + 1);
m.velocity = nan("");
}
res->velocity_measurements.push_back(m);
}
// Report a list of invalid beams
if (invalid_beams.size() > 0)
{
std::string to_log{ "DVL: didn't return bottom velocity for beam(s): " };
for (auto beam : invalid_beams)
to_log += std::to_string(beam) + " ";
ROS_ERROR_THROTTLE(0.5, "%s", to_log.c_str());
}
}
else if (section_id == 0x0600) // Bottom Track
{
for (int i = 0; i < 4; i++)
correlations[i] = *(ensemble.data() + offset + 32 + i);
}
else if (section_id == 0x5804) // Bottom Track Range
{
if (ensemble.size() - offset < 2 + 4 * 3)
continue;
if (gets32le(ensemble.data() + offset + 10) <= 0)
{
ROS_ERROR_THROTTLE(0.5, "%s", "DVL: didn't return height over bottom");
continue;
}
height_res = boost::make_optional(mil_msgs::RangeStamped());
height_res->header.stamp = stamp;
height_res->range = gets32le(ensemble.data() + offset + 10) * 0.1e-3;
}
if (res)
{
for (int i = 0; i < 4; i++)
{
res->velocity_measurements[i].correlation = correlations[i];
}
}
}
}
void abort()
{
p.close();
}
void callback(const std_msgs::Float32::ConstPtr& angle)
{
unsigned char d = (char)angle->data;
s_p.write_some(boost::asio::buffer(&d, 1));
}
void wait_callback(boost::asio::serial_port& ser_port, const boost::system::error_code& error) {
if (error) {
return;
}
ser_port.cancel();
}
void SetOption(boost::asio::serial_port& arPort, const T& arOption)
{
error_code ec;
arPort.set_option(arOption, ec);
if(ec) throw Exception(LOCATION, ec.message());
}
