首页 > 电脑手机

ros2 action server示例、拓展、练习

电脑手机 2024-11-26 01:30:01 1405

注意：以下代码全部由ai生成，没有大问题，运用时需根据报错逐步调试

action server示例

ros2 action server示例、拓展、练习

将 goal、result 和 feedback 作为类的成员变量的 C++ 示例代码：

示例代码

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "your_package/action/my_action.hpp"
#include "your_package/srv/my_service.hpp"

class MyActionServer : public rclcpp::Node
{
public:
    using MyAction = your_package::action::MyAction;
    using GoalHandleMyAction = rclcpp_action::ServerGoalHandle<MyAction>;

    MyActionServer()
    : Node("my_action_server"),
      feedback_(std::make_shared<MyAction::Feedback>()),
      result_(std::make_shared<MyAction::Result>())
    {
        // 创建 Action 服务器
        action_server_ = rclcpp_action::create_server<MyAction>(
            this,
            "my_action",
            std::bind(&MyActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&MyActionServer::handle_cancel, this, std::placeholders::_1),
            std::bind(&MyActionServer::handle_accepted, this, std::placeholders::_1)
        );

        // 创建服务
        service_ = this->create_service<your_package::srv::MyService>(
            "my_service",
            std::bind(&MyActionServer::handle_service, this, std::placeholders::_1, std::placeholders::_2)
        );
    }

private:
    rclcpp_action::Server<MyAction>::SharedPtr action_server_;
    rclcpp::Service<your_package::srv::MyService>::SharedPtr service_;

    std::shared_ptr<const MyAction::Goal> goal_;
    std::shared_ptr<MyAction::Result> result_;
    std::shared_ptr<MyAction::Feedback> feedback_;

    // 处理目标
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const MyAction::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Received goal: %f", goal->goal_value);
        goal_ = goal;  // 存储目标
        return rclcpp_action::GoalResponse::YES;
    }

    // 处理取消请求
    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleMyAction> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Canceling goal");
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    // 处理已接受的目标
    void handle_accepted(const std::shared_ptr<GoalHandleMyAction> goal_handle)
    {
        std::thread([this, goal_handle]() {
            this->execute(goal_handle);
        }).detach();
    }

    // 执行目标
    void execute(const std::shared_ptr<GoalHandleMyAction> goal_handle)
    {
        for (int i = 1; i <= 5; ++i) {
            feedback_->current_value = i * goal_->goal_value;
            goal_handle->publish_feedback(feedback_);
            RCLCPP_INFO(this->get_logger(), "Feedback: %f", feedback_->current_value);
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }

        // 设置结果
        result_->result_value = goal_->goal_value * 10;
        goal_handle->succeed(result_);
        RCLCPP_INFO(this->get_logger(), "Action succeeded with result: %f", result_->result_value);
    }

    // 处理服务
    void handle_service(
        const std::shared_ptr<your_package::srv::MyService::Request> request,
        std::shared_ptr<your_package::srv::MyService::Response> response)
    {
        response->response_value = request->request_value * 2;  // 示例处理
        RCLCPP_INFO(this->get_logger(), "Service called with value: %f, response: %f",
                     request->request_value, response->response_value);
    }
};

int main(int argc, char ** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MyActionServer>());
    rclcpp::shutdown();
    return 0;
}

代码说明

  • 成员变量：

    • goal_: 存储接收到的目标。
    • result_: 存储结果信息。
    • feedback_: 存储反馈信息。

  • 在 handle_goal 方法中，接收到的目标被存储到 goal_ 中。

  • 在 execute 方法中，根据 goal_ 的值生成反馈，并发布反馈和结果。

拓展1 在execute中创建一个导航action client，并发布。

要在 execute 方法中创建一个导航目标点的 Action 客户端并发布一个固定点，你需要确保在代码中添加相关的导航 Action 依赖。以下是更新后的 C++ 示例代码：

示例代码

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "your_package/action/my_action.hpp"
#include "your_package/srv/my_service.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp" // 导航 Action

class MyActionServer : public rclcpp::Node
{
public:
    using MyAction = your_package::action::MyAction;
    using GoalHandleMyAction = rclcpp_action::ServerGoalHandle<MyAction>;
    using NavigateToPose = nav2_msgs::action::NavigateToPose;
    using NavigateToPoseClient = rclcpp_action::Client<NavigateToPose>;

    MyActionServer()
    : Node("my_action_server"),
      feedback_(std::make_shared<MyAction::Feedback>()),
      result_(std::make_shared<MyAction::Result>()),
      navigate_client_(rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose"))
    {
        action_server_ = rclcpp_action::create_server<MyAction>(
            this,
            "my_action",
            std::bind(&MyActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&MyActionServer::handle_cancel, this, std::placeholders::_1),
            std::bind(&MyActionServer::handle_accepted, this, std::placeholders::_1)
        );

        service_ = this->create_service<your_package::srv::MyService>(
            "my_service",
            std::bind(&MyActionServer::handle_service, this, std::placeholders::_1, std::placeholders::_2)
        );
    }

private:
    rclcpp_action::Server<MyAction>::SharedPtr action_server_;
    rclcpp::Service<your_package::srv::MyService>::SharedPtr service_;
    std::shared_ptr<const MyAction::Goal> goal_;
    std::shared_ptr<MyAction::Result> result_;
    std::shared_ptr<MyAction::Feedback> feedback_;
    rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_client_;

    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const MyAction::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Received goal: %f", goal->goal_value);
        goal_ = goal;
        return rclcpp_action::GoalResponse::YES;
    }

    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleMyAction> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Canceling goal");
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void handle_accepted(const std::shared_ptr<GoalHandleMyAction> goal_handle)
    {
        std::thread([this, goal_handle]() {
            this->execute(goal_handle);
        }).detach();
    }

    void execute(const std::shared_ptr<GoalHandleMyAction> goal_handle)
    {
        // 创建导航目标点
        auto navigation_goal = NavigateToPose::Goal();
        navigation_goal.pose.header.frame_id = "map"; // 或者其他适当的坐标框架
        navigation_goal.pose.header.stamp = this->now();
        navigation_goal.pose.pose.position.x = 1.0; // 固定点的坐标
        navigation_goal.pose.pose.position.y = 1.0;
        navigation_goal.pose.pose.orientation.w = 1.0; // 方向

        // 调用导航客户端
        if (!navigate_client_->wait_for_action_server(std::chrono::seconds(5))) {
            RCLCPP_ERROR(this->get_logger(), "Navigation action server not available");
            goal_handle->abort(result_);
            return;
        }

        auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
        send_goal_options.result_callback = [](const rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr &) {
            RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Navigation goal accepted");
        };

        auto goal_handle_navigation = navigate_client_->async_send_goal(navigation_goal, send_goal_options);

        // 处理导航反馈
        while (rclcpp::ok()) {
            // 这里可以添加导航反馈处理逻辑
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
        }

        // 返回结果
        result_->result_value = goal_->goal_value * 10;
        goal_handle->succeed(result_);
        RCLCPP_INFO(this->get_logger(), "Action succeeded with result: %f", result_->result_value);
    }

    void handle_service(
        const std::shared_ptr<your_package::srv::MyService::Request> request,
        std::shared_ptr<your_package::srv::MyService::Response> response)
    {
        response->response_value = request->request_value * 2;
        RCLCPP_INFO(this->get_logger(), "Service called with value: %f, response: %f",
                     request->request_value, response->response_value);
    }
};

int main(int argc, char ** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MyActionServer>());
    rclcpp::shutdown();
    return 0;
}

代码说明

  1. 导航 Action 客户端：

    • 创建 NavigateToPose 的 Action 客户端。
    • 使用 navigate_client_ 来发送导航目标。

  2. 导航目标点：

    • 在 execute 方法中定义固定的导航目标点（例如 (1.0, 1.0)）。
    • 设置导航目标的坐标和方向。

  3. 异步发送目标：

    • 使用 async_send_goal 方法发送导航目标，处理结果和反馈。
    • 可以在循环中添加对导航状态的反馈处理逻辑。

使用

  1. 确保导航 Action 服务器（例如 Nav2）正在运行。
  2. 启动此 Action 服务器节点。
  3. 在客户端调用 Action 和服务，观察服务器的导航行为。

拓展2 针对send_goal_options配置结构，创建一个新成员函数，替代上一段的lambda函数， 并使用send_goal_options的所有成员

好的，下面是更新后的代码示例，其中我们创建了一个新的成员函数来处理 send_goal_options 的配置，替代了之前使用的 Lambda 函数。这一新函数将使用 send_goal_options 的所有成员。

更新后的示例代码

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "your_package/action/my_action.hpp"
#include "your_package/srv/my_service.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp" // 导航 Action

class MyActionServer : public rclcpp::Node
{
public:
    using MyAction = your_package::action::MyAction;
    using GoalHandleMyAction = rclcpp_action::ServerGoalHandle<MyAction>;
    using NavigateToPose = nav2_msgs::action::NavigateToPose;
    using NavigateToPoseClient = rclcpp_action::Client<NavigateToPose>;

    MyActionServer()
    : Node("my_action_server"),
      feedback_(std::make_shared<MyAction::Feedback>()),
      result_(std::make_shared<MyAction::Result>()),
      navigate_client_(rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose"))
    {
        action_server_ = rclcpp_action::create_server<MyAction>(
            this,
            "my_action",
            std::bind(&MyActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&MyActionServer::handle_cancel, this, std::placeholders::_1),
            std::bind(&MyActionServer::handle_accepted, this, std::placeholders::_1)
        );

        service_ = this->create_service<your_package::srv::MyService>(
            "my_service",
            std::bind(&MyActionServer::handle_service, this, std::placeholders::_1, std::placeholders::_2)
        );
    }

private:
    rclcpp_action::Server<MyAction>::SharedPtr action_server_;
    rclcpp::Service<your_package::srv::MyService>::SharedPtr service_;
    std::shared_ptr<const MyAction::Goal> goal_;
    std::shared_ptr<MyAction::Result> result_;
    std::shared_ptr<MyAction::Feedback> feedback_;
    rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_client_;

    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const MyAction::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Received goal: %f", goal->goal_value);
        goal_ = goal;
        return rclcpp_action::GoalResponse::YES;
    }

    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleMyAction> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Canceling goal");
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void handle_accepted(const std::shared_ptr<GoalHandleMyAction> goal_handle)
    {
        std::thread([this, goal_handle]() {
            this->execute(goal_handle);
        }).detach();
    }

    void execute(const std::shared_ptr<GoalHandleMyAction> goal_handle)
    {
        // 创建导航目标点
        auto navigation_goal = NavigateToPose::Goal();
        navigation_goal.pose.header.frame_id = "map";
        navigation_goal.pose.header.stamp = this->now();
        navigation_goal.pose.pose.position.x = 1.0;
        navigation_goal.pose.pose.position.y = 1.0;
        navigation_goal.pose.pose.orientation.w = 1.0;

        // 调用导航客户端
        if (!navigate_client_->wait_for_action_server(std::chrono::seconds(5))) {
            RCLCPP_ERROR(this->get_logger(), "Navigation action server not available");
            goal_handle->abort(result_);
            return;
        }

        // 使用新成员函数配置 send_goal_options
        configure_send_goal_options(goal_handle, navigation_goal);
    }

    void configure_send_goal_options(const std::shared_ptr<GoalHandleMyAction> goal_handle, const NavigateToPose::Goal &navigation_goal)
    {
        auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();

        // 设置 result_callback
        send_goal_options.result_callback = [this](const rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr &goal_handle) {
            if (goal_handle->get_status() == rclcpp_action::GoalStatus::SUCCEEDED) {
                RCLCPP_INFO(this->get_logger(), "Navigation goal succeeded");
            } else {
                RCLCPP_ERROR(this->get_logger(), "Navigation goal failed");
            }
        };

        // 设置 feedback_callback
        send_goal_options.feedback_callback = [this](const rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr &goal_handle, const std::shared_ptr<const NavigateToPose::Feedback> feedback) {
            RCLCPP_INFO(this->get_logger(), "Navigation feedback: current position %f, %f",
                        feedback->current_pose.position.x, feedback->current_pose.position.y);
        };

        // 设置 goal_response_callback
        send_goal_options.goal_response_callback = [](const rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr &goal_handle) {
            if (goal_handle) {
                RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Navigation goal accepted");
            } else {
                RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Navigation goal rejected");
            }
        };

        // 发送导航目标
        auto goal_handle_navigation = navigate_client_->async_send_goal(navigation_goal, send_goal_options);
    }

    void handle_service(
        const std::shared_ptr<your_package::srv::MyService::Request> request,
        std::shared_ptr<your_package::srv::MyService::Response> response)
    {
        response->response_value = request->request_value * 2;
        RCLCPP_INFO(this->get_logger(), "Service called with value: %f, response: %f",
                     request->request_value, response->response_value);
    }
};

int main(int argc, char ** argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MyActionServer>());
    rclcpp::shutdown();
    return 0;
}

代码说明

  1. 新成员函数 configure_send_goal_options：

    • 此函数负责配置 send_goal_options 的所有成员，包括 result_callback、feedback_callback 和 goal_response_callback。
    • 通过捕获 this，可以在回调中访问类的成员函数和变量。

  2. 回调函数的实现：

    • result_callback: 检查导航目标的状态，并根据成功或失败打印日志信息。
    • feedback_callback: 输出当前导航位置的反馈信息。
    • goal_response_callback: 检查目标是否被接受，并输出相应的日志信息。

  3. 调用导航目标：

    • 在 execute 方法中，调用 configure_send_goal_options 来配置并发送导航目标。

这种方式将发送目标的逻辑与配置的回调逻辑分离，使代码更清晰、可维护。如果你有其他问题或者需要进一步的解释，请随时告诉我！

注意：只要设置了 send_goal_options.feedback_callback，回调函数就会在导航服务器发送反馈时被调用，可以在函数内部获取并处理这些反馈信息。
示例：
将其改写为

send_goal_options.feedback_callback = 
[this](const rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr &goal_handle
, const std::shared_ptr<const NavigateToPose::Feedback> feedback) 
{};

根据定义

#goal definition
geometry_msgs/PoseStamped pose
string behavior_tree
---
#result definition
std_msgs/Empty result
---
#feedback definition
geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
builtin_interfaces/Duration estimated_time_remaining
int16 number_of_recoveries
float32 distance_remaining
】

action的客户端也能获取
【geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
builtin_interfaces/Duration estimated_time_remaining
int16 number_of_recoveries
float32 distance_remaining】
这些信息

练习1

1.实现拓展中创建的navigation的导航action与自定义的action通信
2.找到isaac_mission_client实现其中自定义action与isaac_mission_client的通信，注意参考mqtt协议。

示例练习actionserver
版权声明：如无特殊标注，文章均来自网络，本站编辑整理，转载时请以链接形式注明文章出处，请自行分辨。

本文链接：https://www.shbk5.com/dnsj/74433.html

上一篇rog和黑鲨哪个好（黑鲨2pro和华硕rog2选哪个）

下一篇Python Web项目管理与团队协作

相关文章

热门文章

最新发布

标签列表