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

asked 2019-01-24 08:18:41 -0500

curi_ROS
166 ●13 ●15 ●22

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.

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2019-01-30 07:33:45 -0500

curi_ROS
166 ●13 ●15 ●22

updated 2019-01-30 07:35:42 -0500

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();
}

edit flag offensive delete link

add a comment

answered 2019-01-24 09:39:39 -0500

harshal
35 ●7 ●12 ●14

updated 2019-01-24 09:42:05 -0500

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.

edit flag offensive delete link

Comments

Thanks, that was one of the reasons!

curi_ROS ( 2019-01-28 07:38:27 -0500 )edit

add a comment

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

2 Answers

Comments

Question Tools

Stats

Related questions

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

2 Answers

Comments

Question Tools

Stats

Related questions

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