ros通过switch语句实现多目标点导航（c++实现）

首先，需要在ROS中创建一个节点，并订阅当前机器人的位置信息和目标点信息。

接着，可以使用switch语句来实现多目标点导航。在每次接收到新的目标点信息时，判断目标点的编号，然后根据不同的目标点编号执行相应的导航操作。例如，可以调用move_base节点来规划机器人的运动路径，然后使用ROS的导航功能包来控制机器人移动到目标点。

以下是一个简单的C语言实现：

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

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

void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    int goal_num = msg->header.seq;

    MoveBaseClient ac("move_base", true);

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

    move_base_msgs::MoveBaseGoal goal;

    // Set the goal position and orientation
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose.position.x = msg->pose.position.x;
    goal.target_pose.pose.position.y = msg->pose.position.y;
    goal.target_pose.pose.orientation.w = 1.0;

    switch(goal_num)
    {
        case 1:
            ROS_INFO("Navigating to Goal 1");
            break;

        case 2:
            ROS_INFO("Navigating to Goal 2");
            break;

        case 3:
            ROS_INFO("Navigating to Goal 3");
            break;

        default:
            ROS_INFO("Invalid goal number");
            return;
    }

    ac.sendGoal(goal);

    ac.waitForResult();

    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("Successfully reached the goal");
    }
    else
    {
        ROS_INFO("Failed to reach the goal");
    }

}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "multi_goal_navigation");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("/goal", 10, goalCallback);

    ros::spin();

    return 0;
}

在上面的代码中，我们定义了一个goalCallback函数来处理接收到的目标点信息。在函数中，我们首先获取目标点的编号，然后根据不同的目标点编号执行相应的导航操作。我们使用了move_base_msgs::MoveBaseAction来规划机器人的运动路径，并使用actionlib::SimpleActionClient来发送目标点信息。在机器人到达目标点后，我们会收到一个响应，然后根据响应的结果来判断是否成功到达目标点。

最后，我们在主函数中订阅了/goal主题，并使用ros::spin()函数来保持节点的运行状态。这样，我们就可以通过发送不同的目标点信息来控制机器人导航到不同的目标点。