Program Listing for File local_xy_util.h

↰ Return to documentation for file (/tmp/ws/src/marti_common/swri_transform_util/include/swri_transform_util/local_xy_util.h)

// *****************************************************************************
//
// Copyright (c) 2014, 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 TRANSFORM_UTIL_LOCAL_XY_UTIL_H_
#define TRANSFORM_UTIL_LOCAL_XY_UTIL_H_

#include <string>

#include <rclcpp/rclcpp.hpp>
#include <swri_transform_util/transform_util.h>
#include <geometry_msgs/msg/pose_stamped.hpp>

namespace swri_transform_util
{
  void LocalXyFromWgs84(
      double latitude,
      double longitude,
      double reference_latitude,
      double reference_longitude,
      double& x,
      double& y);

  void Wgs84FromLocalXy(
      double x,
      double y,
      double reference_latitude,
      double reference_longitude,
      double& latitude,
      double& longitude);

  class LocalXyWgs84Util
  {
  public:
    LocalXyWgs84Util(
        double reference_latitude,
        double reference_longitude,
        double reference_angle = 0,
        double reference_altitude = 0,
        rclcpp::Node::SharedPtr node = nullptr);

    explicit LocalXyWgs84Util(rclcpp::Node::SharedPtr node);

    bool Initialized() const { return initialized_; }

    void ResetInitialization();

    double ReferenceLongitude() const;

    double ReferenceLatitude() const;

    double ReferenceAngle() const;

    double ReferenceAltitude() const;

    std::string Frame() const { return frame_; }

    std::string NormalizedFrame() const { return NormalizeFrameId(frame_); }

    bool ToLocalXy(
        double latitude,
        double longitude,
        double& x,
        double& y) const;

    bool ToWgs84(
        double x,
        double y,
        double& latitude,
        double& longitude) const;

  protected:
    rclcpp::Node::SharedPtr node_;

    double reference_latitude_;   //< Reference latitude in radians.
    double reference_longitude_;  //< Reference longitude in radians.
    double reference_angle_;      //< Reference angle in radians ENU.
    double reference_altitude_;   //< Reference altitude in meters.

    double rho_lat_;
    double rho_lon_;
    double cos_angle_;
    double sin_angle_;

    std::string frame_;

    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
    bool initialized_;

    void Initialize();

    void HandleOrigin(double latitude, double longitude, double altitude, double angle, const std::string& frame_id);

    void HandlePoseStamped(geometry_msgs::msg::PoseStamped::UniquePtr pose);
  };
  typedef std::shared_ptr<LocalXyWgs84Util> LocalXyWgs84UtilPtr;
}

#endif  // TRANSFORM_UTIL_LOCAL_XY_UTIL_H_