path.h

#ifndef PATH_H
#define PATH_H

#include <iostream>
#include <cstring>
#include <vector>

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <visualization_msgs/MarkerArray.h>

#include "node3d.h"
#include "constants.h"
#include "helper.h"
namespace HybridAStar {
class Path {
 public:
  Path(bool smoothed = false) {
    std::string pathTopic = "/path";
    std::string pathNodesTopic = "/pathNodes";
    std::string pathVehicleTopic = "/pathVehicle";

    if (smoothed) {
      pathTopic = "/sPath";
      pathNodesTopic = "/sPathNodes";
      pathVehicleTopic = "/sPathVehicle";
      this->smoothed = smoothed;
    }

    // _________________
    // TOPICS TO PUBLISH
    pubPath = n.advertise<nav_msgs::Path>(pathTopic, 1);
    pubPathNodes = n.advertise<visualization_msgs::MarkerArray>(pathNodesTopic, 1);
    pubPathVehicles = n.advertise<visualization_msgs::MarkerArray>(pathVehicleTopic, 1);

    // CONFIGURE THE CONTAINER
    path.header.frame_id = "path";
  }

  //  // __________
  //  // TRACE PATH
  //  /*!
  //     \brief Given a node pointer the path to the root node will be traced recursively
  //     \param node a 3D node, usually the goal node
  //     \param i a parameter for counting the number of nodes
  //  */
  //  void tracePath(const Node3D* node, int i = 0);
  void updatePath(std::vector<Node3D> nodePath);
  void addSegment(const Node3D& node);
  void addNode(const Node3D& node, int i);
  void addVehicle(const Node3D& node, int i);

  // ______________
  // PUBLISH METHODS

  void clear();
  void publishPath() { pubPath.publish(path); }
  void publishPathNodes() { pubPathNodes.publish(pathNodes); }
  void publishPathVehicles() { pubPathVehicles.publish(pathVehicles); }

 private:
  ros::NodeHandle n;
  ros::Publisher pubPath;
  ros::Publisher pubPathNodes;
  ros::Publisher pubPathVehicles;
  nav_msgs::Path path;
  visualization_msgs::MarkerArray pathNodes;
  visualization_msgs::MarkerArray pathVehicles;
  bool smoothed = false;
};
}
#endif // PATH_H