planner_node.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2018, Locus Robotics
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #include <ros/ros.h>
00036 #include <dlux_global_planner/dlux_global_planner.h>
00037 #include <nav_core2/exceptions.h>
00038 #include <nav_2d_utils/conversions.h>
00039 #include <geometry_msgs/PoseStamped.h>
00040 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00041 #include <visualization_msgs/Marker.h>
00042 #include <string>
00043 
00044 namespace dlux_global_planner
00045 {
00060 class PlannerNode
00061 {
00062 public:
00063   PlannerNode() : costmap_loader_("nav_core2", "nav_core2::Costmap"), has_start_(false), has_goal_(false)
00064   {
00065     ros::NodeHandle nh("~");
00066     marker_pub_ = nh.advertise<visualization_msgs::Marker>("/visualization_marker", 10);
00067 
00068     tf_ = std::make_shared<tf::TransformListener>(ros::Duration(10));
00069     std::string costmap_class;
00070     nh.param("global_costmap_class", costmap_class, std::string("nav_core_adapter::CostmapAdapter"));
00071     costmap_ = costmap_loader_.createUniqueInstance(costmap_class);
00072     costmap_->initialize(nh, std::string("global_costmap"), tf_);
00073 
00074     gp_.initialize(nh, "planner", tf_, costmap_);
00075     goal_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1,
00076                                                          boost::bind(&PlannerNode::goalCB, this, _1));
00077     pose_sub_ = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1,
00078                                                          boost::bind(&PlannerNode::poseCB, this, _1));
00079     nh.param("red", red_, 1.0);
00080     nh.param("green", green_, 1.0);
00081     nh.param("blue", blue_, 1.0);
00082     nh.param("marker_ns", marker_ns_, std::string(""));
00083   }
00084 
00085   ~PlannerNode()
00086   {
00087     costmap_.reset();
00088   }
00089 private:
00090   void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal)
00091   {
00092     has_goal_ = true;
00093     goal_ = nav_2d_utils::poseStampedToPose2D(*goal);
00094     publishPointMarker(goal_, false);
00095     plan();
00096   }
00097 
00098   void poseCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& goal)
00099   {
00100     has_start_ = true;
00101     start_.header = goal->header;
00102     start_.pose.x = goal->pose.pose.position.x;
00103     start_.pose.y = goal->pose.pose.position.y;
00104     publishPointMarker(start_, true);
00105     plan();
00106   }
00107 
00108   void plan()
00109   {
00110     if (!has_goal_ || !has_start_) return;
00111     nav_2d_msgs::Path2D plan;
00112     try
00113     {
00114       plan = gp_.makePlan(start_, goal_);
00115     }
00116     catch (nav_core2::PlannerException& e)
00117     {
00118       ROS_ERROR("%s", e.what());
00119     }
00120 
00121     // publish plan as markers
00122     nav_msgs::Path path = nav_2d_utils::pathToPath(plan);
00123     double resolution = costmap_->getResolution();
00124 
00125     visualization_msgs::Marker m;
00126     m.header.stamp = ros::Time::now();
00127     m.header.frame_id = plan.header.frame_id;
00128     m.ns = marker_ns_ + "path";
00129     m.type = visualization_msgs::Marker::LINE_STRIP;
00130     m.scale.x = resolution / 4;
00131     m.color.r = red_;
00132     m.color.g = green_;
00133     m.color.b = blue_;
00134     m.color.a = 0.7;
00135     for (unsigned int i = 0; i < plan.poses.size(); i++)
00136     {
00137       m.points.push_back(path.poses[i].pose.position);
00138     }
00139     marker_pub_.publish(m);
00140 
00141     m.type = visualization_msgs::Marker::SPHERE_LIST;
00142     m.id += 1;
00143     m.color.r = red_ / 2;
00144     m.color.g = green_ / 2;
00145     m.color.b = blue_ / 2;
00146     m.scale.x = resolution / 2;
00147     m.scale.y = resolution / 2;
00148     m.scale.z = resolution / 2;
00149     marker_pub_.publish(m);
00150   }
00151 
00152   void publishPointMarker(nav_2d_msgs::Pose2DStamped pose, bool start)
00153   {
00154     visualization_msgs::Marker m;
00155     m.header = pose.header;
00156     m.header.stamp = ros::Time::now();
00157     m.ns = (start ? "start" : "goal");
00158     m.type = visualization_msgs::Marker::CYLINDER;
00159     m.pose.position.x = pose.pose.x;
00160     m.pose.position.y = pose.pose.y;
00161 
00162     double resolution = costmap_->getResolution();
00163     m.scale.x = 4 * resolution;
00164     m.scale.y = 4 * resolution;
00165     m.scale.z = 0.1;
00166     m.color.r = start ? 0.0 : 1.0;
00167     m.color.g = start ? 1.0 : 0.0;
00168     m.color.a = 0.5;
00169     marker_pub_.publish(m);
00170   }
00171 
00172   ros::Subscriber goal_sub_, pose_sub_;
00173   ros::Publisher marker_pub_;
00174 
00175   TFListenerPtr tf_;
00176   nav_core2::Costmap::Ptr costmap_;
00177   pluginlib::ClassLoader<nav_core2::Costmap> costmap_loader_;
00178   dlux_global_planner::DluxGlobalPlanner gp_;
00179 
00180   nav_2d_msgs::Pose2DStamped start_, goal_;
00181   bool has_start_, has_goal_;
00182 
00183   double red_, green_, blue_;
00184   std::string marker_ns_;
00185 };
00186 }  // namespace dlux_global_planner
00187 
00188 int main(int argc, char** argv)
00189 {
00190   ros::init(argc, argv, "global_planner");
00191   dlux_global_planner::PlannerNode pn;
00192   ros::spin();
00193   return 0;
00194 }


dlux_global_planner
Author(s):
autogenerated on Wed Jun 26 2019 20:09:53