├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── launch ├── test-mapping.launch ├── test-pathplanner.launch ├── vrep-absolem.yaml └── vrep-p3dx.yaml ├── package.xml └── src ├── move_base_node.cpp ├── navigation_function_node.cpp └── next_best_view_node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | 30 | # vim swap files 31 | .*.swp 32 | 33 | # catkin tools 34 | .catkin_tools 35 | 36 | /build 37 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(octomap_path_planner) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | sensor_msgs 9 | geometry_msgs 10 | octomap_msgs 11 | octomap_ros 12 | pcl_ros 13 | roscpp 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | find_package(PCL REQUIRED) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 38 | ## pulled in transitively but can be declared for certainty nonetheless: 39 | ## * add a build_depend tag for "message_generation" 40 | ## * add a run_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # geometry_msgs# octomap_msgs 76 | # ) 77 | 78 | ################################### 79 | ## catkin specific configuration ## 80 | ################################### 81 | ## The catkin_package macro generates cmake config files for your package 82 | ## Declare things to be passed to dependent projects 83 | ## INCLUDE_DIRS: uncomment this if you package contains header files 84 | ## LIBRARIES: libraries you create in this project that dependent projects also need 85 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 86 | ## DEPENDS: system dependencies of this project that dependent projects also need 87 | catkin_package( 88 | # INCLUDE_DIRS include 89 | # LIBRARIES octomap_path_planner 90 | # CATKIN_DEPENDS sensor_msgs geometry_msgs octomap_msgs octomap_ros roscpp 91 | # DEPENDS system_lib 92 | ) 93 | 94 | ########### 95 | ## Build ## 96 | ########### 97 | 98 | ## Specify additional locations of header files 99 | ## Your package locations should be listed before other locations 100 | # include_directories(include) 101 | include_directories( 102 | ${catkin_INCLUDE_DIRS} 103 | ${PCL_INCLUDE_DIRS} 104 | ) 105 | 106 | link_directories( 107 | ${PCL_LIBRARY_DIRS} 108 | ) 109 | 110 | ## Declare a cpp library 111 | # add_library(octomap_path_planner 112 | # src/${PROJECT_NAME}/octomap_path_planner.cpp 113 | # ) 114 | 115 | ## Declare a cpp executable 116 | add_executable(navigation_function_node src/navigation_function_node.cpp) 117 | add_executable(move_base_node src/move_base_node.cpp) 118 | add_executable(next_best_view_node src/next_best_view_node.cpp) 119 | 120 | ## Add cmake target dependencies of the executable/library 121 | ## as an example, message headers may need to be generated before nodes 122 | # add_dependencies(octomap_path_planner_node octomap_path_planner_generate_messages_cpp) 123 | 124 | ## Specify libraries to link a library or executable target against 125 | target_link_libraries(navigation_function_node 126 | ${catkin_LIBRARIES} 127 | ${PCL_LIBRARIES} 128 | ) 129 | target_link_libraries(move_base_node 130 | ${catkin_LIBRARIES} 131 | ${PCL_LIBRARIES} 132 | ) 133 | target_link_libraries(next_best_view_node 134 | ${catkin_LIBRARIES} 135 | ${PCL_LIBRARIES} 136 | ) 137 | 138 | ############# 139 | ## Install ## 140 | ############# 141 | 142 | # all install targets should use catkin DESTINATION variables 143 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 144 | 145 | ## Mark executable scripts (Python etc.) for installation 146 | ## in contrast to setup.py, you can choose the destination 147 | # install(PROGRAMS 148 | # scripts/my_python_script 149 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 150 | # ) 151 | 152 | ## Mark executables and/or libraries for installation 153 | # install(TARGETS octomap_path_planner octomap_path_planner_node 154 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 155 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 156 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 157 | # ) 158 | 159 | ## Mark cpp header files for installation 160 | # install(DIRECTORY include/${PROJECT_NAME}/ 161 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 162 | # FILES_MATCHING PATTERN "*.h" 163 | # PATTERN ".svn" EXCLUDE 164 | # ) 165 | 166 | ## Mark other files for installation (e.g. launch and bag files, etc.) 167 | # install(FILES 168 | # # myfile1 169 | # # myfile2 170 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 171 | # ) 172 | 173 | ############# 174 | ## Testing ## 175 | ############# 176 | 177 | ## Add gtest based cpp test target and link libraries 178 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_octomap_path_planner.cpp) 179 | # if(TARGET ${PROJECT_NAME}-test) 180 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 181 | # endif() 182 | 183 | ## Add folders to be run by python nosetests 184 | # catkin_add_nosetests(test) 185 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Federico Ferri 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of octomap_path_planner nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # octomap_path_planner 2 | A wavefront path planner for planning in octree 3D maps. 3 | -------------------------------------------------------------------------------- /launch/test-mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/test-pathplanner.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /launch/vrep-absolem.yaml: -------------------------------------------------------------------------------- 1 | frame_id: /map 2 | laser_frame_id: /laser 3 | robot_frame_id: /base_link 4 | robot_height: 0.5 5 | robot_radius: 0.46 6 | -------------------------------------------------------------------------------- /launch/vrep-p3dx.yaml: -------------------------------------------------------------------------------- 1 | frame_id: /map 2 | laser_frame_id: /fast3DLaserScanner_sensor 3 | robot_frame_id: /Pioneer_p3dx 4 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | octomap_path_planner 4 | 0.0.1 5 | The octomap_path_planner package 6 | Federico Ferri 7 | BSD 8 | Federico Ferri 9 | catkin 10 | sensor_msgs 11 | geometry_msgs 12 | octomap_msgs 13 | octomap_ros 14 | octomap_server 15 | pcl_ros 16 | roscpp 17 | sensor_msgs 18 | geometry_msgs 19 | octomap_msgs 20 | octomap_ros 21 | octomap_server 22 | pcl_ros 23 | roscpp 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/move_base_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | 48 | template 49 | double sqdist(const PointA& a, const PointB& b) 50 | { 51 | double dx = a.x - b.x; 52 | double dy = a.y - b.y; 53 | double dz = a.z - b.z; 54 | return dx * dx + dy * dy + dz * dz; 55 | } 56 | 57 | 58 | template 59 | double dist(const PointA& a, const PointB& b) 60 | { 61 | return sqrt(sqdist(a, b)); 62 | } 63 | 64 | 65 | class MoveBase 66 | { 67 | protected: 68 | ros::NodeHandle nh_; 69 | ros::NodeHandle pnh_; 70 | std::string frame_id_; 71 | std::string robot_frame_id_; 72 | ros::Subscriber navfn_sub_; 73 | ros::Subscriber goal_point_sub_; 74 | ros::Subscriber goal_pose_sub_; 75 | ros::Publisher twist_pub_; 76 | ros::Publisher target_pub_; 77 | ros::Publisher position_error_pub_; 78 | ros::Publisher orientation_error_pub_; 79 | tf::TransformListener tf_listener_; 80 | geometry_msgs::PoseStamped robot_pose_; 81 | geometry_msgs::PoseStamped goal_; 82 | pcl::PointCloud navfn_; 83 | pcl::octree::OctreePointCloudSearch::Ptr navfn_octree_ptr_; 84 | ros::Timer controller_timer_; 85 | double robot_radius_; 86 | double goal_reached_threshold_; 87 | double controller_frequency_; 88 | double local_target_radius_; 89 | double twist_linear_gain_; 90 | double twist_angular_gain_; 91 | bool reached_position_; 92 | int controller_repeated_failures_; 93 | void startController(); 94 | public: 95 | MoveBase(); 96 | ~MoveBase(); 97 | void onNavigationFunctionChange(const sensor_msgs::PointCloud2::ConstPtr& msg); 98 | void onGoal(const geometry_msgs::PointStamped::ConstPtr& msg); 99 | void onGoal(const geometry_msgs::PoseStamped::ConstPtr& msg); 100 | int projectPositionToNavigationFunction(const geometry_msgs::Point& pos); 101 | void getNavigationFunctionNeighborhood(const geometry_msgs::Point& pos, pcl::PointCloud& neighbors); 102 | bool projectGoalPositionToNavigationFunction(); 103 | bool getRobotPose(); 104 | double positionError(); 105 | double orientationError(); 106 | bool generateLocalTarget(geometry_msgs::PointStamped& p_local); 107 | void generateTwistCommand(const geometry_msgs::PointStamped& local_target, geometry_msgs::Twist& twist); 108 | void controllerCallback(const ros::TimerEvent& event); 109 | }; 110 | 111 | 112 | MoveBase::MoveBase() 113 | : pnh_("~"), 114 | frame_id_("/map"), 115 | robot_frame_id_("/base_link"), 116 | robot_radius_(0.2), 117 | goal_reached_threshold_(0.5), 118 | controller_frequency_(2.0), 119 | local_target_radius_(0.4), 120 | twist_linear_gain_(0.5), 121 | twist_angular_gain_(1.0), 122 | reached_position_(false), 123 | controller_repeated_failures_(0) 124 | { 125 | pnh_.param("frame_id", frame_id_, frame_id_); 126 | pnh_.param("robot_frame_id", robot_frame_id_, robot_frame_id_); 127 | pnh_.param("robot_radius", robot_radius_, robot_radius_); 128 | pnh_.param("goal_reached_threshold", goal_reached_threshold_, goal_reached_threshold_); 129 | pnh_.param("controller_frequency", controller_frequency_, controller_frequency_); 130 | pnh_.param("local_target_radius", local_target_radius_, local_target_radius_); 131 | pnh_.param("twist_linear_gain", twist_linear_gain_, twist_linear_gain_); 132 | pnh_.param("twist_angular_gain", twist_angular_gain_, twist_angular_gain_); 133 | navfn_sub_ = nh_.subscribe("navfn_in", 1, &MoveBase::onNavigationFunctionChange, this); 134 | goal_point_sub_ = nh_.subscribe("goal_point_in", 1, &MoveBase::onGoal, this); 135 | goal_pose_sub_ = nh_.subscribe("goal_pose_in", 1, &MoveBase::onGoal, this); 136 | twist_pub_ = nh_.advertise("twist_out", 1, false); 137 | target_pub_ = nh_.advertise("target_out", 1, false); 138 | position_error_pub_ = nh_.advertise("position_error", 10, false);; 139 | orientation_error_pub_ = nh_.advertise("orientation_error", 10, false);; 140 | } 141 | 142 | 143 | MoveBase::~MoveBase() 144 | { 145 | } 146 | 147 | 148 | void MoveBase::onNavigationFunctionChange(const sensor_msgs::PointCloud2::ConstPtr& pcl_msg) 149 | { 150 | pcl::PointCloud pcl; 151 | pcl::fromROSMsg(*pcl_msg, pcl); 152 | 153 | navfn_.clear(); 154 | pcl_ros::transformPointCloud(frame_id_, pcl, navfn_, tf_listener_); 155 | 156 | navfn_octree_ptr_ = pcl::octree::OctreePointCloudSearch::Ptr(new pcl::octree::OctreePointCloudSearch(0.01)); 157 | navfn_octree_ptr_->setInputCloud(navfn_.makeShared()); 158 | navfn_octree_ptr_->addPointsFromInputCloud(); 159 | } 160 | 161 | 162 | void MoveBase::startController() 163 | { 164 | reached_position_ = false; 165 | controller_repeated_failures_ = 0; 166 | controller_timer_ = nh_.createTimer(ros::Duration(1.0 / controller_frequency_), &MoveBase::controllerCallback, this); 167 | } 168 | 169 | 170 | void MoveBase::onGoal(const geometry_msgs::PointStamped::ConstPtr& msg) 171 | { 172 | try 173 | { 174 | geometry_msgs::PointStamped msg2; 175 | tf_listener_.transformPoint(frame_id_, *msg, msg2); 176 | goal_.header.stamp = msg2.header.stamp; 177 | goal_.header.frame_id = msg2.header.frame_id; 178 | goal_.pose.position.x = msg2.point.x; 179 | goal_.pose.position.y = msg2.point.y; 180 | goal_.pose.position.z = msg2.point.z; 181 | goal_.pose.orientation.x = 0.0; 182 | goal_.pose.orientation.y = 0.0; 183 | goal_.pose.orientation.z = 0.0; 184 | goal_.pose.orientation.w = 0.0; 185 | 186 | if(projectGoalPositionToNavigationFunction()) 187 | { 188 | ROS_INFO("goal set to point (%f, %f, %f)", 189 | goal_.pose.position.x, goal_.pose.position.y, goal_.pose.position.z); 190 | 191 | startController(); 192 | } 193 | } 194 | catch(tf::TransformException& ex) 195 | { 196 | ROS_ERROR("Failed to lookup robot position: %s", ex.what()); 197 | } 198 | } 199 | 200 | 201 | void MoveBase::onGoal(const geometry_msgs::PoseStamped::ConstPtr& msg) 202 | { 203 | try 204 | { 205 | tf_listener_.transformPose(frame_id_, *msg, goal_); 206 | 207 | if(projectGoalPositionToNavigationFunction()) 208 | { 209 | ROS_INFO("goal set to pose (%f, %f, %f), (%f, %f, %f, %f)", 210 | goal_.pose.position.x, goal_.pose.position.y, goal_.pose.position.z, 211 | goal_.pose.orientation.x, goal_.pose.orientation.y, goal_.pose.orientation.z, 212 | goal_.pose.orientation.w); 213 | 214 | startController(); 215 | } 216 | } 217 | catch(tf::TransformException& ex) 218 | { 219 | ROS_ERROR("Failed to lookup robot position: %s", ex.what()); 220 | } 221 | } 222 | 223 | 224 | int MoveBase::projectPositionToNavigationFunction(const geometry_msgs::Point& pos) 225 | { 226 | pcl::PointXYZI p; 227 | p.x = pos.x; 228 | p.y = pos.y; 229 | p.z = pos.z; 230 | std::vector pointIdx; 231 | std::vector pointDistSq; 232 | if(navfn_octree_ptr_->nearestKSearch(p, 1, pointIdx, pointDistSq) < 1) 233 | { 234 | return -1; 235 | } 236 | return pointIdx[0]; 237 | } 238 | 239 | 240 | void MoveBase::getNavigationFunctionNeighborhood(const geometry_msgs::Point& pos, pcl::PointCloud& neighbors) 241 | { 242 | pcl::PointXYZI robot_position; 243 | 244 | robot_position.x = pos.x; 245 | robot_position.y = pos.y; 246 | robot_position.z = pos.z; 247 | 248 | std::vector pointIdx; 249 | std::vector pointDistSq; 250 | 251 | navfn_octree_ptr_->radiusSearch(robot_position, local_target_radius_, pointIdx, pointDistSq); 252 | 253 | neighbors.header.frame_id = navfn_.header.frame_id; 254 | neighbors.header.stamp = navfn_.header.stamp; 255 | for(std::vector::iterator it = pointIdx.begin(); it != pointIdx.end(); ++it) 256 | neighbors.push_back(navfn_[*it]); 257 | } 258 | 259 | 260 | bool MoveBase::projectGoalPositionToNavigationFunction() 261 | { 262 | int goal_index = projectPositionToNavigationFunction(goal_.pose.position); 263 | if(goal_index == -1) 264 | { 265 | ROS_ERROR("Failed to project goal position to navfn pcl"); 266 | return false; 267 | } 268 | if(dist(goal_.pose.position, navfn_[goal_index]) > robot_radius_) 269 | { 270 | ROS_ERROR("Failed to project goal position to navfn pcl (point is too far from ground)"); 271 | return false; 272 | } 273 | goal_.pose.position.x = navfn_[goal_index].x; 274 | goal_.pose.position.y = navfn_[goal_index].y; 275 | goal_.pose.position.z = navfn_[goal_index].z; 276 | return true; 277 | } 278 | 279 | 280 | bool MoveBase::getRobotPose() 281 | { 282 | try 283 | { 284 | geometry_msgs::PoseStamped robot_pose_local; 285 | robot_pose_local.header.frame_id = robot_frame_id_; 286 | robot_pose_local.pose.position.x = 0.0; 287 | robot_pose_local.pose.position.y = 0.0; 288 | robot_pose_local.pose.position.z = 0.0; 289 | robot_pose_local.pose.orientation.x = 0.0; 290 | robot_pose_local.pose.orientation.y = 0.0; 291 | robot_pose_local.pose.orientation.z = 0.0; 292 | robot_pose_local.pose.orientation.w = 1.0; 293 | tf_listener_.transformPose(frame_id_, robot_pose_local, robot_pose_); 294 | return true; 295 | } 296 | catch(tf::TransformException& ex) 297 | { 298 | ROS_ERROR("Failed to lookup robot position: %s", ex.what()); 299 | } 300 | } 301 | 302 | 303 | double MoveBase::positionError() 304 | { 305 | return sqrt( 306 | pow(robot_pose_.pose.position.x - goal_.pose.position.x, 2) + 307 | pow(robot_pose_.pose.position.y - goal_.pose.position.y, 2) + 308 | pow(robot_pose_.pose.position.z - goal_.pose.position.z, 2) 309 | ); 310 | } 311 | 312 | 313 | double MoveBase::orientationError() 314 | { 315 | // check if goal is only by position: 316 | double qnorm = pow(goal_.pose.orientation.w, 2) + 317 | pow(goal_.pose.orientation.x, 2) + 318 | pow(goal_.pose.orientation.y, 2) + 319 | pow(goal_.pose.orientation.z, 2); 320 | 321 | // if so, we never have an orientation error: 322 | if(qnorm < 1e-5) return 0; 323 | 324 | // "Robotica - Modellistica Pianificazione e Controllo" eq. 3.88 325 | double nd = goal_.pose.orientation.w, 326 | ne = robot_pose_.pose.orientation.w; 327 | Eigen::Vector3d ed(goal_.pose.orientation.x, goal_.pose.orientation.y, goal_.pose.orientation.z), 328 | ee(robot_pose_.pose.orientation.x, robot_pose_.pose.orientation.y, robot_pose_.pose.orientation.z); 329 | Eigen::Vector3d eo = ne * ed - nd * ee - ed.cross(ee); 330 | return eo(2); 331 | } 332 | 333 | 334 | bool MoveBase::generateLocalTarget(geometry_msgs::PointStamped& p_local) 335 | { 336 | // get navigation function neighborhood centered at robot pos: 337 | pcl::PointCloud neighbors, neighbors_local; 338 | getNavigationFunctionNeighborhood(robot_pose_.pose.position, neighbors); 339 | 340 | if(!pcl_ros::transformPointCloud(robot_frame_id_, neighbors, neighbors_local, tf_listener_)) 341 | { 342 | ROS_ERROR("Failed to transform robot neighborhood"); 343 | return false; 344 | } 345 | 346 | // find minimum distance point in neighborhood: 347 | int min_index = -1, min_index_straight = -1; 348 | float min_value = std::numeric_limits::infinity(); 349 | 350 | for(size_t i = 0; i < neighbors_local.size(); i++) 351 | { 352 | pcl::PointXYZI& p = neighbors_local[i]; 353 | 354 | if(p.intensity < min_value) 355 | { 356 | min_value = p.intensity; 357 | min_index = i; 358 | } 359 | } 360 | 361 | if(min_index == -1) 362 | { 363 | ROS_ERROR("Failed to find a target in robot vicinity"); 364 | return false; 365 | } 366 | 367 | // check if we are actually improving the value in the navigation function 368 | int rob_index = projectPositionToNavigationFunction(robot_pose_.pose.position); 369 | double delta = navfn_[rob_index].intensity - min_value; 370 | if(delta < 1e-6) 371 | { 372 | ROS_ERROR("Failed to generate a target: gradient is null"); 373 | return false; 374 | } 375 | 376 | p_local.header.stamp = ros::Time::now(); 377 | p_local.header.frame_id = neighbors_local.header.frame_id; 378 | p_local.point.x = neighbors_local[min_index].x; 379 | p_local.point.y = neighbors_local[min_index].y; 380 | p_local.point.z = neighbors_local[min_index].z; 381 | 382 | target_pub_.publish(p_local); 383 | 384 | return true; 385 | } 386 | 387 | 388 | void MoveBase::generateTwistCommand(const geometry_msgs::PointStamped& local_target, geometry_msgs::Twist& twist) 389 | { 390 | if(local_target.header.frame_id != robot_frame_id_) 391 | { 392 | ROS_ERROR("generateTwistCommand: local_target must be in frame '%s'", robot_frame_id_.c_str()); 393 | return; 394 | } 395 | 396 | twist.linear.x = 0.0; 397 | twist.linear.y = 0.0; 398 | twist.linear.z = 0.0; 399 | twist.angular.x = 0.0; 400 | twist.angular.y = 0.0; 401 | twist.angular.z = 0.0; 402 | 403 | const geometry_msgs::Point& p = local_target.point; 404 | 405 | if(p.x < 0 || fabs(p.y) > p.x) 406 | { 407 | // turn in place 408 | twist.angular.z = (p.y > 0 ? 1 : -1) * twist_angular_gain_; 409 | } 410 | else 411 | { 412 | // make arc 413 | double center_y = (pow(p.x, 2) + pow(p.y, 2)) / (2 * p.y); 414 | double theta = fabs(atan2(p.x, fabs(center_y) - fabs(p.y))); 415 | double arc_length = fabs(center_y * theta); 416 | 417 | twist.linear.x = twist_linear_gain_ * arc_length; 418 | twist.angular.z = twist_angular_gain_ * (p.y >= 0 ? 1 : -1) * theta; 419 | } 420 | } 421 | 422 | 423 | void MoveBase::controllerCallback(const ros::TimerEvent& event) 424 | { 425 | if(controller_repeated_failures_ >= 5) 426 | { 427 | ROS_ERROR("controller keeps failing. giving up :-("); 428 | controller_timer_.stop(); 429 | return; 430 | } 431 | 432 | if(!getRobotPose()) 433 | { 434 | controller_repeated_failures_++; 435 | ROS_ERROR("controllerCallback: failed to get robot pose"); 436 | return; 437 | } 438 | 439 | geometry_msgs::Twist twist; 440 | twist.linear.x = 0.0; 441 | twist.linear.y = 0.0; 442 | twist.linear.z = 0.0; 443 | twist.angular.x = 0.0; 444 | twist.angular.y = 0.0; 445 | twist.angular.z = 0.0; 446 | 447 | std_msgs::Float32 ep, eo; 448 | 449 | ep.data = positionError(); 450 | position_error_pub_.publish(ep); 451 | 452 | eo.data = orientationError(); 453 | orientation_error_pub_.publish(eo); 454 | 455 | const char *status_str; 456 | 457 | if((!reached_position_ && ep.data > goal_reached_threshold_) 458 | || (reached_position_ && ep.data > 2 * goal_reached_threshold_)) 459 | { 460 | // regulate position 461 | 462 | status_str = "REGULATING POSITION"; 463 | 464 | reached_position_ = false; 465 | 466 | geometry_msgs::PointStamped local_target; 467 | 468 | if(!generateLocalTarget(local_target)) 469 | { 470 | controller_repeated_failures_++; 471 | ROS_ERROR("controllerCallback: failed to generate a local target to follow"); 472 | return; 473 | } 474 | 475 | generateTwistCommand(local_target, twist); 476 | } 477 | else 478 | { 479 | reached_position_ = true; 480 | 481 | if(fabs(eo.data) > 0.02) 482 | { 483 | // regulate orientation 484 | 485 | status_str = "REGULATING ORIENTATION"; 486 | 487 | twist.angular.z = twist_angular_gain_ * eo.data; 488 | } 489 | else 490 | { 491 | // goal reached 492 | 493 | status_str = "REACHED GOAL"; 494 | 495 | ROS_INFO("goal reached! stopping controller timer"); 496 | 497 | controller_timer_.stop(); 498 | } 499 | } 500 | 501 | ROS_INFO("controller: ep=%f, eo=%f, status=%s", ep.data, eo.data, status_str); 502 | 503 | twist_pub_.publish(twist); 504 | 505 | controller_repeated_failures_ = 0; 506 | } 507 | 508 | 509 | int main(int argc, char **argv) 510 | { 511 | ros::init(argc, argv, "move_base"); 512 | 513 | MoveBase p; 514 | ros::spin(); 515 | 516 | return 0; 517 | } 518 | 519 | -------------------------------------------------------------------------------- /src/navigation_function_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | 46 | namespace pcl 47 | { 48 | template 49 | double sqdist(const PointA& a, const PointB& b) 50 | { 51 | double dx = a.x - b.x; 52 | double dy = a.y - b.y; 53 | double dz = a.z - b.z; 54 | return dx * dx + dy * dy + dz * dz; 55 | } 56 | 57 | template 58 | double dist(const PointA& a, const PointB& b) 59 | { 60 | return sqrt(sqdist(a, b)); 61 | } 62 | } 63 | 64 | class NavigationFunction 65 | { 66 | protected: 67 | ros::NodeHandle nh_; 68 | ros::NodeHandle pnh_; 69 | std::string frame_id_; 70 | std::string robot_frame_id_; 71 | ros::Subscriber octree_sub_; 72 | ros::Subscriber goal_point_sub_; 73 | ros::Subscriber goal_pose_sub_; 74 | ros::Publisher ground_pub_; 75 | ros::Publisher obstacles_pub_; 76 | ros::Publisher reprojected_point_goal_pub_; 77 | ros::Publisher reprojected_pose_goal_pub_; 78 | tf::TransformListener tf_listener_; 79 | geometry_msgs::PoseStamped robot_pose_; 80 | geometry_msgs::PoseStamped goal_; 81 | octomap::OcTree* octree_ptr_; 82 | pcl::PointCloud ground_pcl_; 83 | pcl::PointCloud obstacles_pcl_; 84 | pcl::octree::OctreePointCloudSearch::Ptr ground_octree_ptr_; 85 | pcl::octree::OctreePointCloudSearch::Ptr obstacles_octree_ptr_; 86 | bool treat_unknown_as_free_; 87 | double robot_height_; 88 | double robot_radius_; 89 | double max_superable_height_; 90 | double ground_voxel_connectivity_; 91 | public: 92 | NavigationFunction(); 93 | ~NavigationFunction(); 94 | void onOctomap(const octomap_msgs::Octomap::ConstPtr& msg); 95 | void onGoal(const geometry_msgs::PointStamped::ConstPtr& msg); 96 | void onGoal(const geometry_msgs::PoseStamped::ConstPtr& msg); 97 | void expandOcTree(); 98 | bool isGround(const octomap::OcTreeKey& key); 99 | bool isObstacle(const octomap::OcTreeKey& key); 100 | bool isNearObstacle(const pcl::PointXYZI& point); 101 | void filterInflatedRegionFromGround(); 102 | void computeGround(); 103 | void projectGoalPositionToGround(); 104 | void publishGroundCloud(); 105 | int getGoalIndex(); 106 | void getNeighboringGroundPoints(int index, std::vector& neighbors, double search_radius, bool exclude_already_labeled); 107 | void computeDistanceTransform(); 108 | double getAverageIntensity(int index, double search_radius); 109 | void smoothIntensity(double search_radius); 110 | void normalizeIntensity(); 111 | }; 112 | 113 | 114 | NavigationFunction::NavigationFunction() 115 | : pnh_("~"), 116 | frame_id_("/map"), 117 | robot_frame_id_("/base_link"), 118 | octree_ptr_(0L), 119 | treat_unknown_as_free_(false), 120 | robot_height_(0.5), 121 | robot_radius_(0.5), 122 | max_superable_height_(0.2), 123 | ground_voxel_connectivity_(1.8) 124 | { 125 | pnh_.param("frame_id", frame_id_, frame_id_); 126 | pnh_.param("robot_frame_id", robot_frame_id_, robot_frame_id_); 127 | pnh_.param("treat_unknown_as_free", treat_unknown_as_free_, treat_unknown_as_free_); 128 | pnh_.param("robot_height", robot_height_, robot_height_); 129 | pnh_.param("robot_radius", robot_radius_, robot_radius_); 130 | pnh_.param("max_superable_height", max_superable_height_, max_superable_height_); 131 | pnh_.param("ground_voxel_connectivity", ground_voxel_connectivity_, ground_voxel_connectivity_); 132 | octree_sub_ = nh_.subscribe("octree_in", 1, &NavigationFunction::onOctomap, this); 133 | goal_point_sub_ = nh_.subscribe("goal_point_in", 1, &NavigationFunction::onGoal, this); 134 | goal_pose_sub_ = nh_.subscribe("goal_pose_in", 1, &NavigationFunction::onGoal, this); 135 | ground_pub_ = nh_.advertise("ground_cloud_out", 1, true); 136 | obstacles_pub_ = nh_.advertise("obstacles_cloud_out", 1, true); 137 | reprojected_point_goal_pub_ = nh_.advertise("reprojected_point_goal", 1, true); 138 | reprojected_pose_goal_pub_ = nh_.advertise("reprojected_pose_goal", 1, true); 139 | ground_pcl_.header.frame_id = frame_id_; 140 | obstacles_pcl_.header.frame_id = frame_id_; 141 | } 142 | 143 | 144 | NavigationFunction::~NavigationFunction() 145 | { 146 | if(octree_ptr_) delete octree_ptr_; 147 | } 148 | 149 | 150 | void NavigationFunction::onOctomap(const octomap_msgs::Octomap::ConstPtr& msg) 151 | { 152 | if(octree_ptr_) delete octree_ptr_; 153 | octree_ptr_ = octomap_msgs::binaryMsgToMap(*msg); 154 | 155 | expandOcTree(); 156 | computeGround(); 157 | computeDistanceTransform(); 158 | } 159 | 160 | 161 | void NavigationFunction::onGoal(const geometry_msgs::PointStamped::ConstPtr& msg) 162 | { 163 | geometry_msgs::PointStamped msg2; 164 | 165 | try 166 | { 167 | tf_listener_.transformPoint(frame_id_, *msg, msg2); 168 | goal_.header.stamp = msg2.header.stamp; 169 | goal_.header.frame_id = msg2.header.frame_id; 170 | goal_.pose.position.x = msg2.point.x; 171 | goal_.pose.position.y = msg2.point.y; 172 | goal_.pose.position.z = msg2.point.z; 173 | goal_.pose.orientation.x = 0.0; 174 | goal_.pose.orientation.y = 0.0; 175 | goal_.pose.orientation.z = 0.0; 176 | goal_.pose.orientation.w = 0.0; 177 | projectGoalPositionToGround(); 178 | ROS_INFO("goal set to point (%f, %f, %f)", 179 | goal_.pose.position.x, goal_.pose.position.y, goal_.pose.position.z); 180 | } 181 | catch(tf::TransformException& ex) 182 | { 183 | ROS_ERROR("Failed to lookup robot position: %s", ex.what()); 184 | return; 185 | } 186 | 187 | computeDistanceTransform(); 188 | 189 | msg2.point.x = goal_.pose.position.x; 190 | msg2.point.y = goal_.pose.position.y; 191 | msg2.point.z = goal_.pose.position.z; 192 | reprojected_point_goal_pub_.publish(msg2); 193 | } 194 | 195 | 196 | void NavigationFunction::onGoal(const geometry_msgs::PoseStamped::ConstPtr& msg) 197 | { 198 | try 199 | { 200 | tf_listener_.transformPose(frame_id_, *msg, goal_); 201 | projectGoalPositionToGround(); 202 | ROS_INFO("goal set to pose (%f, %f, %f), (%f, %f, %f, %f)", 203 | goal_.pose.position.x, goal_.pose.position.y, goal_.pose.position.z, 204 | goal_.pose.orientation.x, goal_.pose.orientation.y, goal_.pose.orientation.z, 205 | goal_.pose.orientation.w); 206 | } 207 | catch(tf::TransformException& ex) 208 | { 209 | ROS_ERROR("Failed to lookup robot position: %s", ex.what()); 210 | return; 211 | } 212 | 213 | computeDistanceTransform(); 214 | 215 | reprojected_pose_goal_pub_.publish(goal_); 216 | } 217 | 218 | 219 | void NavigationFunction::expandOcTree() 220 | { 221 | if(!octree_ptr_) return; 222 | 223 | unsigned int maxDepth = octree_ptr_->getTreeDepth(); 224 | 225 | // expand collapsed occupied nodes until all occupied leaves are at maximum depth 226 | std::vector collapsed_occ_nodes; 227 | 228 | size_t initial_size = octree_ptr_->size(); 229 | size_t num_rounds = 0; 230 | size_t expanded_nodes = 0; 231 | 232 | do 233 | { 234 | collapsed_occ_nodes.clear(); 235 | for(octomap::OcTree::iterator it = octree_ptr_->begin(); it != octree_ptr_->end(); ++it) 236 | { 237 | if(octree_ptr_->isNodeOccupied(*it) && it.getDepth() < maxDepth) 238 | { 239 | collapsed_occ_nodes.push_back(&(*it)); 240 | } 241 | } 242 | for(std::vector::iterator it = collapsed_occ_nodes.begin(); it != collapsed_occ_nodes.end(); ++it) 243 | { 244 | (*it)->expandNode(); 245 | } 246 | 247 | expanded_nodes += collapsed_occ_nodes.size(); 248 | num_rounds++; 249 | } while(collapsed_occ_nodes.size() > 0); 250 | 251 | //ROS_INFO("received octree of %ld nodes; expanded %ld nodes in %ld rounds.", initial_size, expanded_nodes, num_rounds); 252 | } 253 | 254 | 255 | bool NavigationFunction::isGround(const octomap::OcTreeKey& key) 256 | { 257 | octomap::OcTreeNode *node = octree_ptr_->search(key); 258 | if(!node) return false; 259 | if(!octree_ptr_->isNodeOccupied(node)) return false; 260 | 261 | double res = octree_ptr_->getResolution(); 262 | int steps = ceil(robot_height_ / res); 263 | octomap::OcTreeKey key1(key); 264 | while(steps-- > 0) 265 | { 266 | key1[2]++; 267 | node = octree_ptr_->search(key1); 268 | if(!node) 269 | { 270 | if(!treat_unknown_as_free_) return false; 271 | } 272 | else 273 | { 274 | if(octree_ptr_->isNodeOccupied(node)) return false; 275 | } 276 | } 277 | return true; 278 | } 279 | 280 | 281 | bool NavigationFunction::isObstacle(const octomap::OcTreeKey& key) 282 | { 283 | octomap::OcTreeNode *node; 284 | double res = octree_ptr_->getResolution(); 285 | int num_voxels = 1; 286 | 287 | // look up... 288 | octomap::OcTreeKey key1(key); 289 | while(true) 290 | { 291 | key1[2]++; 292 | node = octree_ptr_->search(key1); 293 | if(!node) break; 294 | if(node && !octree_ptr_->isNodeOccupied(node)) break; 295 | num_voxels++; 296 | } 297 | 298 | // look down... 299 | octomap::OcTreeKey key2(key); 300 | while(true) 301 | { 302 | key2[2]--; 303 | node = octree_ptr_->search(key2); 304 | if(!node) break; 305 | if(node && !octree_ptr_->isNodeOccupied(node)) break; 306 | num_voxels++; 307 | } 308 | 309 | return res * num_voxels > max_superable_height_; 310 | } 311 | 312 | 313 | bool NavigationFunction::isNearObstacle(const pcl::PointXYZI& point) 314 | { 315 | std::vector pointIdx2; 316 | std::vector pointDistSq2; 317 | pcl::PointXYZ p; 318 | p.x = point.x; 319 | p.y = point.y; 320 | p.z = point.z; 321 | int num_points = obstacles_octree_ptr_->nearestKSearch(p, 1, pointIdx2, pointDistSq2); 322 | return num_points >= 1 && pointDistSq2[0] < (robot_radius_ * robot_radius_); 323 | } 324 | 325 | 326 | void NavigationFunction::filterInflatedRegionFromGround() 327 | { 328 | for(int i = 0; i < ground_pcl_.size(); ) 329 | { 330 | if(isNearObstacle(ground_pcl_[i])) 331 | { 332 | std::swap(*(ground_pcl_.begin() + i), ground_pcl_.back()); 333 | ground_pcl_.points.pop_back(); 334 | } 335 | else i++; 336 | } 337 | ground_pcl_.width = ground_pcl_.points.size(); 338 | ground_pcl_.height = 1; 339 | } 340 | 341 | 342 | void NavigationFunction::computeGround() 343 | { 344 | if(!octree_ptr_) return; 345 | 346 | ground_pcl_.clear(); 347 | obstacles_pcl_.clear(); 348 | 349 | for(octomap::OcTree::leaf_iterator it = octree_ptr_->begin(); it != octree_ptr_->end(); ++it) 350 | { 351 | if(!octree_ptr_->isNodeOccupied(*it)) continue; 352 | 353 | if(isGround(it.getKey())) 354 | { 355 | pcl::PointXYZI point; 356 | point.x = it.getX(); 357 | point.y = it.getY(); 358 | point.z = it.getZ(); 359 | point.intensity = std::numeric_limits::infinity(); 360 | ground_pcl_.push_back(point); 361 | } 362 | else if(isObstacle(it.getKey())) 363 | { 364 | pcl::PointXYZ point; 365 | point.x = it.getX(); 366 | point.y = it.getY(); 367 | point.z = it.getZ(); 368 | obstacles_pcl_.push_back(point); 369 | } 370 | } 371 | 372 | double res = octree_ptr_->getResolution(); 373 | 374 | obstacles_octree_ptr_ = pcl::octree::OctreePointCloudSearch::Ptr(new pcl::octree::OctreePointCloudSearch(res)); 375 | obstacles_octree_ptr_->setInputCloud(obstacles_pcl_.makeShared()); 376 | obstacles_octree_ptr_->addPointsFromInputCloud(); 377 | 378 | filterInflatedRegionFromGround(); 379 | 380 | ground_octree_ptr_ = pcl::octree::OctreePointCloudSearch::Ptr(new pcl::octree::OctreePointCloudSearch(res)); 381 | ground_octree_ptr_->setInputCloud(ground_pcl_.makeShared()); 382 | ground_octree_ptr_->addPointsFromInputCloud(); 383 | } 384 | 385 | 386 | void NavigationFunction::projectGoalPositionToGround() 387 | { 388 | pcl::PointXYZI goal; 389 | goal.x = goal_.pose.position.x; 390 | goal.y = goal_.pose.position.y; 391 | goal.z = goal_.pose.position.z; 392 | std::vector pointIdx; 393 | std::vector pointDistSq; 394 | if(ground_octree_ptr_->nearestKSearch(goal, 1, pointIdx, pointDistSq) < 1) 395 | { 396 | ROS_ERROR("Failed to project goal position to ground pcl"); 397 | return; 398 | } 399 | int i = pointIdx[0]; 400 | goal_.pose.position.x = ground_pcl_[i].x; 401 | goal_.pose.position.y = ground_pcl_[i].y; 402 | goal_.pose.position.z = ground_pcl_[i].z; 403 | } 404 | 405 | 406 | void NavigationFunction::publishGroundCloud() 407 | { 408 | if(ground_pub_.getNumSubscribers() > 0) 409 | { 410 | sensor_msgs::PointCloud2 msg; 411 | pcl::toROSMsg(ground_pcl_, msg); 412 | ground_pub_.publish(msg); 413 | } 414 | 415 | if(obstacles_pub_.getNumSubscribers() > 0) 416 | { 417 | sensor_msgs::PointCloud2 msg; 418 | pcl::toROSMsg(obstacles_pcl_, msg); 419 | obstacles_pub_.publish(msg); 420 | } 421 | } 422 | 423 | 424 | int NavigationFunction::getGoalIndex() 425 | { 426 | // find goal index in ground pcl: 427 | std::vector pointIdx; 428 | std::vector pointDistSq; 429 | pcl::PointXYZI goal; 430 | goal.x = goal_.pose.position.x; 431 | goal.y = goal_.pose.position.y; 432 | goal.z = goal_.pose.position.z; 433 | if(ground_octree_ptr_->nearestKSearch(goal, 1, pointIdx, pointDistSq) < 1) 434 | { 435 | return -1; 436 | } 437 | else 438 | { 439 | return pointIdx[0]; 440 | } 441 | } 442 | 443 | 444 | void NavigationFunction::getNeighboringGroundPoints(int index, std::vector& neighbors, double search_radius, bool exclude_already_labeled) 445 | { 446 | neighbors.clear(); 447 | std::vector pointDistSq; 448 | ground_octree_ptr_->radiusSearch(ground_pcl_[index], search_radius, neighbors, pointDistSq); 449 | } 450 | 451 | 452 | void NavigationFunction::computeDistanceTransform() 453 | { 454 | if(ground_pcl_.size() == 0) 455 | { 456 | ROS_INFO("skip computing distance transform because ground_pcl_ is empty"); 457 | return; 458 | } 459 | 460 | // find goal index in ground pcl: 461 | int goal_idx = getGoalIndex(); 462 | if(goal_idx == -1) 463 | { 464 | ROS_ERROR("unable to find goal in ground pcl"); 465 | return; 466 | } 467 | 468 | double search_radius = ground_voxel_connectivity_ * octree_ptr_->getResolution(); 469 | 470 | // distance to goal is zero (stored in the intensity channel): 471 | ground_pcl_[goal_idx].intensity = 0.0; 472 | 473 | std::queue q; 474 | q.push(goal_idx); 475 | while(!q.empty()) 476 | { 477 | int i = q.front(); 478 | q.pop(); 479 | 480 | std::vector neighbors; 481 | getNeighboringGroundPoints(i, neighbors, search_radius, true); 482 | 483 | for(std::vector::iterator it = neighbors.begin(); it != neighbors.end(); it++) 484 | { 485 | int j = *it; 486 | 487 | // intensity value are initially set to infinity. 488 | // if i is finite it means it has already been labeled. 489 | if(std::isfinite(ground_pcl_[j].intensity)) continue; 490 | 491 | // otherwise, label it: 492 | ground_pcl_[j].intensity = ground_pcl_[i].intensity + 493 | dist(ground_pcl_[j], ground_pcl_[i]); 494 | 495 | // continue exploring neighbours: 496 | q.push(j); 497 | } 498 | } 499 | 500 | //smoothIntensity(search_radius); 501 | normalizeIntensity(); 502 | 503 | publishGroundCloud(); 504 | } 505 | 506 | 507 | double NavigationFunction::getAverageIntensity(int index, double search_radius) 508 | { 509 | std::vector pointIdx; 510 | std::vector pointDistSq; 511 | ground_octree_ptr_->radiusSearch(ground_pcl_[index], search_radius, pointIdx, pointDistSq); 512 | 513 | if(pointIdx.size() == 0) return std::numeric_limits::infinity(); 514 | 515 | double i = 0.0; 516 | for(std::vector::iterator it = pointIdx.begin(); it != pointIdx.end(); ++it) 517 | { 518 | i += ground_pcl_[*it].intensity; 519 | } 520 | i /= (double)pointIdx.size(); 521 | return i; 522 | } 523 | 524 | 525 | void NavigationFunction::smoothIntensity(double search_radius) 526 | { 527 | std::vector smoothed_intensity; 528 | smoothed_intensity.resize(ground_pcl_.size()); 529 | for(size_t i = 0; i < ground_pcl_.size(); i++) 530 | { 531 | smoothed_intensity[i] = getAverageIntensity(i, search_radius); 532 | } 533 | for(size_t i = 0; i < ground_pcl_.size(); i++) 534 | { 535 | ground_pcl_[i] = smoothed_intensity[i]; 536 | } 537 | } 538 | 539 | 540 | void NavigationFunction::normalizeIntensity() 541 | { 542 | float imin = std::numeric_limits::infinity(); 543 | float imax = -std::numeric_limits::infinity(); 544 | for(pcl::PointCloud::iterator it = ground_pcl_.begin(); it != ground_pcl_.end(); ++it) 545 | { 546 | if(!std::isfinite(it->intensity)) continue; 547 | imin = fmin(imin, it->intensity); 548 | imax = fmax(imax, it->intensity); 549 | } 550 | const float eps = 0.01; 551 | float d = imax - imin + eps; 552 | for(pcl::PointCloud::iterator it = ground_pcl_.begin(); it != ground_pcl_.end(); ++it) 553 | { 554 | if(std::isfinite(it->intensity)) 555 | it->intensity = (it->intensity - imin) / d; 556 | else 557 | it->intensity = 1.0; 558 | } 559 | } 560 | 561 | 562 | int main(int argc, char **argv) 563 | { 564 | ros::init(argc, argv, "navigation_function"); 565 | 566 | NavigationFunction p; 567 | ros::spin(); 568 | 569 | return 0; 570 | } 571 | 572 | -------------------------------------------------------------------------------- /src/next_best_view_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | class NextBestView 41 | { 42 | public: 43 | NextBestView(); 44 | ~NextBestView(); 45 | bool isNearVoid(const octomap::point3d& p1, const unsigned char depth, const double res); 46 | void computeNextBestViews(); 47 | void onOctomap(const octomap_msgs::Octomap::ConstPtr& m); 48 | protected: 49 | std::string frame_id_; 50 | std::string robot_frame_id_; 51 | int num_clusters_; 52 | int min_computation_interval_; 53 | double normal_search_radius_; 54 | int min_pts_per_cluster_; 55 | double eps_angle_; 56 | double tolerance_; 57 | double boundary_angle_threshold_; 58 | ros::NodeHandle nh_; 59 | ros::NodeHandle private_node_handle_; 60 | octomap::OcTree *octree_ptr_; 61 | ros::Time last_computation_time_; 62 | ros::Publisher void_frontier_pub_; 63 | ros::Publisher posearray_pub_; 64 | std::vector cluster_pub_; 65 | ros::Subscriber octree_sub_; 66 | }; 67 | 68 | 69 | NextBestView::NextBestView() : 70 | frame_id_("/map"), 71 | robot_frame_id_("/base_link"), 72 | num_clusters_(3), 73 | min_computation_interval_(5 /*seconds*/), 74 | normal_search_radius_(0.4), 75 | min_pts_per_cluster_(5), 76 | eps_angle_(0.25), 77 | tolerance_(0.3), 78 | boundary_angle_threshold_(2.5), 79 | private_node_handle_("~"), 80 | octree_ptr_(0L) 81 | { 82 | private_node_handle_.param("frame_id", frame_id_, frame_id_); 83 | private_node_handle_.param("robot_frame_id", robot_frame_id_, robot_frame_id_); 84 | private_node_handle_.param("num_clusters", num_clusters_, num_clusters_); 85 | private_node_handle_.param("min_computation_interval", min_computation_interval_, min_computation_interval_); 86 | private_node_handle_.param("normal_search_radius", normal_search_radius_, normal_search_radius_); 87 | private_node_handle_.param("min_pts_per_cluster", min_pts_per_cluster_, min_pts_per_cluster_); 88 | private_node_handle_.param("eps_angle", eps_angle_, eps_angle_); 89 | private_node_handle_.param("tolerance", tolerance_, tolerance_); 90 | private_node_handle_.param("boundary_angle_threshold", boundary_angle_threshold_, boundary_angle_threshold_); 91 | 92 | octree_sub_ = nh_.subscribe("octree_in", 1, &NextBestView::onOctomap, this); 93 | void_frontier_pub_ = nh_.advertise("void_frontier", 1, false); 94 | posearray_pub_ = nh_.advertise("poses", 1, false); 95 | for(int i = 0; i < num_clusters_; i++) 96 | { 97 | std::stringstream ss; ss << "cluster_pcl_" << (i+1); 98 | cluster_pub_.push_back(nh_.advertise(ss.str(), 1, false)); 99 | } 100 | } 101 | 102 | 103 | NextBestView::~NextBestView() 104 | { 105 | if(octree_ptr_) delete octree_ptr_; 106 | } 107 | 108 | 109 | /** 110 | * Check if a point is near the unknown space (by 1 voxel). 111 | * Note: this assumes the point is contained by a cell in the octree. 112 | */ 113 | bool NextBestView::isNearVoid(const octomap::point3d& p1, const unsigned char depth, const double res) 114 | { 115 | for(int dz = 0; dz <= 0; dz += 2) 116 | { 117 | for(int dy = -1; dy <= 1; dy += 2) 118 | { 119 | for(int dx = -1; dx <= 1; dx += 2) 120 | { 121 | octomap::OcTreeNode *pNode = octree_ptr_->search(p1.x() + res * dx, p1.y() + res * dy, p1.z() + res * dz, depth); 122 | if(!pNode) return true; 123 | } 124 | } 125 | } 126 | return false; 127 | } 128 | 129 | 130 | static bool compareClusters(pcl::PointIndices c1, pcl::PointIndices c2) 131 | { 132 | return (c1.indices.size() < c2.indices.size()); 133 | } 134 | 135 | 136 | /** 137 | * Compute void frontier points (leaf points in free space adjacent to unknown space) 138 | */ 139 | void NextBestView::computeNextBestViews() 140 | { 141 | const unsigned char depth = 16; 142 | const double res = octree_ptr_->getResolution(); 143 | 144 | // compute void frontier: 145 | octomap::point3d_list pl; 146 | for(octomap::OcTree::leaf_iterator it = octree_ptr_->begin_leafs(depth); it != octree_ptr_->end_leafs(); it++) 147 | { 148 | if(octree_ptr_->isNodeOccupied(*it)) 149 | continue; 150 | 151 | if(isNearVoid(it.getCoordinate(), depth, res)) 152 | pl.push_back(it.getCoordinate()); 153 | } 154 | if(!pl.size()) 155 | { 156 | ROS_ERROR("Found no frontier points at depth %d!", depth); 157 | return; 158 | } 159 | 160 | pcl::PointCloud border_pcl; 161 | border_pcl.resize(pl.size()); 162 | border_pcl.header.frame_id = frame_id_; 163 | size_t i = 0; 164 | for(octomap::point3d_list::iterator it = pl.begin(); it != pl.end(); ++it) 165 | { 166 | border_pcl[i].x = it->x(); 167 | border_pcl[i].y = it->y(); 168 | border_pcl[i].z = it->z(); 169 | i++; 170 | } 171 | 172 | sensor_msgs::PointCloud2 void_frontier_msg; 173 | pcl::toROSMsg(border_pcl, void_frontier_msg); 174 | void_frontier_pub_.publish(void_frontier_msg); 175 | 176 | // estimate normals: 177 | pcl::PointCloud border_normals; 178 | pcl::NormalEstimation norm_estim; 179 | pcl::search::KdTree::Ptr tree = boost::make_shared > (); 180 | tree->setInputCloud(boost::make_shared > (border_pcl)); 181 | norm_estim.setSearchMethod(tree); 182 | norm_estim.setInputCloud(boost::make_shared > (border_pcl)); 183 | norm_estim.setRadiusSearch(normal_search_radius_); 184 | norm_estim.compute(border_normals); 185 | 186 | // filter NaNs: 187 | pcl::PointIndices nan_indices; 188 | for (unsigned int i = 0; i < border_normals.points.size(); i++) { 189 | if (isnan(border_normals.points[i].normal[0])) 190 | nan_indices.indices.push_back(i); 191 | } 192 | pcl::ExtractIndices extract; 193 | extract.setInputCloud(boost::make_shared >(border_pcl)); 194 | extract.setIndices(boost::make_shared(nan_indices)); 195 | extract.setNegative(true); 196 | extract.filter(border_pcl); 197 | pcl::ExtractIndices nextract; 198 | nextract.setInputCloud(boost::make_shared >(border_normals)); 199 | nextract.setIndices(boost::make_shared(nan_indices)); 200 | nextract.setNegative(true); 201 | nextract.filter(border_normals); 202 | 203 | // tree object used for search 204 | pcl::KdTreeFLANN::Ptr tree2 = boost::make_shared >(); 205 | tree2->setInputCloud(boost::make_shared >(border_pcl)); 206 | // Decompose a region of space into clusters based on the euclidean distance between points, and the normal 207 | std::vector clusters; 208 | pcl::extractEuclideanClusters(border_pcl, border_normals, tolerance_, tree2, clusters, eps_angle_, min_pts_per_cluster_); 209 | 210 | if(clusters.size() > 0) 211 | { 212 | std::sort(clusters.begin(), clusters.end(), compareClusters); 213 | std::vector > cluster_clouds; 214 | cluster_clouds.reserve(num_clusters_); 215 | 216 | geometry_msgs::PoseArray nbv_pose_array; 217 | 218 | for(unsigned int nc = 0; nc < clusters.size(); nc++) { 219 | if(nc == num_clusters_) 220 | break; 221 | 222 | // extract a cluster: 223 | pcl::PointCloud cluster_pcl; 224 | cluster_pcl.header = border_pcl.header; 225 | extract.setInputCloud(boost::make_shared >(border_pcl)); 226 | extract.setIndices(boost::make_shared(clusters.back())); 227 | extract.setNegative(false); 228 | extract.filter(cluster_pcl); 229 | // extract normals of cluster: 230 | pcl::PointCloud cluster_normals; 231 | cluster_normals.header = border_pcl.header; 232 | nextract.setInputCloud(boost::make_shared >(border_normals)); 233 | nextract.setIndices(boost::make_shared(clusters.back())); 234 | nextract.setNegative(false); 235 | nextract.filter(cluster_normals); 236 | // find boundary points of cluster: 237 | pcl::search::KdTree::Ptr tree3 = boost::make_shared >(); 238 | tree3->setInputCloud(boost::make_shared >(cluster_pcl)); 239 | pcl::PointCloud boundary_pcl; 240 | pcl::BoundaryEstimation be; 241 | be.setSearchMethod(tree3); 242 | be.setInputCloud(boost::make_shared >(cluster_pcl)); 243 | be.setInputNormals(boost::make_shared >(cluster_normals)); 244 | be.setRadiusSearch(0.5); 245 | be.setAngleThreshold(boundary_angle_threshold_); 246 | be.compute(boundary_pcl); 247 | 248 | geometry_msgs::Pose nbv_pose; 249 | for (unsigned int i = 0; i < boundary_pcl.points.size(); ++i) { 250 | if (boundary_pcl.points[i].boundary_point) { 251 | nbv_pose.position.x = cluster_pcl.points[i].x; 252 | nbv_pose.position.y = cluster_pcl.points[i].y; 253 | nbv_pose.position.z = cluster_pcl.points[i].z; 254 | tf::Vector3 axis(0, -cluster_normals.points[i].normal[2], 255 | cluster_normals.points[i].normal[1]); 256 | tf::Quaternion quat(axis, axis.length()); 257 | geometry_msgs::Quaternion quat_msg; 258 | tf::quaternionTFToMsg(quat, quat_msg); 259 | nbv_pose.orientation = quat_msg; 260 | nbv_pose_array.poses.push_back(nbv_pose); 261 | } 262 | } 263 | 264 | cluster_clouds.push_back(cluster_pcl); 265 | 266 | // pop the just used cluster from indices: 267 | clusters.pop_back(); 268 | } 269 | 270 | // visualize pose array: 271 | nbv_pose_array.header.frame_id = border_pcl.header.frame_id; 272 | nbv_pose_array.header.stamp = ros::Time::now(); 273 | posearray_pub_.publish(nbv_pose_array); 274 | 275 | // visualize cluster pcls: 276 | for(int i = 0; i < num_clusters_; i++) 277 | { 278 | sensor_msgs::PointCloud2 cluster_pcl_msg; 279 | pcl::toROSMsg(cluster_clouds[i], cluster_pcl_msg); 280 | cluster_pub_[i].publish(cluster_pcl_msg); 281 | } 282 | } 283 | } 284 | 285 | 286 | /** 287 | * Octomap callback. 288 | * 289 | * It will skip if trying to compute poses more frequently than min_goal_interval. 290 | */ 291 | void NextBestView::onOctomap(const octomap_msgs::Octomap::ConstPtr& map) 292 | { 293 | if((last_computation_time_ + ros::Duration(min_computation_interval_, 0)) > ros::Time::now()) 294 | return; 295 | 296 | if(octree_ptr_) delete octree_ptr_; 297 | octree_ptr_ = octomap_msgs::binaryMsgToMap(*map); 298 | 299 | last_computation_time_ = ros::Time::now(); 300 | computeNextBestViews(); 301 | } 302 | 303 | 304 | int main(int argc, char **argv) 305 | { 306 | ros::init(argc, argv, "next_best_view_node"); 307 | 308 | NextBestView nbv; 309 | ros::spin(); 310 | 311 | return 0; 312 | } 313 | --------------------------------------------------------------------------------