nj_costmap_node.cpp
Go to the documentation of this file.
00001 /*
00002  * Node using a Navigating jockey from local map
00003  *
00004  * Parameters:
00005  * navigating_jockey_server_name, String, node_name + "_server"
00006  * odom_frame, String, "odom", frame of the laser sensor
00007  * frontier_width, String, no default, how wide must an exit be
00008  *
00009 */
00010 
00011 #include <ros/ros.h>
00012 #include <ros/console.h> // to change the log level to debug
00013 
00014 #include <nj_costmap/jockey.h>
00015 
00016 // TODO: set these as parameters in the jockey
00017 // const double maxtheta = 1.6;
00018 // const double max_distance = 3.0;
00019 // const double reach_distance = 0.05;
00020 
00021 int main(int argc, char **argv)
00022 {
00023   // Debug log level
00024   if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info))
00025   {
00026     ros::console::notifyLoggerLevelsChanged();
00027   }
00028 
00029   ros::init(argc, argv, "default_name_nj_costmap");
00030   ros::NodeHandle nh; // Log in jockey works better with this line.
00031   ros::NodeHandle private_nh("~");
00032 
00033   /* Minimal frontier width */
00034   if (!private_nh.hasParam("frontier_width"))
00035   {
00036     ROS_ERROR("Parameter %s/frontier_width not set, exiting", private_nh.getNamespace().c_str());
00037     return 1;
00038   }
00039   double frontier_width;
00040   private_nh.param<double>("frontier_width", frontier_width, 0.0);
00041 
00042   /* Navigating jockey name */
00043   std::string navigating_jockey_name;
00044   private_nh.param<std::string>("navigating_jockey_server_name", navigating_jockey_name,
00045       ros::this_node::getName() + "_server");
00046 
00047   nj_costmap::Jockey jockey(navigating_jockey_name, frontier_width);
00048 
00049   ROS_INFO("%s started (with server %s)", ros::this_node::getName().c_str(), jockey.getName().c_str());
00050   ros::spin();
00051 
00052   return 0;
00053 }
00054 


nj_costmap
Author(s): Gaƫl Ecorchard
autogenerated on Sat Jun 8 2019 20:58:46