speed_observer.h

Go to the documentation of this file.

/*
 * Copyright (c) 2019 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 <http://www.gnu.org/licenses/>.
 */

#ifndef SPEED_OBSERVER_H
#define SPEED_OBSERVER_H

#include <atomic>
#include <map>
#include <vector>

#include <ros/node_handle.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

#include <prbt_hardware_support/FrameSpeeds.h>
#include <prbt_hardware_support/SetSpeedLimit.h>

namespace prbt_hardware_support
{
static constexpr unsigned int DEFAULT_ALLOWED_MISSED_CALCULATIONS{3};

class SpeedObserver
{
public:
  SpeedObserver(ros::NodeHandle& nh, std::string& reference_frame, std::vector<std::string>& frames_to_observe);

public:
  void startObserving(const double frequency, const unsigned int allowed_missed_calculations = DEFAULT_ALLOWED_MISSED_CALCULATIONS);

  bool setSpeedLimitCb(SetSpeedLimit::Request& req, SetSpeedLimit::Response& res);

  void terminateNow();

private:
  void triggerStop1();

  FrameSpeeds createFrameSpeedsMessage(const std::map<std::string, double>& speeds) const;

  void waitUntillCanTransform(const std::string& frame, const ros::Time& time,
                              const unsigned short int max_num_retries = 10) const;

  std::pair<tf2::Vector3, ros::Time> getLatestPose(const std::string& frame) const;

  bool isWithinLimit(const double& speed) const;

private:
  static double speedFromTwoPoses(const tf2::Vector3& a, const tf2::Vector3& b, const double& t);

private:
  ros::NodeHandle nh_;
  ros::Publisher frame_speeds_pub_;
  ros::ServiceClient run_permitted_client_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener{ tf_buffer_ };

  const std::string reference_frame_;
  const std::vector<std::string> frames_to_observe_;
  std::atomic_bool terminate_{ false };
  double current_speed_limit_{ DEFAULT_SPEED_LIMIT };
  std::map<std::string, unsigned int> missed_calculations_;

private:
  static constexpr double DEFAULT_SPEED_LIMIT{ .25 };
  static constexpr uint32_t DEFAULT_QUEUE_SIZE{ 10 };
  static constexpr double WAITING_TIME_FOR_TRANSFORM_S{ 0.1 };
  static constexpr double TIME_INTERVAL_EPSILON_S{ 1e-9 };
};

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

inline double SpeedObserver::speedFromTwoPoses(const tf2::Vector3& a, const tf2::Vector3& b, const double& t)
{
  ROS_ASSERT(t != 0);
  double d = tf2::tf2Distance(a, b);
  return d / t;
}

inline bool SpeedObserver::isWithinLimit(const double& speed) const
{
  return speed < current_speed_limit_;
}

inline void SpeedObserver::terminateNow()
{
  ROS_DEBUG("terminateNow");
  terminate_ = true;
}

}  // namespace prbt_hardware_support

#endif  // SPEED_OBSERVER_H