text: /*
   * Copyright 2016-2020 Autoware Foundation. All rights reserved.
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   *     http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  
  /*
   * ROSHelpers.h
   *
   *  Created on: Jun 30, 2016
   *      Author: Hatem Darweesh
   */
  
  #ifndef ROSHELPERS_H_
  #define ROSHELPERS_H_
  
  #include <ros/ros.h>
  
  #include "vector_map_msgs/PointArray.h"
  #include "vector_map_msgs/LaneArray.h"
  #include "vector_map_msgs/NodeArray.h"
  #include "vector_map_msgs/StopLineArray.h"
  #include "vector_map_msgs/DTLaneArray.h"
  
  #include <geometry_msgs/Vector3Stamped.h>
  #include <geometry_msgs/PoseWithCovarianceStamped.h>
  #include <geometry_msgs/PoseStamped.h>
  
  #include "libwaypoint_follower/libwaypoint_follower.h"
  #include "autoware_msgs/LaneArray.h"
  
  #include <visualization_msgs/MarkerArray.h>
  
  #include <tf/transform_broadcaster.h>
  #include <tf/transform_listener.h>
  #include <tf/tf.h>
  #include "op_planner/RoadNetwork.h"
  
  #include <string>
  #include <vector>
  
  namespace WayPlannerNS
  {
  enum MSG_TYPE{COMMAND_MSG = 0, CONFIRM_MSG = 1, OPTIONS_MSG = 2, CURR_OPTION_MSG = 3, UNKNOWN_MSG = 5};
  
  class HMI_MSG
  {
    public:
      MSG_TYPE type;
      std::vector<PlannerHNS::ACTION_TYPE> options;
      PlannerHNS::ACTION_TYPE current;
      int currID;
      bool bErr;
      std::string err_msg;
      HMI_MSG()
      {
        currID = -1;
        type = OPTIONS_MSG;
        current = PlannerHNS::FORWARD_ACTION;
        bErr = false;
      }
  
      static HMI_MSG FromString(std::string msg)
      {
        HMI_MSG recieved_msg;
        std::vector<std::string> sections = SplitString(msg, ",");
        if (sections.size() == 6)
        {
          int type_str = atoi(sections.at(0).c_str());
          switch (type_str)
          {
          case 0:
            recieved_msg.type = COMMAND_MSG;
            break;
          case 1:
            recieved_msg.type = CONFIRM_MSG;
            break;
          case 2:
            recieved_msg.type = OPTIONS_MSG;
            break;
          case 3:
            recieved_msg.type = CURR_OPTION_MSG;
            break;
          default:
            recieved_msg.type = UNKNOWN_MSG;
            break;
          }
  
          std::vector<std::string> directions = SplitString(sections.at(1), ";");
          for (unsigned int i = 0; i < directions.size(); i++)
          {
            int idirect = atoi(directions.at(i).c_str());
            if (idirect == 0)
            {
              recieved_msg.options.push_back(PlannerHNS::FORWARD_ACTION);
            }
            else if (idirect == 3)
            {
              recieved_msg.options.push_back(PlannerHNS::LEFT_TURN_ACTION);
            }
            else if (idirect == 4)
            {
              recieved_msg.options.push_back(PlannerHNS::RIGHT_TURN_ACTION);
            }
          }
          recieved_msg.currID = atoi(sections.at(3).c_str());
          recieved_msg.bErr = atoi(sections.at(4).c_str());
          recieved_msg.err_msg = sections.at(5);
        }
        return recieved_msg;
      }
  
      static std::vector<std::string> SplitString(const std::string& str, const std::string& token)
      {
        std::vector<std::string> str_parts;
        int iFirstPart = 0;
        int iSecondPart = str.find(token, iFirstPart);
  
        while ((iSecondPart > 0) && (iSecondPart < str.size()))
        {
          str_parts.push_back(str.substr(iFirstPart, iSecondPart - iFirstPart));
          iFirstPart = iSecondPart+1;
          iSecondPart = str.find(token, iFirstPart);
        }
  
        return str_parts;
      }
  };
  
  class ROSHelpers
  {
    public:
      ROSHelpers();
      virtual ~ROSHelpers();
      static void GetTransformFromTF(const std::string parent_frame, const std::string child_frame,
            tf::StampedTransform &transform);
      static void ConvertFromPlannerHToAutowarePathFormat(const std::vector<PlannerHNS::WayPoint>& path,
            autoware_msgs::LaneArray& laneArray);
  
      static void ConvertFromPlannerHToAutowareVisualizePathFormat(const std::vector<PlannerHNS::WayPoint>& curr_path,
          const std::vector<std::vector<PlannerHNS::WayPoint>>& paths,
            visualization_msgs::MarkerArray& markerArray);
  
      static void ConvertFromPlannerHToAutowareVisualizePathFormat(
            const std::vector<std::vector<PlannerHNS::WayPoint>>& globalPaths,
            visualization_msgs::MarkerArray& markerArray);
  
      static void ConvertFromRoadNetworkToAutowareVisualizeMapFormat(const PlannerHNS::RoadNetwork& map,
            visualization_msgs::MarkerArray& markerArray);
  
      static void createGlobalLaneArrayMarker(std_msgs::ColorRGBA color,
            const autoware_msgs::LaneArray &lane_waypoints_array, visualization_msgs::MarkerArray& markerArray);
  
      static void createGlobalLaneArrayVelocityMarker(const autoware_msgs::LaneArray &lane_waypoints_array,
            visualization_msgs::MarkerArray& markerArray);
  
      static void createGlobalLaneArrayOrientationMarker(const autoware_msgs::LaneArray &lane_waypoints_array,
            visualization_msgs::MarkerArray& markerArray);
  
      static void ConvertFromPlannerHPointsToAutowarePathFormat(const std::vector<PlannerHNS::GPSPoint>& path,
          autoware_msgs::LaneArray& laneArray);
  
      static void FindIncommingBranches(const std::vector<std::vector<PlannerHNS::WayPoint>>& globalPaths,
          const PlannerHNS::WayPoint& currPose, const double& min_distance,
          std::vector<PlannerHNS::WayPoint*>& branches, PlannerHNS::WayPoint* currOptions);
  };
  
  }  // namespace WayPlannerNS
  
  #endif  // ROSHELPERS_H_
  