〖ROS2 源码解析〗rclcpp_action/src/server_goal_handle.cpp

沉浸式AI

已于 2024-09-14 21:56:50 修改

阅读量1.2k

点赞数 14

CC 4.0 BY-SA版权

分类专栏：《ROS2系列教程》文章标签： linux ROS2

于 2024-08-27 17:46:37 首次发布

本文链接：https://blog.youkuaiyun.com/sinat_39783664/article/details/141605157

一、简要介绍

这段代码展示了 ROS 2 中rclcpp_action命名空间下ServerGoalHandleBase类的成员函数实现。ServerGoalHandleBase在动作服务器中扮演着关键的角色，负责与目标交互并管理其状态。

1. 文件核心功能

析构函数

ServerGoalHandleBase::~ServerGoalHandleBase()：空的析构函数，在对象销毁时可能执行一些清理操作，但目前没有具体实现。

状态判断方法

is_canceling()：通过锁定互斥锁，获取目标的状态，并判断是否处于取消中状态。如果获取状态过程中出现错误，会抛出相应的异常。
is_active()：锁定互斥锁后，直接调用底层函数判断目标是否处于活跃状态。
is_executing()：与is_canceling()类似，获取目标状态并判断是否处于执行中状态。

状态改变方法

_abort()、_succeed()、_cancel_goal()、_canceled()、_execute()：这些方法分别用于改变目标的状态为中止、成功、请求取消、已取消和开始执行。每个方法都通过锁定互斥锁，调用底层的rcl_action函数来更新目标状态。如果更新过程中出现错误，会抛出异常。

尝试取消方法

try_canceling()：这个方法首先检查目标是否可取消，如果可取消则尝试将目标状态转换为取消中，然后再次获取状态判断是否成功取消。如果任何步骤出现错误，返回false。

2. 使用场景

在 ROS 2 的动作服务器中，ServerGoalHandleBase的这些成员函数被用于管理和操作目标的状态。例如，当需要判断目标是否正在被取消、是否活跃或是否执行中时，可以调用相应的状态判断方法。当需要改变目标的状态，如中止目标、标记目标成功或取消目标时，可以调用对应的状态改变方法。而try_canceling()方法则在需要尝试取消目标时使用，例如在对象销毁前如果目标未达到终端状态，可以尝试取消目标以避免资源泄漏。

3. 总结

这段代码实现了ServerGoalHandleBase类的关键成员函数，为 ROS 2 动作服务器提供了强大的目标状态管理功能。通过这些函数，动作服务器可以准确地判断目标的状态，并在需要时改变目标的状态，确保动作服务器的稳定运行和正确响应。

二、源码与解析

// Copyright 2018 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <memory>

#include "rcl_action/action_server.h"
#include "rcl_action/goal_handle.h"

#include "rclcpp_action/server_goal_handle.hpp"
#include "rclcpp/exceptions.hpp"

namespace rclcpp_action
{
   
   
ServerGoalHandleBase::~ServerGoalHandleBase()
{
   
   
}

bool
ServerGoalHandleBase::is_canceling() const
{
   
   
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
  if (RCL_RET_OK != ret) {
   
   
    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
  }
  return GOAL_STATE_CANCELING == state;
}

bool
ServerGoalHandleBase::is_active() const
{
   
   
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  return rcl_action_goal_handle_is_active(rcl_handle_.get());
}

bool
ServerGoalHandleBase::is_executing() const
{
   
   
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_action_goal_state_t state = GOAL_STATE_UNKNOWN;
  rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_handle_.get(), &state);
  if (RCL_RET_OK != ret) {
   
   
    rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to get goal handle state");
  }
  return GOAL_STATE_EXECUTING == state;
}

void
ServerGoalHandleBase::_abort()
{
   
   
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_ABORT);
  if (RCL_RET_OK != ret) {
   
   
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
}

void
ServerGoalHandleBase::_succeed()
{
   
   
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_SUCCEED);
  if (RCL_RET_OK != ret) {
   
   
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
}

void
ServerGoalHandleBase::_cancel_goal()
{
   
   
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCEL_GOAL);
  if (RCL_RET_OK != ret) {
   
   
    rclcpp::exceptions::throw_from_rcl_error(ret);
  }
}

void
ServerGoalHandleBase::_canceled()
{
   
   
  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
  rcl_ret_t ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_CANCELED);
  if (RCL_RET_OK != ret) {
   
   
    rclcpp::exceptions