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();
	}
Beispiel #2
0
  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];
        }
      }
    }
  }