Program Listing for File route_point.h
↰ Return to documentation for file (/tmp/ws/src/marti_common/swri_route_util/include/swri_route_util/route_point.h
)
// *****************************************************************************
//
// Copyright (c) 2016, Southwest Research Institute® (SwRI®)
// 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 Southwest Research Institute® (SwRI®) 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 <COPYRIGHT HOLDER> 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 SWRI_ROUTE_UTIL_ROUTE_POINT_H_
#define SWRI_ROUTE_UTIL_ROUTE_POINT_H_
#include <string>
#include <map>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <marti_nav_msgs/msg/route_position.hpp>
namespace swri_route_util
{
// The RoutePoint class provides a more friendly interface for working
// with the marti_nav_msgs::Route and marti_nav_msgs::RoutePoint
// message types. See comments for swri_route_util::RoutePoint for
// more information. This class is used to represent both route
// points that are part of a Route, and independent points that were
// interpolated from a Route.
class RoutePoint
{
public:
RoutePoint();
// Access to the route point's position as tf datatypes.
void setPosition(const tf2::Vector3 &position);
const tf2::Vector3& position() const;
tf2::Vector3& position();
// Access to the route point's position as message datatypes.
void setPosition(const geometry_msgs::msg::Point &position);
const geometry_msgs::msg::Point positionMsg() const;
// Access to the route point's orientation as tf datatypes.
void setOrientation(const tf2::Quaternion &orientation);
const tf2::Quaternion& orientation() const;
tf2::Quaternion& orientation();
// Access to the route point's orientation as message datatypes.
void setOrientation(const geometry_msgs::msg::Quaternion &orientation);
const geometry_msgs::msg::Quaternion orientationMsg() const;
// Access to the route point's pose (position and orientation) as
// tf datatypes.
//void setPose(const tf2::Pose &pose);
//tf2::Pose pose() const;
// Access to the route point's pose (position and orientation) as
// message datatypes.
void setPose(const geometry_msgs::msg::Pose &pose);
geometry_msgs::msg::Pose poseMsg() const;
// Access to the route point's id. Ids should be unique when used,
// but are typically not set for interpolated points.
const std::string& id() const;
void setId(const std::string &id);
// Native access to the route point "stop_point" property. This is
// a boolean property that defaults to false if it is not explicitly
// defined in the route.
bool stopPoint() const;
void setStopPoint(bool value);
// Native access to the route point "stop_point_delay" property.
// This is a floating point value that specifies how long the
// vehicle should be paused at the specific point, in seconds. The
// delay defaults to 0.0 if it not defined in the route.
double stopPointDelay() const;
void setStopPointDelay(double delay);
// Return a marti_nav_msgs::RoutePosition message that corresponds to this point.
marti_nav_msgs::msg::RoutePosition routePosition() const;
// The following methods provide general purpose access to route
// point properties. They will also correctly map to properties
// that are exposed natively. Native methods are generally faster
// and safer and should be preferred when available. Note: We have
// not yet added any native properties for the route point.
// Get a list of all the properties defined for this route point,
// including native properties.
std::vector<std::string> getPropertyNames() const;
// Get the value of a property. Returns an empty string if the
// property does not exist.
std::string getProperty(const std::string &name) const;
// Get the value of a property, lexically cast to a known type.
// Returns the lexical cast of an empty string if the property does
// not exist..
template <typename T>
T getTypedProperty(const std::string &name) const;
// Determine if the specified property is defined for the route
// point.
bool hasProperty(const std::string &name) const;
// Set the value of a property. If the property doesn't exist, it
// is added.
void setProperty(const std::string &name, const std::string &value);
// Delete a property. If the property doesn't exist or is not
// deletable (e.g. name, guid), this method does nothing.
void deleteProperty(const std::string &name);
private:
tf2::Vector3 position_;
tf2::Quaternion orientation_;
std::string id_;
bool stop_point_;
double stop_point_delay_;
std::map<std::string, std::string> properties_;
}; // class RoutePoint
} // namespace swri_route_util
#include "route_point_inline.h"
#endif // SWRI_ROUTE_UTIL_ROUTE_POINT_H_