Exemple #1
0
	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;
	}
Exemple #2
0
  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();
	}
Exemple #4
0
 /*
   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));
	}
Exemple #6
0
  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());
    }
  }
Exemple #7
0
  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;
	}
Exemple #9
0
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;
}
Exemple #10
0
/// @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());
  }
}
Exemple #11
0
 /*
   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;
     }
   }
 }
Exemple #12
0
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));
}
Exemple #13
0
  /// @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
  }
Exemple #14
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];
        }
      }
    }
  }
Exemple #15
0
 void abort()
 {
   p.close();
 }
Exemple #16
0
 void callback(const std_msgs::Float32::ConstPtr& angle)
 {
   unsigned char d = (char)angle->data;
   s_p.write_some(boost::asio::buffer(&d, 1));
 }
Exemple #17
0
void wait_callback(boost::asio::serial_port& ser_port, const boost::system::error_code& error) {
    if (error) {
        return;
    }
    ser_port.cancel();
}
Exemple #18
0
	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());
	}