Checking if a custom global planner is loaded
I used this this tutorial in order to create my own global planner. However, when I added some ROS_INFO in order to show me if it is loaded or not, I receive no output. So, my question is how can I know if move_base is actually using my custom global planning? And why ROS_INFO in the initialization function does not print?
global_planner.cpp:
#include <pluginlib/class_list_macros.h>
#include "global_planner.h"
//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner)
using namespace std;
//Default Constructor
namespace global_planner {
GlobalPlanner::GlobalPlanner (){
}
GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
initialize(name, costmap_ros);
}
void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
ROS_INFO("Global planner has been initialized");
ros::NodeHandle private_nh("~/" + name);
initialized_ = true;
}
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan ){
if(!initialized_){
ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
return false;
}
plan.push_back(start);
for (int i=0; i<20; i++){
geometry_msgs::PoseStamped new_goal = goal;
tf::Quaternion goal_quat = tf::createQuaternionFromYaw(1.54);
new_goal.pose.position.x = -2.5+(0.05*i);
new_goal.pose.position.y = -3.5+(0.05*i);
new_goal.pose.orientation.x = goal_quat.x();
new_goal.pose.orientation.y = goal_quat.y();
new_goal.pose.orientation.z = goal_quat.z();
new_goal.pose.orientation.w = goal_quat.w();
plan.push_back(new_goal);
}
plan.push_back(goal);
return true;
}
};
test_move_base.launch:
<launch>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_map"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /map 40" />
<!--<arg name="base_global_planner" default="navfn/NavfnROS"/>-->
<arg name="base_global_planner" default="global_planner/GlobalPlanner"/>
<!--<arg name="base_local_planner" default="base_local_planner/TrajectoryPlannerROS"/>-->
<arg name="base_local_planner" default="simple_local_planner/SimpleLocalPlanner"/>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="base_global_planner" value="$(arg base_global_planner)"/>
<param name="base_local_planner" value="$(arg base_local_planner)"/>
<param name="local_costmap/width" value="10.0"/>
<param name="local_costmap/height" value="10.0"/>
<param name="local_costmap/origin_x" value="-5.0"/>
<param name="local_costmap/origin_y" value="-5.0"/>
<param name="global_costmap/width" value="20.0"/>
<param name="global_costmap/height" value="20.0"/>
<param name="global_costmap/origin_x" value="-10.0"/>
<param name="global_costmap/origin_y" value="-10.0"/>
</node>
</launch>
The output I get when i run the launch file:
[ INFO] [1499686041.675973637]: Loading from pre-hydro parameter style
[ INFO] [1499686041.697633251]: Using plugin "obstacle_layer"
[ INFO] [1499686041.759515233]: Subscribed to Topics:
[ INFO] [1499686041.767964584]: Using plugin "inflation_layer"
[ INFO] [1499686041.836836437]: Loading from pre-hydro parameter style
[ INFO] [1499686041.843269364]: Using plugin "obstacle_layer"
[ INFO] [1499686041.874099766]: Subscribed to Topics:
[ INFO] [1499686041.882243033]: Using plugin "inflation_layer"
[ INFO] [1499686041.939767806]: Created local_planner simple_local_planner/SimpleLocalPlanner
[ INFO] [1499686042.182221474]: Recovery behavior will clear layer obstacles
[ INFO] [1499686042.218764040]: Recovery behavior will clear layer obstacles