SLAM小车实现单点多点导航任务（模拟自主充电）

在开发自主充电时，一般是先导航到充电桩附近，然后再进行微调，判断充电桩是否存在；接着小车180度旋转，后退对接充电桩。在这里，第一步就需要先运动至充电桩前，这里模拟发布一个充电桩点位。有两种方法：

1、在RVIZ中使用2D navigation goal 可以实现任务目标点和位姿的发布。
2、通过发布的话题为/move_base_simple/goal

catkin_create_pkg simple_goal_pub roscpp rospy actionlib move_base_msgs

send_goals.cpp

#include 
#include 
#include 

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

struct goal_pose
{
    double pose[3];
    double orientation[4];
};

struct goal_pose set_point[]=
{
	 //这里可以添加自己的多个导航目标点
     {9.81875837467,0.116833412967,0,0,0,-0.00435611210075,0.999990512099},         
};

int size =sizeof(set_point)/sizeof(goal_pose);

 int main(int argc, char *argv[])
{
    ros::init(argc, argv, "simple_navigation_goals");
    MoveBaseClient ac("move_base",true);

    while (!ac.waitForServer(ros::Duration(1.0)))
    {
       ROS_INFO("waiting for the move_base action server to come up");
    }
    
    for(int i=0;i<size;i++)  
    {
        struct goal_pose point=set_point[i];
        move_base_msgs::MoveBaseGoal goal;

        //goal.target_pose.header.frame_id="base_link"; //局部定位
	    goal.target_pose.header.frame_id="map";			//全局定位
        goal.target_pose.header.stamp=ros::Time::now();

        goal.target_pose.pose.position.x=point.pose[0];
        goal.target_pose.pose.position.y=point.pose[1];
        goal.target_pose.pose.position.z=point.pose[2];
        goal.target_pose.pose.orientation.x=point.orientation[0];
        goal.target_pose.pose.orientation.y=point.orientation[1];
        goal.target_pose.pose.orientation.z=point.orientation[2];        
        goal.target_pose.pose.orientation.w=point.orientation[3];

        ROS_INFO("sending goal ...");
        ac.sendGoal(goal);
        ROS_INFO("goal executing ...");
        ac.waitForResult();
        if(ac.getState()==actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("the base move goal execute succsess ...");
            //continue;   //如果到达目标点想继续循环运动，则设为continue
            break;        //如果到达目标点之后，想停止，则break     
        }	 
        else{
            ROS_INFO("the base move goal execute failed ...");
            break;
        }
    }
    return 0;
}

frame_id可以设置成map 就是一个全局定位，设置为base_link 或者base_footprinter 则为一个局部的坐标系，表示以当前机器人位姿作为坐标原点，进行任务点的下发。

通过修改 struct goal_pose set_point里面的节点元素或者顺序，实现机器人按照目标巡航点，依次便利完成。全局定位，需要先使用rviz 点击目标点，然后依次记录下然后修改文件目标点集合。

也可以通过rostopic echo /amcl_pose获取机器人的当前位置（即在rviz上的坐标）。