os32c_node.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <boost/shared_ptr.hpp>
#include <boost/range/algorithm.hpp>
#include <diagnostic_updater/publisher.h>
#include <sensor_msgs/LaserScan.h>

#include <rosconsole_bridge/bridge.h>
REGISTER_ROSCONSOLE_BRIDGE;

#include "odva_ethernetip/socket/tcp_socket.h"
#include "odva_ethernetip/socket/udp_socket.h"
#include "omron_os32c_driver/os32c.h"
#include "omron_os32c_driver/range_and_reflectance_measurement.h"

using std::cout;
using std::endl;
using boost::shared_ptr;
using boost::range::reverse;
using sensor_msgs::LaserScan;
using eip::socket::TCPSocket;
using eip::socket::UDPSocket;
using namespace omron_os32c_driver;
using namespace diagnostic_updater;
const double EPS = 1e-3;


int main(int argc, char* argv[])
{
  ros::init(argc, argv, "os32c");
  ros::NodeHandle nh;

  // get sensor config from params
  string host, frame_id, local_ip;
  double start_angle, end_angle, expected_frequency, frequency_tolerance, timestamp_min_acceptable,
      timestamp_max_acceptable, frequency, reconnect_timeout;
  bool publish_intensities;
  bool invert_scan;
  ros::param::param<std::string>("~host", host, "192.168.1.1");
  ros::param::param<std::string>("~local_ip", local_ip, "0.0.0.0");
  ros::param::param<std::string>("~frame_id", frame_id, "laser");
  ros::param::param<double>("~start_angle", start_angle, OS32C::ANGLE_MAX);
  ros::param::param<double>("~end_angle", end_angle, OS32C::ANGLE_MIN);
  ros::param::param<double>("~frequency", frequency, 12.856);
  ros::param::param<double>("~expected_frequency", expected_frequency, frequency);
  ros::param::param<double>("~frequency_tolerance", frequency_tolerance, 0.1);
  ros::param::param<double>("~timestamp_min_acceptable", timestamp_min_acceptable, -1);
  ros::param::param<double>("~timestamp_max_acceptable", timestamp_max_acceptable, -1);
  ros::param::param<double>("~reconnect_timeout", reconnect_timeout, 2.0);
  ros::param::param<bool>("~publish_intensities", publish_intensities, false);
  ros::param::param<bool>("~invert_scan", invert_scan, false);

  // publisher for laserscans
  ros::Publisher laserscan_pub = nh.advertise<LaserScan>("scan", 1);

  // Validate frequency parameters
  if (frequency > 25)
  {
    ROS_FATAL("Frequency exceeds the limit of 25hz.");
    return -1;
  }
  else if (frequency <= 0)
  {
    ROS_FATAL("Frequency should be positive");
    return -1;
  }

  if (fabs(frequency - expected_frequency) > EPS)
  {
    ROS_WARN("Frequency parameter is not equal to expected frequency parameter.");
  }

  // initialize loop rate
  ros::Rate loop_rate(frequency);

  // diagnostics for frequency
  Updater updater;
  updater.setHardwareID(host);
  DiagnosedPublisher<LaserScan> diagnosed_publisher(
      laserscan_pub, updater, FrequencyStatusParam(&expected_frequency, &expected_frequency, frequency_tolerance),
      TimeStampStatusParam(timestamp_min_acceptable, timestamp_max_acceptable));

  while (ros::ok())
  {
    boost::asio::io_service io_service;
    shared_ptr<TCPSocket> socket = shared_ptr<TCPSocket>(new TCPSocket(io_service));
    shared_ptr<UDPSocket> io_socket = shared_ptr<UDPSocket>(new UDPSocket(io_service, 2222, local_ip));
    OS32C os32c(socket, io_socket);

    try
    {
      os32c.open(host);
    }
    catch (std::runtime_error ex)
    {
      ROS_ERROR("Exception caught opening session: %s. Reconnecting in %.2f seconds ...", ex.what(), reconnect_timeout);
      ros::Duration(reconnect_timeout).sleep();
      continue;
    }

    try
    {
      os32c.setRangeFormat(RANGE_MEASURE_50M);
      os32c.setReflectivityFormat(REFLECTIVITY_MEASURE_TOT_4PS);
      os32c.selectBeams(start_angle, end_angle);
    }
    catch (std::invalid_argument ex)
    {
      ROS_ERROR("Invalid arguments in sensor configuration: %s. Reconnecting in %.2f seconds ...", ex.what(),
                reconnect_timeout);
      ros::Duration(reconnect_timeout).sleep();
      continue;
    }

    sensor_msgs::LaserScan laserscan_msg;
    os32c.fillLaserScanStaticConfig(&laserscan_msg);
    laserscan_msg.header.frame_id = frame_id;

    while (ros::ok())
    {
      try
      {
        // Poll ranges and reflectivity
        RangeAndReflectanceMeasurement report = os32c.getSingleRRScan();

        // Invert range measurements if z-axis is needed to point upwards.
        if (invert_scan)
        {
          reverse(report.range_data);
        }

        OS32C::convertToLaserScan(report, &laserscan_msg);

        // In earlier versions reflectivity was not received. So to be backwards
        // compatible clear reflectivity from msg.
        if (!publish_intensities)
        {
          laserscan_msg.intensities.clear();
        }

        // Stamp and publish message diagnosed
        laserscan_msg.header.stamp = ros::Time::now();
        laserscan_msg.header.seq++;
        diagnosed_publisher.publish(laserscan_msg);

        // Update diagnostics
        updater.update();
      }
      catch (std::runtime_error ex)
      {
        ROS_ERROR_STREAM_DELAYED_THROTTLE(5.0, "Exception caught requesting scan data: " << ex.what());
      }
      catch (std::logic_error ex)
      {
        ROS_ERROR_STREAM("Problem parsing return data: " << ex.what());
      }

      if (!laserscan_msg.header.stamp.isZero() &&
          (ros::Time::now() - laserscan_msg.header.stamp).toSec() > reconnect_timeout)
      {
        ROS_ERROR("No scan received for %.2f seconds, reconnecting ...", reconnect_timeout);
        laserscan_msg.header.stamp = ros::Time();
        break;
      }

      ros::spinOnce();

      // sleep
      loop_rate.sleep();
    }

    if (!ros::ok())
    {
      os32c.closeActiveConnection();
      os32c.close();
    }
  }

  return 0;
}