Ask Your Question
0

Checking if a custom global planner is loaded

asked 2017-07-10 06:33:16 -0500

msz1621 gravatar image

updated 2017-07-18 00:27:37 -0500

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
edit retag flag offensive close merge delete

2 Answers

Sort by » oldest newest most voted
0

answered 2017-07-23 11:12:21 -0500

msz1621 gravatar image

I found that the issue is with the name of my custom planner "global_planner", which happened to be similar to another package already installed (global_planner). It seems that move_base was loading the latter one. I renamed my custom global planner and everything went just fine.

edit flag offensive delete link more
1

answered 2017-07-17 02:30:16 -0500

Procópio gravatar image

Using this command:

rosparam get move_base/base_global_planner
edit flag offensive delete link more

Comments

Thanks it gives me my custom global planner. However, why ROS_INFO does not print anything?

msz1621 gravatar imagemsz1621 ( 2017-07-18 00:26:07 -0500 )edit

Maybe you should check logger level for your plugin.

Akif gravatar imageAkif ( 2017-07-18 02:31:31 -0500 )edit

@Akif Thank you. I changed the logging level through rqt, but still not showing anything

msz1621 gravatar imagemsz1621 ( 2017-07-23 03:03:33 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-07-10 06:33:16 -0500

Seen: 310 times

Last updated: Jul 23 '17