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(); }
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]; } } } }