Send MoveBaseActionGoal based on waypoints using MoveBase API

edit

Hi all, I'm working on autonomous driving and ROS. A high level server will send me waypoints (an array of Pose between the starting pose of the car and the desired destination), meaning that I will need to only deal with path planning between two consecutive waypoints. I wanted to used the Action API of move base to do so, but I can't make it work. I want to use MoveBaseActionGoal, rather than MoveBaseGoal, to have the ID information, which is very relevant to my case.

Here is my code

#include <stdio.h>  
#include <stdlib.h>
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <geometry_msgs/PoseArray.h>

//Client definition
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

//Action Feedback and Result
typedef boost::shared_ptr< ::move_base_msgs::MoveBaseActionFeedback const > MoveBaseActionFeedbackConstPtr;
typedef boost::shared_ptr< ::move_base_msgs::MoveBaseActionResult const > MoveBaseActionResultConstPtr;

//Node Feedback and Result
typedef boost::shared_ptr< ::move_base_msgs::MoveBaseResult const > MoveBaseResultConstPtr;
typedef boost::shared_ptr< ::move_base_msgs::MoveBaseFeedback const > MoveBaseFeedbackConstPtr;

class GoalSender
{
    private:
        //Subscriber
        ros::Subscriber goals_sub_;

        //Params
        std::string goals_topic_; 
        std::string node_name;

        //Client
        MoveBaseClient MB_client;

        //Goal sent to Move Base
        move_base_msgs::MoveBaseActionGoal action_goal;
        move_base_msgs::MoveBaseGoal goal;

        //Goals Array received from AVCH
        geometry_msgs::PoseArray waypoints; 

        //Number of current goal in the waypoints array
        int index; 

        //Waypoints array size
        int max_index;

    public:
        //ROS Handle
        ros::NodeHandle nh;

        GoalSender():
        MB_client("move_base", true)
        {
            //Get Node name
            node_name = ros::this_node::getName();

            //Wait for server to be online
            while(!MB_client.waitForServer(ros::Duration(5.0))){
                ROS_INFO("%s: Waiting for the move_base action server to come up", node_name.c_str());
            }
            ROS_INFO("%s: Action server started", node_name.c_str());

            //Start private ros node handle to retrieve parameter
            ros::NodeHandle nh_p("~");

            if(nh_p.getParam("goals_topic", goals_topic_)){
                ROS_INFO("%s: Fetching goals array on %s", node_name.c_str(), goals_topic_.c_str() );
            }
            else{
                ROS_WARN("%s: No goals topic given. Defaulting to /goals_poses", node_name.c_str());
                goals_topic_ = "/goals_poses";
            }

            goals_sub_ = nh.subscribe(goals_topic_, 5, &GoalSender::goals_registering, this);
            index = 0;
            max_index = 0;
        }

        bool checkWaypoints(geometry_msgs::PoseArray waypoints){
            /***
             * TO DO 
            ***/
            return true;
        }

        //Callback function to goals array
        void goals_registering(const geometry_msgs::PoseArray::ConstPtr& msg){
            if(std::abs((msg->header.stamp - ros::Time::now()).toSec())<0.1){ //allowing 100ms old waypoints
                waypoints.header = msg->header; 
                waypoints.poses = msg->poses;

                if(!checkWaypoints(*msg)){
                    ROS_WARN("%s: Received waypoints were corrupted or undoable. No goal will be sent", node_name.c_str());
                }
                else{
                    ROS_INFO("%s: Received waypoints were correct. Proceeding to goal 1", node_name.c_str());
                    index = 0; 
                    max_index = waypoints.poses.size()-1;
                    sendGoald(waypoints.poses[0], index);

                }
            }    
        }

        void sendGoald(geometry_msgs::Pose goal_pose, int index){
            ROS_DEBUG("%s: A new goal pose will be sent over the path planning node", node_name.c_str());
            ros::Time now = ros::Time::now();

            /* Fill up action goal info */
            //ID (stamp and id)
            action_goal.goal_id.stamp = now; 
            action_goal.goal_id.id = std::to_string(index);

            //Goal (geometry_msgs/PoseStamped)
            action_goal.goal.target_pose.header.frame_id = "base_link";
            action_goal.goal.target_pose.header.stamp = now;
            action_goal.goal.target_pose.pose.position.x = goal_pose.position.x;
            //action_goal.goal.target_pose.pose.position.y = goal_pose.position.y;
            action_goal.goal.target_pose.pose.orientation.w = goal_pose.orientation.w;

            //Header
            action_goal.header.stamp = now; 
            action_goal.header.frame_id = "base_link ...