Ask Your Question
1

How to use a subscriber in a global planner plugin?

asked 2021-03-19 00:15:43 -0500

hanks gravatar image

I am trying to write a custom global planner using this tutorial. I am not computing my path in this global planner, all I want to do is make this global planner pass on a path that is computed by my other package to the move_base/local planner. How do I define a subscriber and its callback function in the plugin so that it takes in the published path? Trying to execute option 2 from this answer.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
3

answered 2021-03-19 10:07:20 -0500

Roberto Z. gravatar image

What you need is to implement a global planner plugin that has service client built-in. Then, in a different node you implement a service server which is responsible for calculating the path each time it gets a service request.

ROS Services are explained Here

This is how to add a service client to the code shown by the Writing A Global Path Planner As Plugin in ROS tutorial:

Class Header

 //  SOME OTHER INCLUDE STATEMENTS
 #include "***YOUR MESSAGE TYPE HERE***"

 namespace global_planner {

 class GlobalPlanner : public nav_core::BaseGlobalPlanner {
      public:
        //  SOME CODE TO DECLARE PUBLIC MEMBERS

      private :
        //  SOME CODE TO DECLARE PRIVATE MEMBERS

        // service client declaration
        ros::ServiceClient service_;
  };
};

Class Implementation

 void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
   //  OTHER INITIALIZATION CODE

   // create a client for the path planning service
  service_ = private_nh.serviceClient<***YOUR MESSAGE TYPE HERE***>("plan");

  // wait for the service to be advertised and available
  service_.waitForExistence();
 }

then send a service request from inside of the body of the function GlobalPlanner::makePlan(), like so:

   bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,  std::vector<geometry_msgs::PoseStamped>& plan ){

  // initialize a message of your custom message type
  **YOUR MESSAGE TYPE** message_name;

   // CODE TO FILL IN YOUR CUSTOM MESSAGE

  // call the path planning service
  service_.call(message_name);

 // you must also process the service response (not shown here)
}

Don't forget to process the service response and store it to the variable name plan which is one of the parameters of the function GlobalPlanner::makePlan().

You will also have to modify your CMakeLists.txt file to include your custom message type. All about how to create a custom service message is explained here: Creating a ROS msg and srv tutorial

Note that this is not a cut-and-paste ready-to-go example. You will have to additionally:
- define a custom message
- create a service server
- fill in the service request, and
- process the service response

Hopefully this information is enough to get you started and that it also answers your question.

edit flag offensive delete link more

Comments

I did implement this exactly as you mentioned and it partly works. The solution is received from another node where the service server is implemented, but then the makePlan() function only outputs a part of the global plan received for some reason. Here is how I implemented the service response -

bool HybridAStarPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::posestamped>& plan) {

    hybrid_astar::GlobalPath p;
    geometry_msgs::PoseStamped pose_stamped;

    sc.call(p);

    for (int i = 0; i < p.response.plan.poses.size(); ++i) {
        pose_stamped.header.stamp = ros::Time::now();
        pose_stamped.header.frame_id = "map";
        pose_stamped.pose = p.response.plan.poses[i].pose;
        plan.push_back(pose_stamped);
    }

    return true;
}

Only the beginning part of the global plan received is being displayed in rviz. The same happened when I used a subscriber instead of a service.

hanks gravatar image hanks  ( 2021-03-20 01:50:02 -0500 )edit

Your code to process the response looks ok to me. Only thing I would add is a check to verify if the plan is empty or not.

// process plan response
  if (p.response.plan.size())
  {

  //  YOUR CODE TO PROCESS THE SERVICE RESPONSE

   return true;
  }
  else
  {
    // empty response, no plan found
    return false;
  }

Maybe the problem lies on the side of the server? Can you confirm that the number of waypoints received by the client is equal to the number of waypoints send by the server? For instance adding a debug statement on both sides?

ROS_DEBUG("Number of points: %d", unsigned( p.response.plan.size()));
Roberto Z. gravatar image Roberto Z.  ( 2021-03-22 03:58:39 -0500 )edit

The problem was with the path being passed, I've solved it now. Thanks!

hanks gravatar image hanks  ( 2021-03-22 07:29:52 -0500 )edit

I'm glad you got it working!

Roberto Z. gravatar image Roberto Z.  ( 2021-03-22 12:53:03 -0500 )edit
0

answered 2021-03-19 01:43:46 -0500

Global planner provides an interface class , I think it was called BaseGlobalPlanner, so the recommended way is that you inherit from this base class and override virtual function makePlan. You should do actual path computing within that function. Finally export this inherited class as a GlobalPlanner plugin. I would not recommend creating any subscribers for this task as in that case you will by pass the whole good practices that are already there for you, I would say just spend a little more time to get more comfortable on how to create a plugin.

edit flag offensive delete link more

Comments

I already have a working package that computes the path, don't want to convert it to a plugin class right now. I am able to use a subscriber and get the path being computed, but for some reason, the global planner isn't passing it down to the local planner or publishing it at all. Here are my plugin files, please have a look - https://drive.google.com/drive/folder... Let me know if you find the mistake.

hanks gravatar image hanks  ( 2021-03-19 09:15:28 -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

2 followers

Stats

Asked: 2021-03-19 00:15:43 -0500

Seen: 392 times

Last updated: Mar 19 '21