opendrive_planner.h 6.47 KB
Newer Older
Sergi Hernandez's avatar
Sergi Hernandez committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
#ifndef _OPENDRIVE_PLANNER_H
#define _OPENDRIVE_PLANNER_H
/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, 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 Willow Garage, Inc. 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 THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS 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.
 *
 *********************************************************************/
#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <nav_msgs/Path.h>
#include <vector>
#include <nav_core/base_global_planner.h>
#include <nav_msgs/GetPlan.h>
#include <dynamic_reconfigure/server.h>
#include <iri_opendrive_global_planner/OpendriveGlobalPlannerConfig.h>
48
49
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
50
51
#include <ros/single_subscriber_publisher.h>
#include <iri_adc_msgs/get_opendrive_map.h>
52
#include <iri_adc_msgs/get_opendrive_nodes.h>
Sergi Hernandez's avatar
Sergi Hernandez committed
53

Fernando Herrero's avatar
Fernando Herrero committed
54
#include "opendrive_road_map.h"
Sergi Hernandez's avatar
Sergi Hernandez committed
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121

namespace iri_opendrive_global_planner {

/**
 * @class PlannerCore
 * @brief Provides a ROS wrapper for the global_planner planner which runs a fast, interpolated navigation function on a costmap.
 */

class OpendriveGlobalPlanner : public nav_core::BaseGlobalPlanner 
{
  public:
    /**
     * @brief  Default constructor for the PlannerCore object
     */
    OpendriveGlobalPlanner();
    /**
     * @brief  Constructor for the PlannerCore object
     * @param  name The name of this planner
     * @param  costmap A pointer to the costmap to use
     * @param  frame_id Frame of the costmap
     */
    OpendriveGlobalPlanner(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id);
    /**
     * @brief  Default deconstructor for the PlannerCore object
     */
    ~OpendriveGlobalPlanner();
    /**
     * @brief  Initialization function for the PlannerCore object
     * @param  name The name of this planner
     * @param  costmap_ros A pointer to the ROS wrapper of the costmap to use for planning
     */
    void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

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

    /**
     * @brief Given a goal pose in the world, compute a plan
     * @param start The start pose
     * @param goal The goal pose
     * @param plan The plan... filled by the planner
     * @return True if a valid plan was found, false otherwise
     */
    bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,std::vector<geometry_msgs::PoseStamped>& plan);

    /**
     * @brief Given a goal pose in the world, compute a plan
     * @param start The start pose
     * @param goal The goal pose
     * @param tolerance The tolerance on the goal point for the planner
     * @param plan The plan... filled by the planner
     * @return True if a valid plan was found, false otherwise
     */
    bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance,std::vector<geometry_msgs::PoseStamped>& plan);

    /**
     * @brief  Publish a path for visualization purposes
     */
    void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);

    bool makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp);

  protected:

    /**
     * @brief Store a copy of the current costmap in \a costmap.  Called by makePlan.
     */
    costmap_2d::Costmap2D* costmap_;
122
    std::string frame_id_,opendrive_frame_id_;
Sergi Hernandez's avatar
Sergi Hernandez committed
123
    ros::Publisher plan_pub_;
124
125
126
127
    ros::Publisher opendrive_map_pub_;
    void map_connect_callback(const ros::SingleSubscriberPublisher &subs);
    ros::ServiceServer opendrive_map_service;
    bool get_opendrive_map(iri_adc_msgs::get_opendrive_map::Request  &req,iri_adc_msgs::get_opendrive_map::Response &res);
128
129
    ros::ServiceServer opendrive_nodes_service;
    bool get_opendrive_nodes(iri_adc_msgs::get_opendrive_nodes::Request  &req,iri_adc_msgs::get_opendrive_nodes::Response &res);
130
131
132
    void create_opendrive_map(std::string &filename,double resolution,double scale_factor,double min_road_length);
    nav_msgs::OccupancyGrid full_path_;

Sergi Hernandez's avatar
Sergi Hernandez committed
133
134
    bool initialized_;

Fernando Herrero's avatar
Fernando Herrero committed
135
    COpendriveRoadMap roadmap;        
136
137
    std::vector<unsigned int> best_path;
    ros::Time best_path_stamp;
138
139
140
    double angle_tol;
    double dist_tol;
    bool multi_hyp;
Sergi Hernandez's avatar
Sergi Hernandez committed
141
142
143
144
145
146
147
148
149
150
151
152
153

  private:
    void mapToWorld(double mx, double my, double& wx, double& wy);
    bool worldToMap(double wx, double wy, double& mx, double& my);
    void clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my);

    boost::mutex mutex_;
    ros::ServiceServer make_plan_srv_;

    dynamic_reconfigure::Server<iri_opendrive_global_planner::OpendriveGlobalPlannerConfig> *dsrv_;
    void reconfigureCB(iri_opendrive_global_planner::OpendriveGlobalPlannerConfig &new_config, uint32_t level);
    iri_opendrive_global_planner::OpendriveGlobalPlannerConfig config;

154
155
    tf2_ros::Buffer tf2_buffer;
    tf2_ros::TransformListener tf2_listener;
Sergi Hernandez's avatar
Sergi Hernandez committed
156
157
158
159
160
};

} //end namespace iri_opendrive_global_planner

#endif