完成rviz自己goal的插件后，来写一个订阅和发布到action的节点

最新推荐文章于 2024-01-24 14:46:16 发布

pd很不专业

最新推荐文章于 2024-01-24 14:46:16 发布

阅读量1.7k

点赞数 1

文章标签： ros move_base

本文链接：https://blog.csdn.net/qq_42145185/article/details/82345286

版权

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <unistd.h>
#include <geometry_msgs/PoseStamped.h>
#include "signal.h"

double pose_x_;
double pose_y_;
double pose_z_;
double pose_w_;

int count = 0;

   geometry_msgs::Point point;
   geometry_msgs::Quaternion quaternion;
   std::vector<geometry_msgs::Pose> pose_list;

void callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{

   pose_x_ = msg->pose.position.x;
   pose_y_ = msg->pose.position.y;
   pose_z_ = msg->pose.orientation.z;
   pose_w_ = msg->pose.orientation.w;
   ROS_INFO("pose_x_ : [%f]",pose_x_);
   ROS_INFO("pose_y_ : [%f]",pose_y_);
   ROS_INFO("pose_z_ : [%f]",pose_z_);
   ROS_INFO("pose_w_ : [%f]",pose_w_);

//设置我们要机器人走的几个点。

geometry_msgs::Pose pose;

   point.x = pose_x_;
   point.y = pose_y_;
   point.z = 0.000;
   quaternion.x = 0.000;
   quaternion.y = 0.000;
   quaternion.z = pose_z_;
   quaternion.w = pose_w_;
   pose.position = point;
   pose.orientation = quaternion;
   pose_list.push_back(pose);

count++;

}

bool flag = true;

void sigint(int signum){
flag = false;
}

int main(int argc, char** argv)
{
   ros::init(argc, argv,"sub_action");
   ros::NodeHandle n;
   ros::Subscriber sub = n.subscribe("goal",1000,callback);
   actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);

   signal(3, sigint);
   while(ros::ok() && flag){
       ros::spinOnce();
   }

while(ros::ok()){
ROS_INFO("Waiting for move_base action server...");
//等待60秒以使操作服务器可用
if(!ac.waitForServer(ros::Duration(30)))
{
ROS_INFO("Can't connected to move base server");

}
ROS_INFO("Connected to move base server");
ROS_INFO("Starting navigation test");

//循环通过四个航点
for(int j=0; j < count ; j++)
{
//初始化航点目标
move_base_msgs::MoveBaseGoal goal;

//使用地图框定义目标姿势
goal.target_pose.header.frame_id = "map";

//将时间戳设置为“now”
goal.target_pose.header.stamp = ros::Time::now();

//将目标姿势设置为第i个航点
goal.target_pose.pose = pose_list[j];

     //让机器人向目标移动
     //将目标姿势发送到MoveBaseAction服务器
     ac.sendGoal(goal);

//等1分钟到达那里
bool finished_within_time = ac.waitForResult();

    //如果我们没有及时赶到那里，就会中止目标
    if(!finished_within_time)
    {
        ac.cancelGoal();
        ROS_INFO("Timed out achieving goal");
    }
    else
    {
        //We made it!
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("Goal succeeded!");
        }
        else
        {
            ROS_INFO("The base failed for some reason");
        }
    }
}

}