environment.h

Go to the documentation of this file.

/*
 * Copyright (c) 2018, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

//
// Base class for all environment models, providing separate collision check
// functions for each type of tracking error bound. All environments are
// boxes in 3D space. Environment is templated on the specific type of sensor
// message (M) which may be generated from or incorporated into a derived class,
// and sensor parameters (P) which may be used to generate sensor readings.
//

#ifndef FASTRACK_ENVIRONMENT_ENVIRONMENT_H
#define FASTRACK_ENVIRONMENT_ENVIRONMENT_H

#include <fastrack/bound/box.h>
#include <fastrack/bound/cylinder.h>
#include <fastrack/bound/sphere.h>
#include <fastrack/bound/tracking_bound.h>
#include <fastrack/utils/types.h>

#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <visualization_msgs/Marker.h>

namespace fastrack {
namespace environment {

using bound::Box;
using bound::Sphere;
using bound::Cylinder;
using bound::TrackingBound;

template <typename M, typename P>
class Environment {
 public:
  virtual ~Environment() {}

  // Initialize from a ROS NodeHandle.
  bool Initialize(const ros::NodeHandle& n);

  // Derived classes must provide a collision checker which returns true if
  // and only if the provided position is a valid collision-free configuration.
  virtual bool IsValid(
      const Vector3d& position, const TrackingBound& bound,
      double time = std::numeric_limits<double>::quiet_NaN()) const = 0;

  // Utility for checking multiple positions.
  bool AreValid(const std::vector<Vector3d>& positions,
                const TrackingBound& bound,
                double time = std::numeric_limits<double>::quiet_NaN()) const {
    // Return Boolean AND of all IsValid calls.
    for (const auto& p : positions) {
      if (!IsValid(p, bound, time)) return false;
    }

    return true;
  }

  // Generate a sensor measurement.
  virtual M SimulateSensor(const P& params) const = 0;

  // Derived classes must have some sort of visualization through RViz.
  virtual void Visualize() const = 0;

 protected:
  explicit Environment() : initialized_(false) {}

  // Load parameters. This may be overridden by derived classes if needed
  // (they should still call this one via Environment::LoadParameters).
  virtual bool LoadParameters(const ros::NodeHandle& n);

  // Register callbacks.
  virtual bool RegisterCallbacks(const ros::NodeHandle& n);

  // Update this environment with the information contained in the given
  // sensor measurement.
  // NOTE! This function needs to publish on `updated_topic_`.
  virtual void SensorCallback(const typename M::ConstPtr& msg) = 0;

  // Upper and lower bounds.
  Vector3d lower_;
  Vector3d upper_;

  // Publishers and subscribers.
  ros::Publisher vis_pub_;
  ros::Publisher updated_pub_;
  ros::Subscriber sensor_sub_;

  std::string vis_topic_;
  std::string updated_topic_;
  std::string sensor_topic_;

  // Frame in which to publish visualization.
  std::string fixed_frame_;

  // Naming and initialization.
  std::string name_;
  bool initialized_;
};  //\class Environment

// ----------------------------- IMPLEMENTATION ----------------------------- //

// Initialize from a ROS NodeHandle.
template <typename M, typename P>
bool Environment<M, P>::Initialize(const ros::NodeHandle& n) {
  name_ = ros::names::append(n.getNamespace(), "Environment");

  if (!LoadParameters(n)) {
    ROS_ERROR("%s: Failed to load parameters.", name_.c_str());
    return false;
  }

  if (!RegisterCallbacks(n)) {
    ROS_ERROR("%s: Failed to register callbacks.", name_.c_str());
    return false;
  }

  // Visualize.
  Visualize();

  initialized_ = true;
  return true;
}

// Load parameters. This may be overridden by derived classes if needed
// (they should still call this one via Environment::LoadParameters).
template <typename M, typename P>
bool Environment<M, P>::LoadParameters(const ros::NodeHandle& n) {
  ros::NodeHandle nl(n);

  // Sensor topic/service.
  if (!nl.getParam("topic/sensor_sub", sensor_topic_)) return false;
  if (!nl.getParam("topic/updated_env", updated_topic_)) return false;
  if (!nl.getParam("vis/env", vis_topic_)) return false;

  // Frame of reference to publish visualization in.
  if (!nl.getParam("frame/fixed", fixed_frame_)) return false;

  // Upper and lower bounds of the environment.
  if (!nl.getParam("env/upper/x", upper_(0))) return false;
  if (!nl.getParam("env/upper/y", upper_(1))) return false;
  if (!nl.getParam("env/upper/z", upper_(2))) return false;

  if (!nl.getParam("env/lower/x", lower_(0))) return false;
  if (!nl.getParam("env/lower/y", lower_(1))) return false;
  if (!nl.getParam("env/lower/z", lower_(2))) return false;

  return true;
}

// Register callbacks.
template <typename M, typename P>
bool Environment<M, P>::RegisterCallbacks(const ros::NodeHandle& n) {
  ros::NodeHandle nl(n);

  // Subscribers.
  sensor_sub_ = nl.subscribe(sensor_topic_.c_str(), 1,
                             &Environment<M, P>::SensorCallback, this);

  // Publishers.
  vis_pub_ =
      nl.advertise<visualization_msgs::Marker>(vis_topic_.c_str(), 1, false);

  updated_pub_ =
      nl.advertise<std_msgs::Empty>(updated_topic_.c_str(), 1, false);

  return true;
}

}  //\namespace environment
}  //\namespace fastrack

#endif