Send MoveBaseActionGoal based on waypoints using MoveBase API

asked 2018-03-06 03:11:30 -0500

GuillaumeHauss gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Have you find the reason or any solution? Would be appreciate to get your response on this issue. Thanks in advance.

songshan gravatar imagesongshan ( 2019-02-20 01:33:06 -0500 )edit