├── .DS_Store ├── example.png ├── src ├── .DS_Store ├── path_planner_node_main.cc ├── common.h ├── common.cc ├── kd_tree.h ├── CMakeLists.txt ├── path_planner_node.h ├── kd_tree.cc └── path_planner_node.cc ├── srv ├── RoadmapQuery.srv ├── PathPlan.srv ├── ReconnectSubmaps.srv ├── ConnectionQuery.srv └── CMakeLists.txt ├── launch ├── .DS_Store └── path_planner.launch └── README.md /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xumzsy/cartographer_ros_path_planner/HEAD/.DS_Store -------------------------------------------------------------------------------- /example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xumzsy/cartographer_ros_path_planner/HEAD/example.png -------------------------------------------------------------------------------- /src/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xumzsy/cartographer_ros_path_planner/HEAD/src/.DS_Store -------------------------------------------------------------------------------- /srv/RoadmapQuery.srv: -------------------------------------------------------------------------------- 1 | int32 trajectory_id 2 | int32 submap_index 3 | --- 4 | int32[] connections 5 | -------------------------------------------------------------------------------- /launch/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xumzsy/cartographer_ros_path_planner/HEAD/launch/.DS_Store -------------------------------------------------------------------------------- /srv/PathPlan.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point start_point 2 | geometry_msgs/Point end_point 3 | --- 4 | geometry_msgs/Point[] path -------------------------------------------------------------------------------- /srv/ReconnectSubmaps.srv: -------------------------------------------------------------------------------- 1 | int32 start_trajectory_id 2 | int32 start_submap_index 3 | int32 end_trajectory_id 4 | int32 end_submap_index 5 | --- 6 | -------------------------------------------------------------------------------- /srv/ConnectionQuery.srv: -------------------------------------------------------------------------------- 1 | int32 start_trajectory_id 2 | int32 start_submap_index 3 | int32 end_trajectory_id 4 | int32 end_submap_index 5 | --- 6 | geometry_msgs/Point[] path 7 | -------------------------------------------------------------------------------- /src/path_planner_node_main.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Author: Mengze Xu 3 | */ 4 | 5 | #include "ros/ros.h" 6 | #include "path_planner_node.h" 7 | 8 | int main(int argc, char** argv){ 9 | ::ros::init(argc, argv, "cartographer_navigation_node"); 10 | ::ros::start(); 11 | cartographer_ros::cartographer_ros_path_planner::PathPlannerNode node; 12 | ::ros::spin(); 13 | ::ros::shutdown(); 14 | return 0; 15 | } 16 | -------------------------------------------------------------------------------- /src/common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Author: Mengze Xu 3 | * Define some common basic functions 4 | */ 5 | 6 | 7 | #ifndef common_h 8 | #define common_h 9 | 10 | #include "geometry_msgs/Point.h" 11 | #include "geometry_msgs/Pose.h" 12 | 13 | namespace cartographer_ros { 14 | namespace cartographer_ros_path_planner{ 15 | 16 | // Return distance2 in XY plane 17 | double Distance2BetweenPoint(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2); 18 | 19 | double Distance2BetweenPose(const geometry_msgs::Pose& pose1, const geometry_msgs::Pose& pose2); 20 | 21 | } // namespace cartographer_ros_path_planner 22 | } // namespace cartographer_ros 23 | 24 | #endif /* common_h */ 25 | -------------------------------------------------------------------------------- /launch/path_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/common.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * 3 | * 4 | * 5 | */ 6 | 7 | 8 | #include "common.h" 9 | 10 | namespace cartographer_ros { 11 | namespace cartographer_ros_path_planner{ 12 | 13 | // Return distance2 in XY plane 14 | double Distance2BetweenPoint(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2){ 15 | return (point1.x-point2.x)*(point1.x-point2.x) + (point1.y-point2.y)*(point1.y-point2.y); 16 | } 17 | 18 | double Distance2BetweenPose(const geometry_msgs::Pose& pose1, const geometry_msgs::Pose& pose2){ 19 | return (pose1.position.x-pose2.position.x)*(pose1.position.x-pose2.position.x)+ 20 | (pose1.position.y-pose2.position.y)*(pose1.position.y-pose2.position.y) 21 | +(pose1.position.z-pose2.position.z)*(pose1.position.z-pose2.position.z); 22 | } 23 | 24 | } // namespace cartographer_ros_path_planner 25 | } // namespace cartographer_ros 26 | -------------------------------------------------------------------------------- /srv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | cmake_minimum_required(VERSION 2.8.3) 16 | 17 | project(cartographer_ros_msgs) 18 | 19 | set(PACKAGE_DEPENDENCIES 20 | geometry_msgs 21 | ) 22 | 23 | find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIES}) 24 | 25 | add_message_files( 26 | FILES 27 | StatusCode.msg 28 | StatusResponse.msg 29 | SubmapList.msg 30 | SubmapEntry.msg 31 | SubmapTexture.msg 32 | SensorTopics.msg 33 | TrajectoryOptions.msg 34 | ) 35 | 36 | add_service_files( 37 | FILES 38 | SubmapQuery.srv 39 | FinishTrajectory.srv 40 | StartTrajectory.srv 41 | WriteState.srv 42 | ConnectionQuery.srv 43 | PathPlan.srv 44 | ReconnectSubmaps.srv 45 | RoadmapQuery.srv 46 | ) 47 | 48 | generate_messages( 49 | DEPENDENCIES 50 | geometry_msgs 51 | ) 52 | 53 | catkin_package( 54 | CATKIN_DEPENDS 55 | ${PACKAGE_DEPENDENCIES} 56 | ) 57 | -------------------------------------------------------------------------------- /src/kd_tree.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Author: Mengze Xu 3 | * Implement 2d kd tree for quick near neighbor search in RRT and submaps. 4 | * 5 | * TODO: Add rebalance method and make the clss more unisersal 6 | */ 7 | 8 | #ifndef kd_tree_h 9 | #define kd_tree_h 10 | 11 | #include 12 | #include "geometry_msgs/Point.h" 13 | 14 | namespace cartographer_ros { 15 | namespace cartographer_ros_path_planner{ 16 | 17 | struct KdTreeNode{ 18 | geometry_msgs::Point point; 19 | KdTreeNode* parent_node; // Used for plan planning 20 | double distance; // Used for plan planning 21 | KdTreeNode* left_node; // Used for kd tree 22 | KdTreeNode* right_node; // used for kd tree 23 | int trajectory_id; // used for submaps 24 | int submap_index; // used for submaps 25 | 26 | KdTreeNode(); 27 | KdTreeNode(geometry_msgs::Point p); 28 | KdTreeNode(geometry_msgs::Point p, int trajectory_idx, int submap_idx); 29 | }; 30 | 31 | 32 | class KdTree{ 33 | public: 34 | // Return nearest node to target point 35 | KdTreeNode* NearestKdTreeNode(const geometry_msgs::Point& target) const; 36 | 37 | // Return near nodes around target within radius 38 | std::vector NearKdTreeNode(const geometry_msgs::Point& target, 39 | double radius) const; 40 | 41 | // Add a new point into RRT tree 42 | KdTreeNode* AddPointToKdTree(geometry_msgs::Point point); 43 | KdTreeNode* AddPointToKdTree(geometry_msgs::Point point, KdTreeNode* parent, int depth); 44 | KdTreeNode* AddPointToKdTree(geometry_msgs::Point point, int trajectory_idx, int submap_idx); 45 | 46 | // Constructor 47 | KdTree(); 48 | KdTree(geometry_msgs::Point start_point); 49 | ~KdTree(){DestroyRRT(root_);} 50 | 51 | // For test 52 | KdTreeNode* BruceNearestKdTreeNode(const geometry_msgs::Point& point); 53 | std::vector BruceNearKdTreeNode(const geometry_msgs::Point& target, double radius); 54 | 55 | private: 56 | 57 | // Help function to go through kd tree 58 | void SearchKdTreeNode(const geometry_msgs::Point& target, 59 | KdTreeNode* current_node, 60 | KdTreeNode*& current_nearest_node, 61 | double& current_cloest_distance2, 62 | int depth) const; 63 | void SearchKdTreeNode(const geometry_msgs::Point& target, 64 | KdTreeNode* current_node, 65 | std::vector& near_nodes, 66 | double radius, 67 | int depth) const; 68 | // Destroy RRT 69 | void DestroyRRT(KdTreeNode* root); 70 | 71 | KdTreeNode* const root_; 72 | }; 73 | 74 | } // namespace cartographer_ros_path_planner 75 | } // namespace cartographer_ros 76 | 77 | #endif /* kd_tree_h */ 78 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Cartographer ROS Path Planner 2 | This package provides online 2D path planning based on cartographer submaps and does not rely on the global occupied grid. Main idea is to build up a global road map for submaps and use RRT* as local planner. 3 | 4 | ![Example image](example.png) 5 | ## Getting Started 6 | ### Prerequisites 7 | The package runs on Ubuntu 16.04 and ROS Kinetic. 8 | ### Installing 9 | * Follow [cartographer_ros](https://github.com/googlecartographer/cartographer_ros) to download cartographer, cacartographer_ros and 2d demo. 10 | * Move files in "src" to cartographer_ros/cartographer_ros/cartographer_ros 11 | * Move files in "srv" to cartographer_ros/cartographer_ros_msgs/srv 12 | * Move files in "launch" to cartographer_ros/cartographer_ros/launch 13 | * Substitute cartographer_ros/cartographer_ros_msgs/CmakeLists.txt with the cmake file in srv 14 | * Remake cartographer_ros 15 | 16 | ### Running the tests 17 | * After launching 2d demo, open another terminal and launch the planner 18 | ``` 19 | roslaunch cartographer_ros cartographer_path_planner.launch 20 | ``` 21 | * Use the "clicked point" to play with it. The planner will plan a path between map origin to clicked point. 22 | * In a few case, the path may go through the wall. This is mainly due to incorrect submaps created by cartographer. 23 | 24 | ## Methods 25 | ### Submap Road Map 26 | Kd tree is used to store each submap's origin. For each submap, I try to connect it to nearby submaps in map, last submap and the next submap in time series. RRT* is used to do local planning and the result path is stored in a map with submap id as key. 27 | 28 | ### Global Path Plan 29 | I decompose the global path plan into three parts: locally plan paths to connect start point and end point to nearest submaps. Use BFS to find the shortest path between these two submaps in road map. Finally combine the three paths. 30 | 31 | ## Parameters 32 | ### Hard Coded Parameters (const) 33 | * kFinishVersion = 180. This is given by cartographer. 34 | * kOccupyThreshold = 64. 0 is definitely free and 100 is definitely occupied. 35 | * kOccupyGridResolution = 0.05 [m] Decided by cartographer configuration but hard coded here. 36 | * kCloseSubmapRadius = 5.0 [m] Radius to find nearby submaps to given point 37 | ### ROS parameters in launch file 38 | * max_rrt_node_num. Max num of node RRT will contain. 39 | * step_to_check_reach_endpoint. Decide how often try to connect end point to RRT 40 | * distance_threshold_for_adding. Distance threshold to decide whether try to connect two submaps 41 | * distance_threshold_for_updating. If the position of the submap changes over this threshold, it will recalculate its road map 42 | * rotation_threshold_for_updating. If the oretation (q.z) of the submap changes over this threshold, it will recalculate its road map 43 | * probability_of_choose_endpoint. Probability to choose end point instead of random point as new point in RRT. 44 | * rrt_grow_step and rrt_trim_radius. Parameters in RRT* algorithm. 45 | 46 | ## ROS API 47 | * "/roadmap_query": Receive the SubmapId and return corresponding submaps connections 48 | * "/connection_query": Receive two SubmapId and return the path in road map between them 49 | * "/reconnect_submaps": Receive two SubmapId, discard existing path and try to reconnect them 50 | * "/plan_path": Receive two geometry_msgs::Point and return a path connecting them 51 | ## C++ API 52 | path_planner_node.h is well documented and provides useful information 53 | 54 | ## Future Work 55 | ### Improvement 56 | * Add functions to smooth the path 57 | * Add more RRT* tricks to accelerate convergence of RRT* 58 | 59 | ### Navigation 60 | To work as navigation stack, continues real time localization and velocity controller are needed. Now cartographer supports pure localization as a new trajectory in map builder but has not provided ROS API. 61 | 62 | Velocity controller transforms trajectory of way points to smooth velocity command which can be directly sent to robot's motion controller. 63 | 64 | Since the submap are not perfect, dynamic navigation is desired. This however needs changes in cartographer and can not be implemented just in ROS. 65 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_binary(cartographer_assets_writer 16 | SRCS 17 | assets_writer_main.cc 18 | ros_map_writing_points_processor.h 19 | ros_map_writing_points_processor.cc 20 | ) 21 | 22 | install(TARGETS cartographer_assets_writer 23 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 24 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 25 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 26 | ) 27 | 28 | google_binary(cartographer_node 29 | SRCS 30 | node_main.cc 31 | ) 32 | 33 | install(TARGETS cartographer_node 34 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 37 | ) 38 | 39 | google_binary(cartographer_offline_node 40 | SRCS 41 | offline_node_main.cc 42 | ) 43 | 44 | install(TARGETS cartographer_offline_node 45 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 47 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 48 | ) 49 | 50 | google_binary(cartographer_start_trajectory 51 | SRCS 52 | start_trajectory_main.cc 53 | ) 54 | 55 | install(TARGETS cartographer_start_trajectory 56 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 58 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 59 | ) 60 | 61 | google_binary(cartographer_occupancy_grid_node 62 | SRCS 63 | occupancy_grid_node_main.cc 64 | ) 65 | 66 | install(TARGETS cartographer_occupancy_grid_node 67 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 68 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 69 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 70 | ) 71 | 72 | google_binary(cartographer_path_planner_node 73 | SRCS 74 | path_planner_node_main.cc 75 | ) 76 | 77 | install(TARGETS cartographer_path_planner_node 78 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 79 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 80 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 81 | ) 82 | 83 | 84 | google_binary(cartographer_rosbag_validate 85 | SRCS 86 | rosbag_validate_main.cc 87 | ) 88 | 89 | install(TARGETS cartographer_rosbag_validate 90 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 91 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 92 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 93 | ) 94 | 95 | google_binary(cartographer_pbstream_to_ros_map 96 | SRCS 97 | pbstream_to_ros_map_main.cc 98 | ) 99 | 100 | install(TARGETS cartographer_pbstream_to_ros_map 101 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 102 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 103 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 104 | ) 105 | 106 | google_binary(cartographer_pbstream_map_publisher 107 | SRCS 108 | pbstream_map_publisher_main.cc 109 | ) 110 | 111 | install(TARGETS cartographer_pbstream_map_publisher 112 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 113 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 114 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 115 | ) 116 | 117 | # TODO(cschuet): Add support for shared library case. 118 | if (${BUILD_GRPC}) 119 | google_binary(cartographer_grpc_node 120 | SRCS 121 | cartographer_grpc/node_grpc_main.cc 122 | ) 123 | 124 | install(TARGETS cartographer_grpc_node 125 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 126 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 127 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 128 | ) 129 | 130 | google_binary(cartographer_grpc_offline_node 131 | SRCS 132 | cartographer_grpc/offline_node_grpc_main.cc 133 | ) 134 | 135 | install(TARGETS cartographer_grpc_offline_node 136 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 137 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 138 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 139 | ) 140 | 141 | install(PROGRAMS 142 | ../scripts/cartographer_grpc_server.sh 143 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 144 | ) 145 | endif() 146 | -------------------------------------------------------------------------------- /src/path_planner_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | */ 3 | 4 | #ifndef PATH_PLANNER_NODE_H 5 | #define PATH_PLANNER_NODE_H 6 | 7 | #include 8 | #include 9 | 10 | #include "cartographer/common/mutex.h" 11 | #include "cartographer/common/port.h" 12 | #include "cartographer/mapping/id.h" 13 | #include "cartographer_ros_msgs/SubmapEntry.h" 14 | #include "cartographer_ros_msgs/SubmapList.h" 15 | #include "cartographer_ros_msgs/SubmapQuery.h" 16 | #include "cartographer_ros_msgs/RoadmapQuery.h" 17 | #include "cartographer_ros_msgs/ConnectionQuery.h" 18 | #include "cartographer_ros_msgs/PathPlan.h" 19 | #include "cartographer_ros_msgs/ReconnectSubmaps.h" 20 | 21 | #include "geometry_msgs/PointStamped.h" 22 | #include "nav_msgs/Path.h" 23 | #include "ros/ros.h" 24 | #include "ros/serialization.h" 25 | #include "kd_tree.h" 26 | 27 | namespace cartographer_ros{ 28 | namespace cartographer_ros_path_planner { 29 | 30 | using ::cartographer::mapping::SubmapId; 31 | using SubmapIndex = int; 32 | using Path = std::vector; 33 | 34 | // Parameters obtained from ROS param server 35 | struct Parameters{ 36 | int max_rrt_node_num; 37 | int step_to_check_reach_endpoint; 38 | double distance_threshold_for_adding; 39 | double distance2_threshold_for_updating; 40 | double rotation_threshold_for_updating; 41 | double probability_of_choose_endpoint; 42 | double rrt_grow_step; 43 | double rrt_trim_radius; 44 | double close_submap_radius; 45 | 46 | }; 47 | 48 | // Core Node provide path plan for cartographer 49 | class PathPlannerNode{ 50 | public: 51 | 52 | PathPlannerNode(); 53 | ~PathPlannerNode(){}; 54 | 55 | PathPlannerNode(const PathPlannerNode&) = delete; 56 | PathPlannerNode& operator=(const PathPlannerNode&) = delete; 57 | 58 | // Get Parameters from ROS param server 59 | void SetParameters(); 60 | 61 | 62 | // Given a point in global map frame and its correspond submap id, 63 | // return the intensity of the point in submap. 64 | // val: -1 for unobserved or out of submap range 65 | // 0 for definitely free and 100 for definitely occupied. 66 | int GetPointIntensity(const geometry_msgs::Point& point, 67 | const SubmapId& submap_id) const; 68 | 69 | 70 | // Return whether a point in global map frame is free or not 71 | bool IsFree(const geometry_msgs::Point& point) const; 72 | 73 | // Return whether the straight line between two points are free or not 74 | // in given submaps 75 | bool IsPathLocalFree(const geometry_msgs::Point& start_point, 76 | const geometry_msgs::Point& end_point, 77 | const std::vector& submap_ids) const; 78 | 79 | // Return the closest submap to given point 80 | SubmapId ClosestSubmap(const geometry_msgs::Point& point) const; 81 | 82 | // Return a list of submaps whose origin is within a circle of radius 83 | // to given point 84 | // TODO: use centre instead of origin 85 | std::vector CloseSubmaps(const geometry_msgs::Point& point, 86 | double radius) const; 87 | 88 | // Return a free path from start point to end point using RRT* 89 | Path PlanPathRRT(const geometry_msgs::Point& start, 90 | const geometry_msgs::Point& end) const; 91 | 92 | // Return a free path between two submaps' origin using RRT* 93 | Path PlanPathRRT(const SubmapId& start_id, const SubmapId& end_id) const; 94 | 95 | // Returan a path connecting two remote submaps using graph search 96 | Path ConnectSubmap(const SubmapId& start_id, const SubmapId& end_id) const; 97 | 98 | // Return a path between two points. It's' supposed that the submaps 99 | // that the points are located are directly connected 100 | Path LocalPlanPathRRT(const geometry_msgs::Point& start_point, 101 | const geometry_msgs::Point& end_point) const; 102 | 103 | // Return a path between two points in given submaps 104 | Path LocalPlanPathRRT(const geometry_msgs::Point& start_point, 105 | const geometry_msgs::Point& end_point, 106 | const std::vector& submap_ids) const; 107 | 108 | // Publish and display the path in RVIZ 109 | void AddDisplayPath(const Path& path); 110 | 111 | private: 112 | struct SubmapConnectState{ 113 | SubmapId start_submap_id; 114 | SubmapId end_submap_id; 115 | double length; 116 | Path path; 117 | 118 | SubmapConnectState(){}; 119 | SubmapConnectState(SubmapId start_id, SubmapId end_id, double d) : 120 | start_submap_id(start_id), 121 | end_submap_id(end_id), 122 | length(d) {}; 123 | }; 124 | 125 | struct SubmapGrid{ 126 | SubmapId submap_id; 127 | double resolution; 128 | double x0; 129 | double y0; 130 | int width; 131 | int height; 132 | std::vector data; 133 | }; 134 | 135 | // Generate random free point in given submaps used in RRT* 136 | geometry_msgs::Point RandomFreePoint(const std::vector& submap_ids) const; 137 | 138 | // Add submap grid into submap_grid_ 139 | void AddSubmapGrid(const SubmapId& submap_id); 140 | 141 | // Discard existing path if existed and reconnect two submaps 142 | // Return false if fail to connect two submaps 143 | bool ReconnectSubmaps(const SubmapId& start_id, const SubmapId& end_id); 144 | 145 | // Add a submap into road_map_ 146 | void AddRoadMapEntry(const SubmapId& submap_id); 147 | 148 | // Update submap_, road_map_ and submap_grid_ every time receive submapList 149 | void UpdateRoadMap(const cartographer_ros_msgs::SubmapList::ConstPtr& msg); 150 | 151 | // ROS Agent 152 | ::cartographer::common::Mutex mutex_; 153 | ::ros::NodeHandle node_handle_ GUARDED_BY(mutex_); 154 | ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_); 155 | ::ros::ServiceClient submap_query_client_ GUARDED_BY(mutex_); 156 | Parameters parameters_; 157 | 158 | // SubmapList 159 | std::map submap_ GUARDED_BY(mutex_); 160 | 161 | // Kd tree for Submap position 162 | KdTree submap_kdtree_; 163 | 164 | // SubmapGrid 165 | std::map submap_grid_ GUARDED_BY(mutex_); 166 | 167 | // Road map to store the connectivity of submaps and corresponding path 168 | std::map> road_map_ GUARDED_BY(mutex_); 169 | 170 | // ROS Server 171 | ::ros::ServiceServer roadmap_query_server_; 172 | ::ros::ServiceServer connection_query_server_; 173 | ::ros::ServiceServer plan_path_server_; 174 | ::ros::ServiceServer reconnect_submaps_server_; 175 | bool QueryRoadmap(cartographer_ros_msgs::RoadmapQuery::Request &req, 176 | cartographer_ros_msgs::RoadmapQuery::Response &res); 177 | bool QueryConnection(cartographer_ros_msgs::ConnectionQuery::Request &req, 178 | cartographer_ros_msgs::ConnectionQuery::Response &res); 179 | bool PlanPath(cartographer_ros_msgs::PathPlan::Request &req, 180 | cartographer_ros_msgs::PathPlan::Response &res); 181 | bool ReconnectSubmapService(cartographer_ros_msgs::ReconnectSubmaps::Request &req, 182 | cartographer_ros_msgs::ReconnectSubmaps::Response &res); 183 | 184 | // Members to publish and display newest path 185 | nav_msgs::Path path_to_display_; 186 | ::ros::WallTimer path_publisher_timer_; 187 | ::ros::Publisher path_publisher_; 188 | void PublishPath(const ::ros::WallTimerEvent& timer_event); 189 | 190 | // print out the current state for testing and debugging 191 | 192 | ::ros::Subscriber clicked_point_subscriber_ GUARDED_BY(mutex_); 193 | 194 | void IsClickedPointFree(const geometry_msgs::PointStamped::ConstPtr& msg) const; 195 | void NavigateToClickedPoint(const geometry_msgs::PointStamped::ConstPtr& msg); 196 | }; 197 | 198 | } // namespace cartographer_ros_path_planner 199 | } // namespace cartographer_ros 200 | #endif /* path_planner_node.h */ 201 | -------------------------------------------------------------------------------- /src/kd_tree.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Author: Mengze Xu 3 | */ 4 | 5 | #include 6 | 7 | #include "common.h" 8 | #include "kd_tree.h" 9 | 10 | namespace cartographer_ros { 11 | namespace cartographer_ros_path_planner{ 12 | 13 | // Constructor for KdTreeNode 14 | KdTreeNode::KdTreeNode() : parent_node(nullptr), distance (0.0), left_node(nullptr), right_node(nullptr), 15 | trajectory_id(0), submap_index(0){ 16 | point.x = 0.0; 17 | point.y = 0.0; 18 | point.z = 0.0; 19 | } 20 | 21 | KdTreeNode::KdTreeNode(geometry_msgs::Point p) : point(p), parent_node(nullptr), distance (0.0), 22 | left_node(nullptr), right_node(nullptr), trajectory_id(0), submap_index(0){} 23 | 24 | KdTreeNode::KdTreeNode(geometry_msgs::Point p, int trajectory_idx, int submap_idx) : point(p), 25 | parent_node(nullptr), distance (0.0), left_node(nullptr), right_node(nullptr), 26 | trajectory_id(trajectory_idx), submap_index(submap_idx){} 27 | 28 | 29 | // Return nearest node to target point 30 | KdTreeNode* KdTree::NearestKdTreeNode(const geometry_msgs::Point& target) const { 31 | KdTreeNode* nearest_node = nullptr; 32 | double closest_distance2 = DBL_MAX; 33 | SearchKdTreeNode(target, root_, nearest_node, closest_distance2,0); 34 | return nearest_node; 35 | } 36 | 37 | // Return near nodes around target within radius 38 | std::vector KdTree::NearKdTreeNode(const geometry_msgs::Point& target, 39 | double radius) const { 40 | std::vector near_nodes; 41 | SearchKdTreeNode(target,root_,near_nodes,radius * radius,0); 42 | return near_nodes; 43 | } 44 | 45 | // Helper function to go through KdTree 46 | void KdTree::SearchKdTreeNode(const geometry_msgs::Point& target, 47 | KdTreeNode* current_node, 48 | KdTreeNode*& current_nearest_node, 49 | double& current_cloest_distance2, 50 | int depth) const{ 51 | if(current_node==nullptr) return; 52 | double distance2 = Distance2BetweenPoint(target, current_node->point); 53 | bool go_to_left = depth%2==0 ? target.x <= current_node->point.x // Compare x 54 | : target.y <= current_node->point.y; // Compare y 55 | // Recursively visit children 56 | go_to_left ? 57 | SearchKdTreeNode(target, current_node->left_node, 58 | current_nearest_node,current_cloest_distance2, depth+1) : 59 | SearchKdTreeNode(target, current_node->right_node, 60 | current_nearest_node,current_cloest_distance2, depth+1); 61 | 62 | // Search current_node 63 | if(distance2point.x) * (target.x - current_node->point.x) 71 | : (target.y - current_node->point.y) * (target.y - current_node->point.y); 72 | if(current_cloest_distance2 > discard_threshold){ 73 | go_to_left ? SearchKdTreeNode(target,current_node->right_node, 74 | current_nearest_node,current_cloest_distance2,depth+1) 75 | : SearchKdTreeNode(target,current_node->left_node, 76 | current_nearest_node,current_cloest_distance2,depth+1); 77 | } 78 | } 79 | 80 | // Helper function to go through KdTree 81 | void KdTree::SearchKdTreeNode(const geometry_msgs::Point& target, 82 | KdTreeNode* current_node, 83 | std::vector& near_nodes, 84 | double radius, 85 | int depth) const{ 86 | if(current_node==nullptr) return; 87 | double distance2 = Distance2BetweenPoint(target, current_node->point); 88 | // Visit current node 89 | if(distance2 < radius) near_nodes.push_back(current_node); 90 | 91 | // Recursively visit children 92 | bool go_to_left = depth%2==0 ? target.x <= current_node->point.x // Compare x 93 | : target.y <= current_node->point.y; // Compare y 94 | if(go_to_left){ 95 | SearchKdTreeNode(target, current_node->left_node, near_nodes,radius, depth+1); 96 | } else{ 97 | SearchKdTreeNode(target, current_node->right_node, near_nodes,radius, depth+1); 98 | } 99 | 100 | // Visit the otherside 101 | double discard_threshold = depth%2==0 102 | ? (target.x - current_node->point.x) * (target.x - current_node->point.x) 103 | : (target.y - current_node->point.y) * (target.y - current_node->point.y); 104 | if(radius > discard_threshold){ 105 | go_to_left ? SearchKdTreeNode(target, current_node->right_node, near_nodes, radius, depth+1) 106 | : SearchKdTreeNode(target, current_node->left_node, near_nodes,radius, depth+1); 107 | } 108 | } 109 | 110 | // Add a new point into RRT tree and return a pointer to it 111 | KdTreeNode* KdTree::AddPointToKdTree(geometry_msgs::Point point){ 112 | return AddPointToKdTree(point, root_, 0); 113 | } 114 | 115 | KdTreeNode* KdTree::AddPointToKdTree(geometry_msgs::Point point, int trajectory_idx, int submap_idx){ 116 | auto added_node = AddPointToKdTree(point, root_, 0); 117 | added_node->trajectory_id = trajectory_idx; 118 | added_node->submap_index = submap_idx; 119 | return added_node; 120 | } 121 | 122 | // Recursively add the point into kd tree 123 | KdTreeNode* KdTree::AddPointToKdTree(geometry_msgs::Point point, KdTreeNode* parent, int depth){ 124 | bool go_to_left = depth%2==0 ? point.x <= parent->point.x // Compare x 125 | : point.y <= parent->point.y; // Compare y 126 | if(go_to_left){ 127 | if(parent->left_node==nullptr){ 128 | parent->left_node = new KdTreeNode(point); 129 | return parent->left_node; 130 | } else{ 131 | return AddPointToKdTree(point, parent->left_node, depth+1); 132 | } 133 | } else{ 134 | if(parent->right_node==nullptr){ 135 | parent->right_node = new KdTreeNode(point); 136 | return parent->right_node; 137 | } else{ 138 | return AddPointToKdTree(point, parent->right_node, depth+1); 139 | } 140 | } 141 | } 142 | 143 | // Constructor 144 | KdTree::KdTree() : root_(new KdTreeNode ()){} 145 | KdTree::KdTree(geometry_msgs::Point start_point) : root_(new KdTreeNode (start_point)){} 146 | 147 | // Destroy RRT 148 | void KdTree::DestroyRRT(KdTreeNode* root){ 149 | if(root!=nullptr){ 150 | DestroyRRT(root->left_node); 151 | DestroyRRT(root->right_node); 152 | delete(root); 153 | } 154 | } 155 | 156 | // For test 157 | KdTreeNode* KdTree::BruceNearestKdTreeNode(const geometry_msgs::Point& target){ 158 | KdTreeNode* nearest_node = nullptr; 159 | double min_distance2 = DBL_MAX; 160 | std::queue q; 161 | q.push(root_); 162 | while(!q.empty()){ 163 | double distance2 = Distance2BetweenPoint(target, q.front()->point); 164 | if(distance2left_node) q.push(q.front()->left_node); 169 | if(q.front()->right_node) q.push(q.front()->right_node); 170 | q.pop(); 171 | } 172 | return nearest_node; 173 | } 174 | 175 | 176 | std::vector KdTree::BruceNearKdTreeNode(const geometry_msgs::Point& target, 177 | double radius){ 178 | std::vector bruce_near_nodes; 179 | std::queue q; 180 | q.push(root_); 181 | while(!q.empty()){ 182 | double distance2 = Distance2BetweenPoint(target, q.front()->point); 183 | if(distance2 < radius){ 184 | bruce_near_nodes.push_back(q.front()); 185 | } 186 | if(q.front()->left_node) q.push(q.front()->left_node); 187 | if(q.front()->right_node) q.push(q.front()->right_node); 188 | q.pop(); 189 | } 190 | return bruce_near_nodes; 191 | } 192 | 193 | } // namespace cartographer_ros_path_planner 194 | } // namespace cartographer_ros 195 | 196 | -------------------------------------------------------------------------------- /src/path_planner_node.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Author: Mengze Xu 3 | */ 4 | 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "Eigen/Core" 15 | #include "Eigen/Geometry" 16 | #include "cairo/cairo.h" 17 | #include "cartographer/io/image.h" 18 | #include "cartographer/io/submap_painter.h" 19 | #include "cartographer/transform/rigid_transform.h" 20 | #include "cartographer_ros/msg_conversion.h" 21 | #include "cartographer_ros/node_constants.h" 22 | #include "cartographer_ros/submap.h" 23 | 24 | #include "common.h" 25 | #include "path_planner_node.h" 26 | 27 | 28 | namespace cartographer_ros{ 29 | namespace cartographer_ros_path_planner { 30 | namespace{ 31 | 32 | using ::cartographer::io::PaintSubmapSlicesResult; 33 | using ::cartographer::io::SubmapSlice; 34 | 35 | const char kSubmapListTopicName [] = "/submap_list"; 36 | const char kSubmapQueryServiceName [] = "/submap_query"; 37 | const char kRoadmapQueryServiceName [] = "/roadmap_query"; 38 | const char kConnectionQueryServiceName [] = "/connection_query"; 39 | const char kReconnectSubmapsServiceName [] = "/reconnect_submaps"; 40 | const char kPathPlanServiceName [] = "/plan_path"; 41 | const int kFinishVersion = 180; 42 | const int kOccupyThreshold = 64; 43 | const double kOccupyGridResolution = 0.05; 44 | const double kProbabilityGridResolution = 0.05; 45 | 46 | // Define add and scalar multiplication for Point 47 | geometry_msgs::Point operator+(const geometry_msgs::Point& a, const geometry_msgs::Point& b){ 48 | geometry_msgs::Point sum; 49 | sum.x = a.x + b.x; 50 | sum.y = a.y + b.y; 51 | sum.z = a.z + b.z; 52 | return sum; 53 | } 54 | 55 | geometry_msgs::Point operator*(double a, const geometry_msgs::Point& b){ 56 | geometry_msgs::Point product; 57 | product.x = a * b.x; 58 | product.y = a * b.y; 59 | product.z = a * b.z; 60 | return product; 61 | } 62 | 63 | std::ostream& operator<<(std::ostream& os, const geometry_msgs::Point& point){ 64 | os<<" ("<& visited_submap_distance, const std::map& submap_, SubmapId end_id): 98 | submap_distance(visited_submap_distance), submap_list(submap_){goal = submap_list.find(end_id)->second.pose.position;} 99 | bool operator ()(const SubmapId& a, const SubmapId& b) { 100 | double h_a = sqrt(Distance2BetweenPoint(submap_list.find(a)->second.pose.position, goal)); 101 | double h_b = sqrt(Distance2BetweenPoint(submap_list.find(b)->second.pose.position, goal)); 102 | return submap_distance[a] + h_a > submap_distance[b] + h_b; 103 | } 104 | private: 105 | std::map& submap_distance; 106 | const std::map& submap_list; 107 | geometry_msgs::Point goal; 108 | }; 109 | } // namespace 110 | 111 | 112 | PathPlannerNode::PathPlannerNode(){ 113 | cartographer::common::MutexLocker lock(&mutex_); 114 | submap_list_subscriber_ = node_handle_.subscribe(kSubmapListTopicName,10, &PathPlannerNode::UpdateRoadMap,this); 115 | submap_query_client_ = node_handle_.serviceClient(kSubmapQueryServiceName); 116 | roadmap_query_server_ = node_handle_.advertiseService(kRoadmapQueryServiceName,&PathPlannerNode::QueryRoadmap,this); 117 | connection_query_server_ = node_handle_.advertiseService(kConnectionQueryServiceName, &PathPlannerNode::QueryConnection,this); 118 | plan_path_server_ = node_handle_.advertiseService(kPathPlanServiceName,&PathPlannerNode::PlanPath,this); 119 | reconnect_submaps_server_ = node_handle_.advertiseService(kReconnectSubmapsServiceName, &PathPlannerNode::ReconnectSubmapService,this); 120 | path_publisher_ = node_handle_.advertise<::nav_msgs::Path>("/test_path", 10); 121 | path_publisher_timer_ = node_handle_.createWallTimer(::ros::WallDuration(0.1), &PathPlannerNode::PublishPath, this); 122 | SetParameters(); 123 | srand (time(NULL)); 124 | 125 | // For test 126 | clicked_point_subscriber_ = node_handle_.subscribe("/clicked_point",1,&PathPlannerNode::NavigateToClickedPoint, this); 127 | std::cout<<"Successfully Create PathPlannerNode"<second; 169 | int x = (point.x - submap_grid.x0) / submap_grid.resolution; 170 | int y = (point.y - submap_grid.y0) / submap_grid.resolution; 171 | if(x>=0 && x=0 && y=kOccupyThreshold) return false; 190 | if(val>=0) is_free = true; 191 | } 192 | return is_free; 193 | } 194 | 195 | 196 | bool PathPlannerNode::IsPathLocalFree(const geometry_msgs::Point& start_point, 197 | const geometry_msgs::Point& end_point, 198 | const std::vector& submap_ids) const { 199 | double distance2 = Distance2BetweenPoint(start_point,end_point); 200 | double step = kOccupyGridResolution / sqrt(distance2); 201 | for(double i=0;i<=1;i+=step){ 202 | geometry_msgs::Point point = (1.0-i) * start_point + i * end_point; 203 | bool is_free = false; 204 | for(const auto& submap_id:submap_ids){ 205 | int val = GetPointIntensity(point, submap_id); 206 | if(val>=kOccupyThreshold) return false; 207 | if(val>=0) is_free = true; 208 | } 209 | if(!is_free) return false; 210 | } 211 | return true; 212 | } 213 | 214 | 215 | SubmapId PathPlannerNode::ClosestSubmap(const geometry_msgs::Point& point) const{ 216 | const auto nearest_node = submap_kdtree_.NearestKdTreeNode(point); 217 | return SubmapId {nearest_node->trajectory_id,nearest_node->submap_index}; 218 | } 219 | 220 | 221 | std::vector PathPlannerNode::CloseSubmaps(const geometry_msgs::Point& point, 222 | double radius) const{ 223 | const auto& near_nodes = submap_kdtree_.NearKdTreeNode(point, radius); 224 | std::vector close_submaps; 225 | close_submaps.reserve(near_nodes.size()); 226 | for(const auto near_node:near_nodes){ 227 | SubmapId submap_id {near_node->trajectory_id,near_node->submap_index}; 228 | close_submaps.push_back(std::move(submap_id)); 229 | } 230 | return close_submaps; 231 | } 232 | 233 | 234 | Path PathPlannerNode::PlanPathRRT(const geometry_msgs::Point& start_point, 235 | const geometry_msgs::Point& end_point) const { 236 | ::ros::WallTime begin_time = ::ros::WallTime::now(); 237 | // Check start and end point is free 238 | if(!IsFree(start_point)){ 239 | std::cout<<"Start point is occupied!"<second.pose.position); 262 | if(!start_path.empty()){ 263 | path.insert(path.end(),start_path.begin(),start_path.end()); 264 | } else{ 265 | std::cout<<"Fail to connect start point to start_submap"<second.pose.position,end_point); 280 | if(!end_path.empty()){ 281 | path.insert(path.end(),end_path.begin(),end_path.end()); 282 | } else{ 283 | std::cout<<" Fail to connect end point to end_submap"<second.pose.position; 307 | geometry_msgs::Point end_point = end_submap->second.pose.position; 308 | Path path = LocalPlanPathRRT(start_point, end_point); 309 | if(!path.empty()){ 310 | std::cout<<"Successfully connect submap "< visited_submap_distance; 323 | std::map previous_submap; 324 | std::priority_queue, 325 | CustomPriorityCompare> submap_to_visit ({visited_submap_distance, submap_, end_id}); 326 | 327 | for(const auto& pair:submap_) visited_submap_distance[pair.first] = DBL_MAX; 328 | visited_submap_distance[start_id] = 0.0; 329 | submap_to_visit.push(start_id); 330 | bool find_end_id = false; 331 | 332 | while(!submap_to_visit.empty()&&!find_end_id){ 333 | SubmapId current_submap = submap_to_visit.top(); 334 | submap_to_visit.pop(); 335 | auto& current_connections = road_map_.find(current_submap)->second; 336 | for(const auto& entry:current_connections){ 337 | if(entry.second.length + visited_submap_distance[current_submap] < 338 | visited_submap_distance[entry.first]){ 339 | visited_submap_distance[entry.first] = entry.second.length + visited_submap_distance[current_submap]; 340 | previous_submap[entry.first] = current_submap; 341 | submap_to_visit.push(entry.first); 342 | if(entry.first==end_id) {find_end_id = true;break;} 343 | } 344 | } 345 | 346 | } 347 | 348 | if(previous_submap.count(end_id)==0){ 349 | return {}; 350 | } 351 | Path path; 352 | SubmapId id = end_id; 353 | while(previous_submap.count(id)==1){ 354 | SubmapId previous_id = previous_submap[id]; 355 | if(previous_submap.count(previous_id)==1){ 356 | auto connection = road_map_.find(previous_id)->second.find(id)->second; 357 | path.insert(path.begin(),connection.path.begin(),connection.path.end()); 358 | } 359 | id = previous_submap[id]; 360 | } 361 | return path; 362 | } 363 | 364 | 365 | // Use RRT* to do local planning 366 | Path PathPlannerNode::LocalPlanPathRRT(const geometry_msgs::Point& start_point, 367 | const geometry_msgs::Point& end_point) const{ 368 | SubmapId start_submap_id = ClosestSubmap(start_point); 369 | SubmapId end_submap_id = ClosestSubmap(end_point); 370 | 371 | // Find local submaps used in later planning 372 | auto start_submap_ids = CloseSubmaps(start_point, parameters_.close_submap_radius); 373 | auto end_submap_ids = CloseSubmaps(end_point, parameters_.close_submap_radius); 374 | std::set union_submap_ids; 375 | for(SubmapId& submap_id:start_submap_ids){ 376 | union_submap_ids.insert(submap_id); 377 | } 378 | for(SubmapId& submap_id:end_submap_ids){ 379 | union_submap_ids.insert(submap_id); 380 | } 381 | union_submap_ids.insert(start_submap_id); 382 | union_submap_ids.insert(end_submap_id); 383 | std::vector submap_ids (union_submap_ids.begin(),union_submap_ids.end()); 384 | 385 | // Call local planner 386 | return LocalPlanPathRRT(start_point,end_point,submap_ids); 387 | } 388 | 389 | 390 | Path PathPlannerNode::LocalPlanPathRRT(const geometry_msgs::Point& start_point, 391 | const geometry_msgs::Point& end_point, 392 | const std::vector& submap_ids) const { 393 | // Naively check the straight line between two points 394 | if(IsPathLocalFree(start_point,end_point,submap_ids)) return {start_point,end_point}; 395 | 396 | // Build RRT using kd tree 397 | KdTree kd_tree (start_point); 398 | for(int node_num=1;node_num<=parameters_.max_rrt_node_num;node_num++){ 399 | // Generate random point in given submaps 400 | // Better sample method can accerate the convergence 401 | geometry_msgs::Point next_point; 402 | if((rand() % 1000) / 1000.0 < parameters_.probability_of_choose_endpoint){ 403 | next_point = (end_point); 404 | } else{ 405 | next_point = RandomFreePoint(submap_ids); 406 | } 407 | 408 | // Search the nearest tree node 409 | KdTreeNode* nearest_node = kd_tree.NearestKdTreeNode(next_point); 410 | 411 | // Or go ahead a given distance 412 | next_point = (parameters_.rrt_grow_step)*next_point + (1-parameters_.rrt_grow_step)*nearest_node->point; 413 | 414 | if(!IsPathLocalFree(nearest_node->point, next_point, submap_ids)){node_num--;continue;} 415 | // Add next_node into RRT 416 | KdTreeNode* next_node = kd_tree.AddPointToKdTree(next_point); 417 | if(next_node==nullptr) std::cout<<"Fail to Add New Node"<distance = nearest_node->distance + sqrt(Distance2BetweenPoint(next_point, nearest_node->point)); 419 | next_node->parent_node = nearest_node; 420 | 421 | // Trim RRT 422 | auto near_nodes = kd_tree.NearKdTreeNode(next_point, parameters_.rrt_trim_radius); 423 | for(auto& near_node : near_nodes){ 424 | double edge_length = sqrt(Distance2BetweenPoint(next_point, near_node->point)); 425 | if(edge_length + next_node->distance < near_node->distance && 426 | IsPathLocalFree(near_node->point,next_point,submap_ids)){ 427 | near_node->parent_node = next_node; 428 | near_node->distance = edge_length + next_node->distance; 429 | } 430 | } 431 | 432 | // Try to connect RRT and end point 433 | if(node_num % parameters_.step_to_check_reach_endpoint == 0){ 434 | //std::cout<<"Try to connect End point! Node Num:"<point,end_point,submap_ids)){ 437 | // find the path! 438 | Path path; 439 | while(path_node!=nullptr){ 440 | path.insert(path.begin(),path_node->point); 441 | path_node = path_node->parent_node; 442 | } 443 | path.push_back(end_point); 444 | return path; 445 | } 446 | } 447 | } 448 | return {}; 449 | } 450 | 451 | 452 | void PathPlannerNode::AddDisplayPath(const Path& path){ 453 | path_to_display_.header.stamp = ::ros::Time::now(); 454 | path_to_display_.header.frame_id = "/map"; 455 | path_to_display_.poses.clear(); 456 | for(auto& point : path){ 457 | geometry_msgs::PoseStamped pose_stamped; 458 | pose_stamped.header.stamp = ::ros::Time::now(); 459 | pose_stamped.header.frame_id = "/map"; 460 | pose_stamped.pose.position = std::move(point); 461 | pose_stamped.pose.orientation.w = 1.0; 462 | pose_stamped.pose.orientation.x = 0.0; 463 | pose_stamped.pose.orientation.y = 0.0; 464 | pose_stamped.pose.orientation.z = 0.0; 465 | path_to_display_.poses.push_back(std::move(pose_stamped)); 466 | } 467 | } 468 | 469 | 470 | // Return a random free point in given submaps 471 | geometry_msgs::Point PathPlannerNode::RandomFreePoint(const std::vector& submap_ids) const { 472 | while(true){ 473 | int random_id = rand() % submap_ids.size(); 474 | auto& submap_grid = submap_grid_.find(submap_ids[random_id])->second; 475 | int random_x = rand() % (submap_grid.width); 476 | int random_y = rand() % (submap_grid.height); 477 | int val = submap_grid.data[random_y * submap_grid.width + random_x]; 478 | if(val>=0 && val=kOccupyThreshold){ 488 | is_free = false; 489 | break; 490 | } 491 | } 492 | if(is_free) return point; 493 | } 494 | } 495 | } 496 | 497 | 498 | void PathPlannerNode::AddSubmapGrid(const SubmapId& submap_id){ 499 | // Clear the existing old data 500 | submap_grid_.erase(submap_id); 501 | 502 | // first fetch the submaptexture 503 | cartographer_ros_msgs::SubmapEntry& submap_entry = submap_[submap_id]; 504 | auto fetched_textures = ::cartographer_ros::FetchSubmapTextures(submap_id, &submap_query_client_); 505 | const auto submap_texture = fetched_textures->textures.begin(); 506 | 507 | // use fake map to do it (map only contain one element) 508 | std::map fake_submap_slices; 509 | ::cartographer::io::SubmapSlice& submap_slice = fake_submap_slices[submap_id]; 510 | 511 | submap_slice.pose = ToRigid3d(submap_entry.pose); 512 | submap_slice.metadata_version = submap_entry.submap_version; 513 | 514 | // push fetched texture to slice and draw the texture 515 | submap_slice.version = fetched_textures->version; 516 | submap_slice.width = submap_texture->width; 517 | submap_slice.height = submap_texture->height; 518 | submap_slice.slice_pose = submap_texture->slice_pose; 519 | submap_slice.resolution = submap_texture->resolution; 520 | submap_slice.cairo_data.clear(); 521 | submap_slice.surface = ::cartographer::io::DrawTexture(submap_texture->pixels.intensity, 522 | submap_texture->pixels.alpha, 523 | submap_texture->width, submap_texture->height, 524 | &submap_slice.cairo_data); 525 | 526 | 527 | // Paint the texture 528 | auto painted_slices = cartographer::io::PaintSubmapSlices(fake_submap_slices, kOccupyGridResolution); 529 | 530 | // Convert painted surface into occupied grid 531 | auto& submap_grid = submap_grid_[submap_id]; 532 | const Eigen::Array2f& origin = painted_slices.origin; 533 | cairo_surface_t* surface = painted_slices.surface.get(); 534 | const int width = cairo_image_surface_get_width(surface); 535 | const int height = cairo_image_surface_get_height(surface); 536 | 537 | submap_grid.submap_id = submap_id; 538 | submap_grid.resolution = kOccupyGridResolution; 539 | submap_grid.width = width; 540 | submap_grid.height = height; 541 | submap_grid.x0 = - origin.x() * kOccupyGridResolution; 542 | submap_grid.y0 = (origin.y() - height) * kOccupyGridResolution; 543 | 544 | const uint32_t* pixel_data = reinterpret_cast(cairo_image_surface_get_data(surface)); 545 | submap_grid.data.reserve(width*height); 546 | for(int y=0;y> 16; 550 | const unsigned char observed = packed >> 8; 551 | const int value = 552 | observed == 0 553 | ? -1 554 | : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.); 555 | submap_grid.data.push_back(value); 556 | } 557 | } 558 | std::cout<<"Successfully Add Submap "< near_submaps = CloseSubmaps(submap_entry.pose.position, parameters_.distance_threshold_for_adding); 585 | SubmapId next_submap {submap_id.trajectory_id,submap_id.submap_index+1}; 586 | SubmapId last_submap {submap_id.trajectory_id,submap_id.submap_index+1}; 587 | 588 | near_submaps.push_back(std::move(next_submap)); 589 | near_submaps.push_back(std::move(last_submap)); 590 | 591 | for(auto& near_submap:near_submaps){ 592 | // Check near_submap exists and is not submap itself 593 | if(submap_.count(near_submap)==0) continue; 594 | if(near_submap==submap_id) continue; 595 | 596 | // Connect two submpas 597 | Path path = PlanPathRRT(submap_id,near_submap); 598 | 599 | if(path.empty()){ 600 | std::cout<<"Warning!! Fail to connect "<submap){ 628 | SubmapId submap_id {submap_entry.trajectory_id, submap_entry.submap_index}; 629 | if(submap_entry.submap_version < kFinishVersion) continue; 630 | 631 | // add new finished submap into road_map_ 632 | if(submap_.find(submap_id)==submap_.end()){ 633 | submap_[submap_id] = submap_entry; 634 | submap_kdtree_.AddPointToKdTree(submap_entry.pose.position, submap_id.trajectory_id, submap_id.submap_index); 635 | AddSubmapGrid(submap_id); 636 | AddRoadMapEntry(submap_id); 637 | } else{ 638 | auto& old_submap_entry = submap_[submap_id]; 639 | double distance2 = Distance2BetweenPose(old_submap_entry.pose,submap_entry.pose); 640 | double rotation = RotationBetweenPose(old_submap_entry.pose,submap_entry.pose); 641 | if(rotation > parameters_.rotation_threshold_for_updating || 642 | distance2 > parameters_.distance2_threshold_for_updating){ 643 | submap_[submap_id] = submap_entry; 644 | AddSubmapGrid(submap_id); 645 | AddRoadMapEntry(submap_id); 646 | } 647 | } 648 | } 649 | } 650 | 651 | 652 | bool PathPlannerNode::ReconnectSubmapService(::cartographer_ros_msgs::ReconnectSubmaps::Request &req, 653 | ::cartographer_ros_msgs::ReconnectSubmaps::Response &res){ 654 | SubmapId start_submap_id {req.start_trajectory_id, req.start_submap_index}; 655 | SubmapId end_submap_id {req.end_trajectory_id, req.end_submap_index}; 656 | ReconnectSubmaps(start_submap_id,end_submap_id); 657 | return true; 658 | } 659 | 660 | bool PathPlannerNode::QueryRoadmap(::cartographer_ros_msgs::RoadmapQuery::Request &req, 661 | ::cartographer_ros_msgs::RoadmapQuery::Response &res){ 662 | SubmapId submap_id {req.trajectory_id, req.submap_index}; 663 | const auto& pair = road_map_.find(submap_id); 664 | if(pair==road_map_.end()){ 665 | std::cout<<"Submap "<second){ 670 | std::cout<second.find(end_submap_id); 688 | if(entry==pair->second.end()){ 689 | return true; 690 | } 691 | res.path = entry->second.path; 692 | return true; 693 | } 694 | 695 | bool PathPlannerNode::PlanPath(cartographer_ros_msgs::PathPlan::Request &req, 696 | cartographer_ros_msgs::PathPlan::Response &res) { 697 | geometry_msgs::Point start_point = req.start_point; 698 | geometry_msgs::Point end_point = req.end_point; 699 | res.path = PlanPathRRT(start_point,end_point); 700 | if(!res.path.empty()) AddDisplayPath(res.path); 701 | return true; 702 | } 703 | 704 | void PathPlannerNode::PublishPath(const ::ros::WallTimerEvent& timer_event){ 705 | path_publisher_.publish(path_to_display_); 706 | } 707 | 708 | /* 709 | Functions below are for test 710 | */ 711 | 712 | void PathPlannerNode::IsClickedPointFree(const geometry_msgs::PointStamped::ConstPtr& msg) const { 713 | //std::cout<point.x<<","<point.y<<":"; 714 | std::cout<point); 715 | std::cout<point.x<<","<point.y<point); 725 | if(path.empty()){ 726 | std::cout<<"Fail to find a valid path!"<point); 728 | std::cout<point,submap_id)<point); 732 | std::cout<point,submap_id)<