Writing a custom path using 'Move Base Flex Exe Path Action'

Question

I would like to send a series of poses to Move Base, and have my robot follow these poses. I am using Move Base Flex, which extends a few extra actions compared to the regular Move Base. I am currently trying to understand how to use ExePathAction.

From what I understand, it allows the controller (i.e. the base local planner) to follow a set of poses. I am trying to give it 5 poses each 0.1 cm apart. What I've written so far:

#include "ros/ros.h"
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <mbf_msgs/ExePathAction.h>
#include <mbf_msgs/GetPathAction.h>
#include <mbf_msgs/MoveBaseAction.h>
#include <nav_msgs/Path.h>

typedef actionlib::SimpleActionClient<mbf_msgs::ExePathAction> PathClient;

int main(int argc, char** argv){
    ros::init(argc, argv, "simple_path");

    PathClient pc("mbf_costmap_nav/exe_path", true); // true doesnt need ros::spin

    while(!pc.waitForServer(ros::Duration(5.0))){
        ROS_INFO("Waiting for Move Base server to come up");
    }

    mbf_msgs::ExePathGoal target_path_;

    //move_base_msgs::MoveBaseGoal goal;

    nav_msgs::Path path_;

    path_.header.frame_id = "base_link";
    path_.poses[0].pose.position.x = 0.1;
    path_.poses[0].pose.position.y = 0.0;
    path_.poses[0].pose.orientation.z = 0.0;
    path_.poses[0].pose.orientation.w = 1.0;
    
    path_.header.frame_id = "base_link";
    path_.poses[1].pose.position.x = 0.2;
    path_.poses[1].pose.position.y = 0.0;
    path_.poses[1].pose.orientation.z = 0.0;
    path_.poses[1].pose.orientation.w = 1.0;
    
    path_.header.frame_id = "base_link";
    path_.poses[2].pose.position.x = 0.3;
    path_.poses[2].pose.position.y = 0.0;
    path_.poses[2].pose.orientation.z = 0.0;
    path_.poses[2].pose.orientation.w = 1.0;
    
    path_.header.frame_id = "base_link";
    path_.poses[3].pose.position.x = 0.4;
    path_.poses[3].pose.position.y = 0.0;
    path_.poses[3].pose.orientation.z = 0.0;
    path_.poses[3].pose.orientation.w = 1.0;
    
    path_.header.frame_id = "base_link";
    path_.poses[4].pose.position.x = 0.5;
    path_.poses[4].pose.position.y = 0.0;
    path_.poses[4].pose.orientation.z = 0.0;
    path_.poses[4].pose.orientation.w = 1.0;
    target_path_.path = path_;

pc.sendGoal(target_path_);

pc.waitForResult();

if(pc.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
  ROS_INFO("Base moved %s", pc.getState().toString().c_str());

else if(pc.getState() == actionlib::SimpleClientGoalState::ABORTED)  
  ROS_INFO("Goal aborted");
  
else 
  ROS_INFO("Base failed to move for some reason");

return 0;
}

However this doesn't execute. I looked at the legacy relay node which relays classic move_base goals that appear on the move_base_simple/goal topic. However, that's not what I am looking for. I would like to directly use the ExePathAction, as I want to program more sophisticated navigation behaviour in the future.

---

Originally posted by curi_ROS on ROS Answers with karma: 166 on 2019-01-24

Post score: 0

Answer 1

Hi,

You need to specify a controller for move_base_flex to move under the controller field in the ExePathGoal message(refer to message definition linked by you under ExePathAction). This is the 'local planner' that you load in the list of controllers for move_base_flex.

P.S.: Add in the time stamp for the 'path_' header.

---

Originally posted by harshal with karma: 35 on 2019-01-24

This answer was NOT ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by curi_ROS on 2019-01-28:
Thanks, that was one of the reasons!

Answer 2

For future users, here is the code that works for me:

#include "ros/ros.h"
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <mbf_msgs/ExePathAction.h>
#include <mbf_msgs/GetPathAction.h>
#include <mbf_msgs/MoveBaseAction.h>
#include <nav_msgs/Path.h>
#include <vector>
#include <geometry_msgs/PoseStamped.h>

typedef actionlib::SimpleActionClient<mbf_msgs::ExePathAction> PathClient; // A client of Exe Path Action Server

int main(int argc, char** argv){
    ros::init(argc, argv, "odom_path");

    PathClient pc("mbf_costmap_nav/exe_path", true); // true doesnt need ros::spin

     while(!pc.waitForServer(ros::Duration(5.0))){
         ROS_INFO("Waiting for Move Base server to come up");
     }

    mbf_msgs::ExePathGoal target_path_;

    nav_msgs::Path path_;

    int no_of_poses_in_path = 4;       // Number of poses to compose the path
    
    std::vector<geometry_msgs::PoseStamped> poses_(no_of_poses_in_path);
     
    // Fill the pose vector with zeroes 
     for (int i=0; i<no_of_poses_in_path; i++){
        memset(&poses_[i].pose.position, 0, 3);
        memset(&poses_[i].pose.orientation, 0, 4);
       
    }

    // Insert your series of poses here using your logic 
    // Here I am using 4 poses 10 cm apart
     for (int i=0; i<no_of_poses_in_path; i++){
        poses_[i].header.frame_id = "odom";
        poses_[i].pose.position.x = (0.1*i);
        poses_[i].pose.position.y = 0;
        poses_[i].pose.orientation.z = 0; 
        poses_[i].pose.orientation.w = 1;    
        poses_[i].header.stamp = ros::Time::now();
 
     }

   // Populate the Path Message
    path_.header.frame_id = "odom";
    path_.poses = poses_;
    path_.header.stamp = ros::Time::now();
   
    ROS_INFO("Goal Path Planned %f %f %f %f", path_.poses[0].pose.position.x, path_.poses[1].pose.position.x, 
                                         path_.poses[2].pose.position.x, path_.poses[3].pose.position.x);
    
   // Populate the controller and path fields (Refer to ExePath.Action)
 
    target_path_.controller = "TebLocalPlannerROS"; // As defined in list of controllers in your yaml

    target_path_.path = path_;

  // Interact with Action Server

  pc.sendGoal(target_path_);

  pc.waitForResult();
  if(pc.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Base moved %s", pc.getState().toString().c_str());

  else if(pc.getState() == actionlib::SimpleClientGoalState::ABORTED)  
    ROS_INFO("Goal aborted");

  else 
    ROS_INFO("Base failed to move for some reason");

  
  ros::shutdown();
}

---

Originally posted by curi_ROS with karma: 166 on 2019-01-30

This answer was ACCEPTED on the original site

Post score: 1