Ask Your Question
0

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

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

curi_ROS gravatar image

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 reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by curi_ROS
close date 2019-05-15 04:07:20.384692

2 Answers

Sort by ยป oldest newest most voted
0

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

curi_ROS gravatar image

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

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 more
1

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

harshal gravatar image

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

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 more

Comments

Thanks, that was one of the reasons!

curi_ROS gravatar imagecuri_ROS ( 2019-01-28 07:38:27 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2019-01-24 08:18:41 -0600

Seen: 286 times

Last updated: Jan 30