Program Listing for File timestamp_manager.hpp
↰ Return to documentation for file (/tmp/ws/src/fuse/fuse_core/include/fuse_core/timestamp_manager.hpp
)
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * 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.
* * 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.
*/
#ifndef FUSE_CORE__TIMESTAMP_MANAGER_HPP_
#define FUSE_CORE__TIMESTAMP_MANAGER_HPP_
#include <functional>
#include <map>
#include <vector>
#include <boost/range/any_range.hpp>
#include <fuse_core/constraint.hpp>
#include <fuse_core/fuse_macros.hpp>
#include <fuse_core/transaction.hpp>
#include <fuse_core/variable.hpp>
#include <rclcpp/duration.hpp>
namespace fuse_core
{
class TimestampManager
{
public:
FUSE_SMART_PTR_DEFINITIONS(TimestampManager)
using MotionModelFunction = std::function<void (const rclcpp::Time & beginning_stamp,
const rclcpp::Time & ending_stamp,
std::vector<Constraint::SharedPtr> & constraints,
std::vector<Variable::SharedPtr> & variables)>;
using const_stamp_range = boost::any_range<const rclcpp::Time, boost::forward_traversal_tag>;
explicit TimestampManager(
MotionModelFunction generator,
const rclcpp::Duration & buffer_length = rclcpp::Duration::max());
template<class T>
TimestampManager(
void(T::* fp)(
const rclcpp::Time & beginning_stamp,
const rclcpp::Time & ending_stamp,
std::vector<Constraint::SharedPtr> & constraints,
std::vector<Variable::SharedPtr> & variables),
T * obj,
const rclcpp::Duration & buffer_length = rclcpp::Duration::max());
template<class T>
TimestampManager(
void(T::* fp)(
const rclcpp::Time & beginning_stamp,
const rclcpp::Time & ending_stamp,
std::vector<Constraint::SharedPtr> & constraints,
std::vector<Variable::SharedPtr> & variables) const,
T * obj,
const rclcpp::Duration & buffer_length = rclcpp::Duration::max());
virtual ~TimestampManager() = default;
const rclcpp::Duration & bufferLength() const
{
return buffer_length_;
}
void bufferLength(const rclcpp::Duration & buffer_length)
{
buffer_length_ = buffer_length;
}
void clear()
{
motion_model_history_.clear();
}
void query(Transaction & transaction, bool update_variables = false);
const_stamp_range stamps() const;
protected:
struct MotionModelSegment
{
rclcpp::Time beginning_stamp;
rclcpp::Time ending_stamp;
std::vector<Constraint::SharedPtr> constraints;
std::vector<Variable::SharedPtr> variables;
explicit MotionModelSegment(rcl_clock_type_t clock_t)
: beginning_stamp{0, 0, clock_t},
ending_stamp{0, 0, clock_t}
{
}
MotionModelSegment(
const rclcpp::Time & beginning_stamp,
const rclcpp::Time & ending_stamp,
const std::vector<Constraint::SharedPtr> & constraints,
const std::vector<Variable::SharedPtr> & variables)
: beginning_stamp(beginning_stamp),
ending_stamp(ending_stamp),
constraints(constraints),
variables(variables)
{
}
};
using MotionModelHistory = std::map<rclcpp::Time, MotionModelSegment>;
MotionModelFunction generator_;
rclcpp::Duration buffer_length_;
MotionModelHistory motion_model_history_;
void addSegment(
const rclcpp::Time & beginning_stamp,
const rclcpp::Time & ending_stamp,
Transaction & transaction);
void removeSegment(
MotionModelHistory::iterator & iter,
Transaction & transaction);
void splitSegment(
MotionModelHistory::iterator & iter,
const rclcpp::Time & stamp,
Transaction & transaction);
void purgeHistory();
};
template<class T>
TimestampManager::TimestampManager(
void(T::* fp)(
const rclcpp::Time & beginning_stamp,
const rclcpp::Time & ending_stamp,
std::vector<Constraint::SharedPtr> & constraints,
std::vector<Variable::SharedPtr> & variables),
T * obj,
const rclcpp::Duration & buffer_length)
: TimestampManager(std::bind(fp,
obj,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3,
std::placeholders::_4),
buffer_length)
{
}
template<class T>
TimestampManager::TimestampManager(
void(T::* fp)(
const rclcpp::Time & beginning_stamp,
const rclcpp::Time & ending_stamp,
std::vector<Constraint::SharedPtr> & constraints,
std::vector<Variable::SharedPtr> & variables) const,
T * obj,
const rclcpp::Duration & buffer_length)
: TimestampManager(std::bind(fp,
obj,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3,
std::placeholders::_4),
buffer_length)
{
}
} // namespace fuse_core
#endif // FUSE_CORE__TIMESTAMP_MANAGER_HPP_