How to use move_base Action API with a navigation goals subscriber

edit

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 ...