ros_scanner_node.h

Go to the documentation of this file.

// Copyright (c) 2020-2022 Pilz GmbH & Co. KG
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.

#ifndef PSEN_SCAN_V2_ROS_SCANNER_NODE_H
#define PSEN_SCAN_V2_ROS_SCANNER_NODE_H

#include <stdexcept>
#include <string>
#include <atomic>
#include <chrono>
#include <future>
#include <algorithm>

#include <fmt/format.h>

#include <gtest/gtest_prod.h>

#include <ros/ros.h>
#include <std_msgs/UInt8.h>
#include <sensor_msgs/LaserScan.h>

#include "psen_scan_v2_standalone/scanner_v2.h"

#include "psen_scan_v2/laserscan_ros_conversions.h"
#include "psen_scan_v2/io_state_ros_conversion.h"
#include "psen_scan_v2_standalone/data_conversion_layer/angle_conversions.h"
#include "psen_scan_v2_standalone/util/format_range.h"

namespace psen_scan_v2
{
using namespace psen_scan_v2_standalone;
using namespace std::chrono_literals;

template <typename S = ScannerV2>
class ROSScannerNodeT
{
public:
  ROSScannerNodeT(ros::NodeHandle& nh,
                  const std::string& topic,
                  const std::string& tf_prefix,
                  const double& x_axis_rotation,
                  const ScannerConfiguration& scanner_config);

  void run();
  void terminate();

private:
  void laserScanCallback(const LaserScan& scan);
  void publishChangedIOStates(const std::vector<psen_scan_v2_standalone::IOState>& io_states);

private:
  ros::NodeHandle nh_;
  ros::Publisher pub_scan_;
  ros::Publisher pub_zone_;
  ros::Publisher pub_io_;
  std::string tf_prefix_;
  double x_axis_rotation_;
  S scanner_;
  std::atomic_bool terminate_{ false };

  psen_scan_v2_standalone::IOState last_io_state_{};

  friend class RosScannerNodeTests;
  FRIEND_TEST(RosScannerNodeTests, shouldStartAndStopSuccessfullyIfScannerRespondsToRequests);
  FRIEND_TEST(RosScannerNodeTests, shouldPublishScansWhenLaserScanCallbackIsInvoked);
  FRIEND_TEST(RosScannerNodeTests, shouldPublishActiveZonesetWhenLaserScanCallbackIsInvoked);
  FRIEND_TEST(RosScannerNodeTests, shouldWaitWhenStopRequestResponseIsMissing);
  FRIEND_TEST(RosScannerNodeTests, shouldProvideScanTopic);
  FRIEND_TEST(RosScannerNodeTests, shouldProvideActiveZonesetTopic);
  FRIEND_TEST(RosScannerNodeTests, shouldPublishScanEqualToConversionOfSuppliedLaserScan);
  FRIEND_TEST(RosScannerNodeTests, shouldThrowExceptionSetInScannerStartFuture);
  FRIEND_TEST(RosScannerNodeTests, shouldThrowExceptionSetInScannerStopFuture);
  FRIEND_TEST(RosScannerNodeTests, shouldPublishChangedIOStatesEqualToConversionOfSuppliedStandaloneIOStates);
  FRIEND_TEST(RosScannerNodeTests, shouldPublishLatchedOnIOStatesTopic);
  FRIEND_TEST(RosScannerNodeTests, shouldLogChangedIOStates);
};

typedef ROSScannerNodeT<> ROSScannerNode;

template <typename S>
ROSScannerNodeT<S>::ROSScannerNodeT(ros::NodeHandle& nh,
                                    const std::string& topic,
                                    const std::string& tf_prefix,
                                    const double& x_axis_rotation,
                                    const ScannerConfiguration& scanner_config)
  : nh_(nh)
  , tf_prefix_(tf_prefix)
  , x_axis_rotation_(x_axis_rotation)
  , scanner_(scanner_config, std::bind(&ROSScannerNodeT<S>::laserScanCallback, this, std::placeholders::_1))
{
  pub_scan_ = nh_.advertise<sensor_msgs::LaserScan>(topic, 1);
  pub_zone_ = nh_.advertise<std_msgs::UInt8>("active_zoneset", 1);
  pub_io_ = nh_.advertise<psen_scan_v2::IOState>("io_state", 6, true /* latched */);
}

template <typename S>
void ROSScannerNodeT<S>::laserScanCallback(const LaserScan& scan)
{
  try
  {
    const auto laser_scan_msg = toLaserScanMsg(scan, tf_prefix_, x_axis_rotation_);
    PSENSCAN_INFO_ONCE(
        "ScannerNode",
        "Publishing laser scan with angle_min={:.1f} angle_max={:.1f} angle_increment={:.1f} degrees. {} angle values.",
        data_conversion_layer::radianToDegree(laser_scan_msg.angle_min),
        data_conversion_layer::radianToDegree(laser_scan_msg.angle_max),
        data_conversion_layer::radianToDegree(laser_scan_msg.angle_increment),
        laser_scan_msg.ranges.size());
    pub_scan_.publish(laser_scan_msg);

    std_msgs::UInt8 active_zoneset;
    active_zoneset.data = scan.activeZoneset();
    pub_zone_.publish(active_zoneset);

    publishChangedIOStates(scan.ioStates());
  }
  // LCOV_EXCL_START
  catch (const std::invalid_argument& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
  // LCOV_EXCL_STOP
}

template <typename S>
void ROSScannerNodeT<S>::publishChangedIOStates(const std::vector<psen_scan_v2_standalone::IOState>& io_states)
{
  for (const auto& io : io_states)
  {
    if (last_io_state_ != io)
    {
      pub_io_.publish(toIOStateMsg(io, tf_prefix_));

      PSENSCAN_INFO("RosScannerNode",
                    "IOs changed, new input: {}, new output: {}",
                    formatPinStates(io.changedInputStates(last_io_state_)),
                    formatPinStates(io.changedOutputStates(last_io_state_)));
      last_io_state_ = io;
    }
  }
}

template <typename S>
void ROSScannerNodeT<S>::terminate()
{
  terminate_ = true;
}

template <typename S>
void ROSScannerNodeT<S>::run()
{
  ros::Rate r(10);
  auto start_future = scanner_.start();
  const auto start_status = start_future.wait_for(3s);
  if (start_status == std::future_status::ready)
  {
    start_future.get();  // Throws std::runtime_error if start not successful
  }

  while (ros::ok() && !terminate_)
  {
    r.sleep();  // LCOV_EXCL_LINE can not be reached deterministically
  }
  auto stop_future = scanner_.stop();

  const auto stop_status = stop_future.wait_for(3s);
  if (stop_status == std::future_status::ready)
  {
    stop_future.get();  // Throws std::runtime_error if stop not successful
  }
}

}  // namespace psen_scan_v2

#endif  // PSEN_SCAN_V2_ROS_SCANNER_NODE_H