SLAMカーはシングルポイントマルチポイントナビゲーションタスクを実現（自律充電をシミュレート）

自動充電を開発する場合、通常、最初に充電パイルの近くまで移動し、次に微調整を実行して充電パイルが存在するかどうかを判断し、その後、自動車が 180 度回転し、後退して充電パイルにドッキングします。ここで、最初のステップは、充電パイルの位置がシミュレートされる充電パイルの前に移動することです。次の 2 つの方法があります。

1. RVIZ の 2D ナビゲーションゴールを使用すると、ミッションターゲットポイントとポーズの解放を実現できます。
2. 公開されたトピックは /move_base_simple/goal です

catkin_create_pkg simple_goal_pub roscpp rospy actionlib move_base_msgs

send_goals.cpp

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

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 は、グローバルポジショニングであるマップに設定することも、ローカル座標系である Base_link または Base_footprinter に設定することもできます。これは、現在のロボットのポーズがタスクポイントを発行するための座標原点として使用されることを意味します。

struct goal_pose set_point 内のノード要素または順序を変更することにより、ロボットは目標点に従って順番に巡航を完了できます。グローバルポジショニングの場合は、rviz を使用して最初にターゲットポイントをクリックし、次にファイルターゲットポイントセットを記録して変更する必要があります。

rostopic echo /amcl_poseロボットの現在位置 (rviz 上の座標) を取得することもできます。

SLAMカーはシングルポイントマルチポイントナビゲーションタスクを実現（自律充電をシミュレート）

おすすめ