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"
#include "psen_scan_v2_standalone/configuration/scanner_ids.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_;
  using ScannerId = psen_scan_v2_standalone::configuration::ScannerId;
  std::unordered_map<ScannerId, ros::Publisher> pubs_scan_;
  ros::Publisher pub_zone_;
  ros::Publisher pub_io_;
  std::unordered_map<ScannerId, std::string> tf_prefixes_;
  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)
  , x_axis_rotation_(x_axis_rotation)
  , scanner_(scanner_config, std::bind(&ROSScannerNodeT<S>::laserScanCallback, this, std::placeholders::_1))
{
  pubs_scan_.insert(std::make_pair(ScannerId::master, nh_.advertise<sensor_msgs::LaserScan>(topic, 1)));
  tf_prefixes_.insert(std::make_pair(ScannerId::master, tf_prefix));
  for (int i = 0; i < scanner_config.nrSubscribers(); i++)
  {
    ScannerId id = psen_scan_v2_standalone::configuration::subscriber_number_to_scanner_id(i);
    std::string topic_subscriber = topic + "_" + psen_scan_v2_standalone::configuration::SCANNER_ID_TO_STRING.at(id);
    pubs_scan_.insert(std::make_pair(id, nh_.advertise<sensor_msgs::LaserScan>(topic_subscriber, 1)));
    std::string tf_prefix_subscriber =
        tf_prefix + "_" + psen_scan_v2_standalone::configuration::SCANNER_ID_TO_STRING.at(id);
    tf_prefixes_.insert(std::make_pair(id, tf_prefix_subscriber));
  }
  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
  {
    std::string tf_prefix_with_subscriber = tf_prefixes_.at(scan.scannerId());
    const auto laser_scan_msg = toLaserScanMsg(scan, tf_prefix_with_subscriber, 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());
    pubs_scan_.at(scan.scannerId()).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)
{
  std::string tf_prefix_with_subscriber = tf_prefixes_.at(ScannerId::master);  // assuming all IO is at the master
  for (const auto& io : io_states)
  {
    if (last_io_state_ != io)
    {
      pub_io_.publish(toIOStateMsg(io, tf_prefix_with_subscriber));
 
      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