三轮全向轮底盘SLAM挖坑系列-正方形算法

在《ROS机器人程序设计》第二版后面send_goal.cpp的基础上改过来的，这里给出正方形的代码，其他的弓字型可以在这基础上改进。不过这容错性并没有那么好，不像小米扫地机器人那么智能。

#include 
#include 
#include 
#include 
#include 
#include 
#include 

typedef actionlib::SimpleActionClient
    MoveBaseClient;

MoveBaseClient* ac;

move_base_msgs::MoveBaseGoal goal;

void setPose(move_base_msgs::MoveBaseGoal &goal, double p_x, double p_y, double theta)
{
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();

    goal.target_pose.pose.position.x = p_x;
    goal.target_pose.pose.position.y = p_y;
    goal.target_pose.pose.orientation.z = sin(theta/2);
    goal.target_pose.pose.orientation.w = cos(theta/2);

    ac->sendGoal(goal);
    ac->waitForResult();
}

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

    ac = new MoveBaseClient("move_base", true);

    while (!ac->waitForServer(ros::Duration(5.0)))
    {
       ROS_INFO("Waiting for the move_base action server");
    }

    ROS_INFO("Sending goal");

    setPose(goal, 1.0, 0.0, 0.0);
    setPose(goal, 3.0, 0.0, 90.0);
    setPose(goal, 3.0, 2.0, 180.0);
    setPose(goal, 1.0, 2.0, 270.0);
    setPose(goal, 1.0, 0.0, 0.0);

    if (ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
       ROS_INFO("You have arrived to the goal position");
    }

    else
    {
       ROS_INFO("The base failed for some reason");
    }

    return 0;

}