exploration_node.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 
00030 #include <ros/ros.h>
00031 #include <hector_exploration_planner/hector_exploration_planner.h>
00032 #include <costmap_2d/costmap_2d_ros.h>
00033 #include <hector_nav_msgs/GetRobotTrajectory.h>
00034 
00035 class SimpleExplorationPlanner
00036 {
00037 public:
00038   SimpleExplorationPlanner()
00039   {
00040     ros::NodeHandle nh;
00041 
00042     costmap_2d_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tfl_);
00043 
00044     planner_ = new hector_exploration_planner::HectorExplorationPlanner();
00045     planner_->initialize("hector_exploration_planner",costmap_2d_ros_);
00046 
00047     exploration_plan_service_server_ = nh.advertiseService("get_exploration_path", &SimpleExplorationPlanner::explorationServiceCallback, this);
00048 
00049     exploration_plan_pub_ = nh.advertise<nav_msgs::Path>("exploration_path",2);
00050   }
00051 
00052   bool explorationServiceCallback(hector_nav_msgs::GetRobotTrajectory::Request  &req,
00053                                   hector_nav_msgs::GetRobotTrajectory::Response &res )
00054     {
00055       ROS_INFO("Exploration Service called");
00056 
00057       tf::Stamped<tf::Pose> robot_pose_tf;
00058       costmap_2d_ros_->getRobotPose(robot_pose_tf);
00059 
00060       geometry_msgs::PoseStamped pose;
00061       tf::poseStampedTFToMsg(robot_pose_tf, pose);
00062       planner_->doExploration(pose, res.trajectory.poses);
00063       res.trajectory.header.frame_id = "map";
00064       res.trajectory.header.stamp = ros::Time::now();
00065 
00066       if (exploration_plan_pub_.getNumSubscribers() > 0)
00067       {
00068         exploration_plan_pub_.publish(res.trajectory);
00069       }
00070 
00071       return true;
00072     }
00073 
00074 protected:
00075   hector_exploration_planner::HectorExplorationPlanner* planner_;
00076   ros::ServiceServer exploration_plan_service_server_;
00077   ros::Publisher exploration_plan_pub_;
00078   costmap_2d::Costmap2DROS* costmap_2d_ros_;
00079   tf::TransformListener tfl_;
00080 
00081 };
00082 
00083 int main(int argc, char **argv) {
00084   ros::init(argc, argv, ROS_PACKAGE_NAME);
00085 
00086   SimpleExplorationPlanner ep;
00087 
00088   ros::spin();
00089 
00090   return 0;
00091 }


hector_exploration_node
Author(s):
autogenerated on Wed Sep 6 2017 02:41:35