sending multiple goals via global planner plugin [closed]

edit

Hi all,

I have a mobile robot and I would like it to navigate around the building and I already have a map of the building. I am using wheel encoders to generate odometry message, robot_pose_ekf to fuse data from IMU and wheel_encoders, amcl for localization and move_base for planning. I am using hokuyo laser for navigation. Now I have started writing a global planner plugin and I have to send multiple goals in this global planner. For testing, I just added three points into the plan. When I run it, the robot starts from the start_position and heads towards the second goal but before it reaches the second goal, the path towards the third goal is generated and it starts moving towards it and after that it starts behaving in a weird manner.
The link to the video: https://www.youtube.com/watch?v=Y0CiN...

The makePlan function of the global planner plugin:

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

        plan.push_back(start);
        ROS_INFO("makePlan called!!!!!!!!!!!!!!!!!!");

        geometry_msgs::PoseStamped inter;
        tf::Quaternion inter_quat = tf::createQuaternionFromYaw(1.57);
        inter.pose.position.x = 4.5;
        inter.pose.position.y = 3.0;
        inter.pose.orientation.x = inter_quat.x();
        inter.pose.orientation.y = inter_quat.y();
        inter.pose.orientation.z = inter_quat.z();
        inter.pose.orientation.w = inter_quat.w();

        plan.push_back(inter);
        plan.push_back(goal)

        return true;

    }

I feel that the problem is because the makePlan function is called again and again by move_base which leads to this weird behavior. Why is makePlan function called again and again from move_base although there is no need for it, for example if there are just two points in the plan(start and goal) and there is nothing between them, move_base still calls make_plan of the global planner plugin again and again : https://www.youtube.com/watch?v=7k6yz... ? Also, I would like to know how can I make sure that the robot reaches the current goal and then only moves towards the next goal in the std::vector<geometry_msgs::PoseStamped>& plan? I have used Action API to send multiple goals and it works well with it but I have to write my own global planner so that I can add more functionality later. Any help will be appreciated. Please let me know if you need more information from me.

Update:
I have not set planner_frequency, so its the default which is 0. The issue is there only when I use my own global planner plugin or carrot_planner plugin. When I use SBPL Lattice Planner, then it works as expected and move_base does not call makePlan function of the global planner again and again when its not needed. So, I feel it has something to do with the way the global planner plugin is written and not with move_base but I am not able to figure it out. Does anyone have any clue why is this happening?

Thanks in advance.
Naman Kumar