Ask Your Question
0

How to use move_base Action API with a navigation goals subscriber

asked 2021-04-27 05:40:34 -0600

AlexandrosNic gravatar image

I try to get the navigation goal points from a subscriber that identifies them through lidar. In particular I use the lidar to get the corners of a wall and my aim is to navigate close to the corner.

My problem is that I can't get a subscriber and move_base Action API to work simultaneously. I followed the tutorial of SendingSimpleGoals, and turned it into the Action API while commenting out the waitForResult since it was conflicting with subscriber's spin.

The goal is passed to move_base, and I see through rviz that a path is generated, but the robot never starts moving. The only way to start moving is to start the move_base launch file, rosrun this node and then cancel the node (while move_base launch file keeps running).

Can anyone identify where is the problem here? Thank you in advance

I use ROS melodic with Gazebo simulation

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>


typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
geometry_msgs::Point starting_point_;
ros::Publisher goal_pub;
MoveBaseClient* ac_ptr;

// Called once when the goal completes
void doneCb(const actionlib::SimpleClientGoalState& state,
            const move_base_msgs::MoveBaseResultConstPtr& result)
{
  ROS_INFO("Finished in state [%s]", state.toString().c_str());
  // ROS_INFO("Answer: %i", result->sequence.back());
  ros::shutdown();
}

// Called once when the goal becomes active
void activeCb()
{
  #ifdef DEBUG
    ROS_INFO("Goal just went active");
  #endif
}

// Called every time feedback is received for the goal
void feedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback)
{
  #ifdef DEBUG
    ROS_INFO("Got Feedback");
  #endif
}

void starting_point (const visualization_msgs::Marker &corner)
{
  starting_point_.x = corner.points[0].x-1.0;
  starting_point_.y = corner.points[0].y-1.0;
  starting_point_.z = 0.0;
  goal_pub.publish(starting_point_);

  ROS_INFO("starting_point_x and starting_point_y are: %f, %f", starting_point_.x, starting_point_.y);
  ROS_INFO("Move_base subscriber initialized");


  // Build the destination message (geometry_msgs::PoseStamped)
  move_base_msgs::MoveBaseGoal goal;
  move_base_msgs::MoveBaseActionGoal action_goal;


  action_goal.goal_id.stamp = ros::Time::now(); 
  action_goal.goal_id.id = "map";

  //Goal (geometry_msgs/PoseStamped)
  action_goal.goal.target_pose.header.frame_id = "map";
  action_goal.goal.target_pose.header.stamp = ros::Time::now();
  action_goal.goal.target_pose.pose.position.x = starting_point_.x;
  action_goal.goal.target_pose.pose.position.y = starting_point_.y;
  action_goal.goal.target_pose.pose.position.z = starting_point_.z;
  action_goal.goal.target_pose.pose.orientation.x = 0.0;
  action_goal.goal.target_pose.pose.orientation.y = 0.0;
  action_goal.goal.target_pose.pose.orientation.z = 0.0;
  action_goal.goal.target_pose.pose.orientation.w = 1.0;

   //Header
  action_goal.header.stamp = ros::Time::now(); 
  action_goal.header.frame_id = "map";
  goal = action_goal.goal;
  ac_ptr->sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
}

int main(int argc, char** argv){

  // Initialize ROS
  ros::init(argc, argv, "simple_navigation_goals");

  //tell the action client that we want to spin a thread by default
  MoveBaseClient ac("move_base", true);

  ac_ptr = &ac;

  //wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  ros::NodeHandle nh;

  // Create a ROS subscriber for the input line segments
  ros::Subscriber sub = nh.subscribe("/corners", 10, starting_point);

  // Create a ROS publisher for the input point cloud
  goal_pub = nh.advertise<geometry_msgs::Point>("goal_points ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-04-27 10:09:25 -0600

miura gravatar image

It seems that when you call spin, the challenge is to keep spin going. Using spinOnce may solve this.

int main(int argc, char** argv){

  // Initialize ROS
  ros::init(argc, argv, "simple_navigation_goals");

  ros::NodeHandle nh;

  // Create a ROS subscriber for the input line segments
  ros::Subscriber sub = nh.subscribe("/corners", 10, starting_point);

  // Create a ROS publisher for the input point cloud
  goal_pub = nh.advertise<geometry_msgs::Point>("goal_points", 10);

  //tell the action client that we want to spin a thread by default
  MoveBaseClient ac("move_base", true);

  ac_ptr = &ac;

  //wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
    ros::spinOnce();
  }

  return 0;
}

Prepare the subscriber before the loop and use spinOnce in the loop. This will allow you to subscribe during the loop.

edit flag offensive delete link more

Comments

Indeed this solved my problem. I think I have to understand spin a bit more. Thank you very much for your answer

AlexandrosNic gravatar image AlexandrosNic  ( 2021-04-28 03:19:02 -0600 )edit

I'm glad I could help you.

miura gravatar image miura  ( 2021-04-28 12:18:16 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-04-27 05:40:34 -0600

Seen: 58 times

Last updated: Apr 27