ppplanner.h

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, SEAOS, Inc.
 *  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 <organization> 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.
 *
 * Author: t-ogata@seaos.co.jp
 *********************************************************************/
#ifndef PIPELINEPLANNER_H
#define PIPELINEPLANNER_H
#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <nav_msgs/Path.h>
#include <tf/transform_datatypes.h>
#include <vector>
#include <nav_core/base_global_planner.h>
#include <nav_msgs/GetPlan.h>
#include <dynamic_reconfigure/server.h>
//#include <dynamic_reconfigure/IntParameter.h>
//#include <dynamic_reconfigure/Reconfigure.h>
//#include <dynamic_reconfigure/Config.h>
//#include <dynamic_reconfigure/client.h>
//#include <global_planner/planner_core.h>
#include <pipeline_planner/PipelinePlannerConfig.h>
#include <pipeline_planner/RobotPosition.h>
#include <pipeline_planner/SegmentInfo.h>
#include <std_msgs/UInt32.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Bool.h>
#include <pipeline_planner/GetReadStatus.h>
#include <pipeline_planner/GetRobotStatus.h>
#include <pipeline_planner/GetCheckpoints.h>
#include <pipeline_planner/GetNumofCheckpoints.h>
#include <pipeline_planner/InquireSegments.h>
#include <pipeline_planner/ReceiveCheckpoints.h>
#include <pipeline_planner/SetARadius.h>
#include <pipeline_planner/SetARightshift.h>
#include <navfn/navfn_ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <boost/thread.hpp>

namespace pipeline_planner {

  class PipelinePlanner : public nav_core::BaseGlobalPlanner {
    public:
      PipelinePlanner();
      PipelinePlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);

      ~PipelinePlanner();

      void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
      bool initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);

      bool ReceiveCheckpoints(pipeline_planner::ReceiveCheckpoints::Request &req,pipeline_planner::ReceiveCheckpoints::Response &res);

      void setPipeRadius(float radius);
//      void setPipeRadius(float radius){
//       pipe_radius_float=radius;
//       pipe_radius_=(double)pipe_radius_float;
//      }

      void setLethal(unsigned char lethal_cost){
       lethal_cost_ = lethal_cost;
      }

      void setWeight(float centre_weight){
       centre_weight_= centre_weight;
      }

#if MODE==MODE_GPU
      void setNumThread(float num_threads){
       num_threads_= num_threads;
      }
#endif

      void setStraight(bool use_straight){
       use_straight_ = use_straight;
      }

      void setCharge(bool charge){
       charge_= charge;
      }

      void setTimeDisplay(bool time_display){
       time_display_ = time_display;
      }

      void setTorchArea(bool use_torch,float torch_area_x,float torch_area_y){
       use_torch_=use_torch;
       torch_area_x_=torch_area_x;
       torch_area_y_=torch_area_y;
      }


      bool makePlan(const geometry_msgs::PoseStamped& start,
                    const geometry_msgs::PoseStamped& goal,
                    std::vector<geometry_msgs::PoseStamped>& plan);

    private:

      void reconfigureCB(pipeline_planner::PipelinePlannerConfig &config,uint32_t level);

      bool getReadStatus(pipeline_planner::GetReadStatus::Request &req,pipeline_planner::GetReadStatus::Response &res){
       res.read_status=read_status_.data;
       return true;
      }

      bool getRobotStatus(pipeline_planner::GetRobotStatus::Request &req,pipeline_planner::GetRobotStatus::Response &res){
       res.robot_status=robot_status_.data;
       return true;
      }

      bool getCheckpoints(pipeline_planner::GetCheckpoints::Request &req,pipeline_planner::GetCheckpoints::Response &res){
       int i,size;
       size=check_points_.size();
       res.checkpoints.resize(size);
       for(i=0;i<size;i++){
        res.checkpoints[i]=check_points_.at(i);
       };
       if(openclose_==1){
        res.openclose=1;
       } else if(openclose_==2){
        res.openclose=2;
       } else {
        res.openclose=3;
       };
       return true;
      }

      bool getNumofCheckpoints(pipeline_planner::GetNumofCheckpoints::Request &req,pipeline_planner::GetNumofCheckpoints::Response &res){
       //res.num_checkpoints=read_status_.data;
       res.num_checkpoints=check_points_.size();
       return true;
      }

      bool InquireSegments(pipeline_planner::InquireSegments::Request &req,pipeline_planner::InquireSegments::Response &res);

      bool SetARadius(pipeline_planner::SetARadius::Request &req,pipeline_planner::SetARadius::Response &res);

      bool SetARightshift(pipeline_planner::SetARightshift::Request &req,pipeline_planner::SetARightshift::Response &res);

      //void SubscribeCheckpoints(const geometry_msgs::PoseArray::ConstPtr& checkpoints);

      void PublishCheckpointsResult(std_msgs::UInt32 result);

      bool InPipeSegment(const geometry_msgs::PoseStamped& pose,double radius,double start_edge_radius,double end_edge_radius,double tolerance,int index);

      bool isBehindInaSegment(int idx,
       const geometry_msgs::PoseStamped& start,
       const geometry_msgs::PoseStamped& goal);

      bool hasNoCross(const int endpoint,const std::vector<geometry_msgs::PoseStamped> check_points);

      int costCheck(int endpoint);

      bool DistanceFromCentre(int endpoint);

      bool InitSegInfo();

      bool VeilCostmap(const std::vector<int> segids,const int startindex,
             const int goalindex,const geometry_msgs::PoseStamped& start);

      bool ConnectPoints(const geometry_msgs::PoseStamped& start,const geometry_msgs::PoseStamped& goal,
                const std::vector<int> segids,std::vector<geometry_msgs::PoseStamped>& plan);

      bool DrawOpenSegment(const geometry_msgs::PoseStamped init_point,
                const geometry_msgs::PoseStamped end_point,
                std::vector<geometry_msgs::PoseStamped>& plan);

      std::vector<double> dist_from_pipe_centre_;

      dynamic_reconfigure::Server<pipeline_planner::PipelinePlannerConfig> *dsrv_;

      bool knowRobotPosition(const geometry_msgs::Pose pose,int& ID,
              double& distance);
      //struct used in the knowRobotPosition function
      struct asegment {
       int id; int part; double dist; bool in;
      };

      bool whereInaPipeSegment(const geometry_msgs::Pose pose,const int id,const double radius,const double sradius,const double eradius,int& part,double &dist,double tolerance);

      void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);

      void makePipeline();
      void makePipeline(bool legal);

      void calcPipeline(const int numcheckpoints,const double *stonex,const double *stoney,const bool isClose,const double *radii,const double *sradii,const double *eradii,const double origin_x,const double origin_y,const unsigned int costsizex,const unsigned int costsizey,const double resolution,int *data);

      void callbackgoal(const move_base_msgs::MoveBaseActionGoal::ConstPtr& goal);

      void callbackRobotPose(const geometry_msgs::Pose::ConstPtr& pose);

      void informRobotPose();

      void TimeDisplay(const bool time_display,const double duration);

      bool TakeLocalGoal(const geometry_msgs::PoseStamped& start,
                    const geometry_msgs::PoseStamped& goal,
                    geometry_msgs::PoseStamped& local_goal,int& err);

      int CrossTorchArea(const double tx,const double ty,
             const double sx,const double sy,
             const double ex,const double ey,
             double& bx,double& by);

      double TakeSegmentRadius(const double contained_radius);

      bool initialized_;
      std::string gained_frame_id_;
      costmap_2d::Costmap2D* costmap_;
      std::vector<geometry_msgs::PoseStamped> check_points_;
      nav_msgs::OccupancyGrid pipeline_;
      std::vector<double> segment_lengths_;
      std::vector<pipeline_planner::SegmentInfo> segment_infos_;
      //ros::Subscriber sub_checkpoints_;
      ros::Subscriber sub_robot_pose_;
      //global_planner::GlobalPlanner* global_planner_;
      navfn::NavfnROS* navfn_planner_;
      costmap_2d::Costmap2D *navfn_costmap_;
      std::string name_;
      // status of reading checkpoints
      // status is contained in read_status_.data
      // 1:not yet read, 2:reading now, 3:OK, 4:read but empty,
      // 5:route overlaps obstacle, 6:out of costmap, 7:openclose flag error,
      // 8:having a crossing, others:unknown error
      std_msgs::UInt32 read_status_;
      // status of a robot
      // 1:not yet initialised, 2:ready,
      // 3:the robot is on the way to a goal or reached the goal,
      // 4:start is out of pipeline, 5:all pipe segments include start,
      // 6:goal is out of pipeline, 7:all pipe segments include goal,
      // 8:goal is behind start in a open pipeline,
      // 9:invalid check points, 10:global planner fails to make a plan,
      // 11:goal is behind start in one pipe segment
      // 12:now calculating global path
      std_msgs::UInt32 robot_status_;
      ros::Publisher pub_result_;
      ros::Publisher pub_status_;
      ros::Publisher pub_plan_;
      ros::Publisher pub_robot_position_;
      ros::Publisher pub_initialised_;
      ros::Publisher pub_pipeline_;
      double pipe_radius_;
      //double pipe_radius_float;
      unsigned char lethal_cost_;
      double centre_weight_;
      //1:close, 2:open
      int openclose_;
      double tolerance_;
      ros::ServiceServer read_state_srv_;
      ros::ServiceServer robot_state_srv_;
      ros::ServiceServer get_checkpoints_srv_;
      ros::ServiceServer get_numof_checkpoints_srv_;
      ros::ServiceServer inquire_segments_srv_;
      ros::ServiceServer receive_checkpoints_srv_;
      ros::ServiceServer set_a_radius_srv_;
      ros::ServiceServer set_a_rightshift_srv_;
      boost::thread* thread_robot_position_;

      //taking straight line model
      bool use_straight_;
      bool charge_;
      //fixing pipe radius in each pipe segment or not
      bool fix_pipe_radius_;
      //whether displaying calculation time or not
      bool time_display_;
      bool willDisplay_;
      std_msgs::Float32 consumed_time_;
      //robot position to inform
      geometry_msgs::Pose robot_position_;
      boost::mutex mutex_robot_position_;
      //Subscription of actionlib
      ros::Publisher pub_time_;
      ros::Subscriber sub_movebase_goal_;
      //torch model
      bool use_torch_;
      double torch_area_x_,torch_area_y_;

      bool debug;

      //CUDA
#if MODE==MODE_GPU

      bool VeilCostmap_cuda(const std::vector<int> segids,const int startindex,
             const int goalindex,const geometry_msgs::PoseStamped& start);

      bool DistanceFromCentre_cuda(int endpoint);

      void ThreadNumberAdjusment(bool debug);

      int num_threads_;
#endif

  };
};
#endif