ROS2 rolling的action举例

项目文件结构与命令行

文件树：（新项目文件树必须含相同结构部分）

~/yaboomcar/src$ tree -L 3
.
├── pkg_action
│   ├── package.xml
│   ├── pkg_action
│   │   ├── action_client_demo.py
│   │   ├── action_server_demo.py
│   │   └── __init__.py
│   ├── resource
│   │   └── pkg_action
│   ├── setup.cfg
│   ├── setup.py
│   └── test
│       ├── test_copyright.py
│       ├── test_flake8.py
│       ├── test_pep257.py
│       └── test_xmllint.py
└── pkg_interfaces
    ├── action
    │   └── Progress.action
    ├── CMakeLists.txt
    ├── commandLines
    ├── include
    │   └── pkg_interfaces
    ├── package.xml
    ├── Progress.action
    └── src

命令行有：

ros2 pkg create --build-type ament_cmake pkg_interfaces

colcon build --packages-select pkg_interfaces

source install/setup.bash
ros2 interface show pkg_interfaces/action/Progress



ros2 pkg create pkg_action --build-type ament_python\
 --dependencies rclpy pkg_interfaces\
 --node-name action_server_demo

colcon build --packages-select pkg_interfaces pkg_action

source install/setup.bash

ros2 run pkg_action action_server_demo
# new terminal
ros2 action list
source install/setup.bash
ros2 action send_goal /get_sum pkg_interfaces/action/Progress num:\ 10



colcon build --packages-select pkg_action
source install/setup.bash
ros2 run pkg_action action_client_demo

包：pkg_interfaces

文件名：Progress.action

编码：

int64 num

---

int64 sum

---

float64 progress

文件名：CMakeLists.txt

编码：

cmake_minimum_required(VERSION 3.8)
project(pkg_interfaces)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)


find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}

  "action/Progress.action"

)


if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

 

文件名：package.xml

编码：

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>pkg_interfaces</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <buildtool_depend>rosidl_default_generators</buildtool_depend>

  <depend>action_msgs</depend>

  <member_of_group>rosidl_interface_packages</member_of_group>


  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

 

包：pkg_action

文件名：action_client_demo.py

编码：

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from pkg_interfaces.action import Progress
from action_msgs.msg import GoalStatus

class Action_Client(Node):
    def __init__(self):
        super().__init__('progress_action_client')
        self._action_client = ActionClient(self, Progress, 'get_sum')
        self._goal_handle = None  # Future类对象
        self._cancel_timer = None # 定时器类对象

    def send_goal(self, num):
        goal_msg = Progress.Goal()
        goal_msg.num = num  #目标值
        self._action_client.wait_for_server()  #阻塞等待

        # send request  to action server
        self._send_goal_future = self._action_client.send_goal_async(
            goal_msg, #interface接口类对象
            feedback_callback=self.feedback_callback  #连续反馈 
        )
        # attach  callback for action server response
        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        """回调函数"""
        self._goal_handle = future.result()
        if not self._goal_handle.accepted:
            self.get_logger().info('请求被拒绝')
            return

        self.get_logger().info('请求被接收，开始执行任务！')
    
        # 3秒后取消任务（根据需求调整）
        self._cancel_timer = self.create_timer(3.0, self.cancel_goal)
        
        self._get_result_future = self._goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def cancel_goal(self):
        """触发取消action服务,也作为定时器回调函数"""
        if self._goal_handle is not None:
            self.get_logger().info('发送取消请求...')
            future = self._goal_handle.cancel_goal_async()
            future.add_done_callback(self.cancel_done_callback)
            self._cancel_timer.cancel()  # 取消定时器，避免重复发送

    def cancel_done_callback(self, future):
        """回调函数"""
        cancel_response = future.result()
        if len(cancel_response.goals_canceling) > 0:
            self.get_logger().info('取消请求已被接受')
        else:
            self.get_logger().info('取消请求未被接受（可能任务已完成）')

    def get_result_callback(self, future):
        """回调函数"""
        result = future.result()
        if result.status == GoalStatus.STATUS_CANCELED:
            self.get_logger().info('任务已被取消')
        else:
            self.get_logger().info(f'最终计算结果: sum = {result.result.sum}')
        rclpy.shutdown()

    def feedback_callback(self, feedback_msg):
        """回调函数"""
        feedback = int(feedback_msg.feedback.progress * 100)
        self.get_logger().info(f'当前进度: {feedback}%')

def main(args=None):
    rclpy.init(args=args)
    client = Action_Client()
    client.send_goal(10)  # 计算1到10的和
    rclpy.spin(client)

if __name__ == '__main__':
    main()

# import rclpy

# from rclpy.action import ActionClient

# from rclpy.node import Node

# from pkg_interfaces.action import Progress


# class Action_Client(Node):

#     def __init__(self):

#         super().__init__('progress_action_client')

#         # 创建动作客户端；

#         self._action_client = ActionClient(self, Progress, 'get_sum')


#     def send_goal(self, num):

#         # 发送请求；

#         goal_msg = Progress.Goal()

#         goal_msg.num = num

#         self._action_client.wait_for_server()

#         self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)

#         self._send_goal_future.add_done_callback(self.goal_response_callback)


#     def goal_response_callback(self, future):

#         # 处理目标发送后的反馈；

#         goal_handle = future.result()

#         if not goal_handle.accepted:

#             self.get_logger().info('请求被拒绝')

#             return


#         self.get_logger().info('请求被接收，开始执行任务！')


#         self._get_result_future = goal_handle.get_result_async()

#         self._get_result_future.add_done_callback(self.get_result_callback)


#     # 处理最终响应。

#     def get_result_callback(self, future):

#         result = future.result().result

#         self.get_logger().info('最终计算结果:sum = %d' % result.sum)

#         # 5.释放资源。

#         rclpy.shutdown()


#     # 处理连续反馈；

#     def feedback_callback(self, feedback_msg):

#         feedback = (int)(feedback_msg.feedback.progress * 100)

#         self.get_logger().info('当前进度: %d%%' % feedback)



# def main(args=None):

#     rclpy.init(args=args)

#     action_client = Action_Client()

#     action_client.send_goal(10)

#     rclpy.spin(action_client)

 

文件名：action_server_demo.py

编码：

import rclpy
from rclpy.action import ActionServer, CancelResponse
from rclpy.node import Node
from pkg_interfaces.action import Progress
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup

class Action_Server(Node):
    def __init__(self):
        super().__init__('progress_action_server')
        self._action_server = ActionServer(
            self,
            Progress,
            'get_sum',
             execute_callback=self.execute_callback,#必须给出值
            #cancel_callback=self.cancel_callback,
            callback_group=ReentrantCallbackGroup())
        self.get_logger().info('动作服务已经启动！')
        self._cancel_requested = False
        self._current_goal_handle = None
        #self._action_server.register_execute_callback(self.execute_callback)
        self._action_server.register_cancel_callback(self.cancel_callback)

    def cancel_callback(self, goal_handle):
        """处理取消请求的回调函数"""
        self.get_logger().info('收到取消请求')
        self._cancel_requested = True
        return CancelResponse.ACCEPT

    def execute_callback(self, goal_handle):
        self.get_logger().info('开始执行任务....')
        self._cancel_requested = False
        self._current_goal_handle = goal_handle
        
        feedback_msg = Progress.Feedback()
        sum = 0

        for i in range(1, goal_handle.request.num + 1):
            if self._cancel_requested:
                goal_handle.canceled()
                self.get_logger().info('任务已取消')
                self._current_goal_handle = None
                return Progress.Result()
            
            sum += i
            feedback_msg.progress = i / goal_handle.request.num
            self.get_logger().info(f'连续反馈: {feedback_msg.progress:.2f}')
            goal_handle.publish_feedback(feedback_msg)
            
            # 使用可中断的等待方式
            start_time = self.get_clock().now()
            while (self.get_clock().now() - start_time).nanoseconds < 1e9:
                if self._cancel_requested:
                    goal_handle.canceled()
                    self.get_logger().info('任务已取消')
                    self._current_goal_handle = None
                    return Progress.Result()
                rclpy.spin_once(self, timeout_sec=0.1)

        if not self._cancel_requested:
            goal_handle.succeed()
            result = Progress.Result()
            result.sum = sum
            self.get_logger().info('任务完成！')
            self._current_goal_handle = None
            return result
        return Progress.Result()

def main(args=None):
    rclpy.init(args=args)
    server = Action_Server()
    
    # 使用多线程执行器
    executor = MultiThreadedExecutor()
    rclpy.spin(server, executor=executor)
    
    server.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
# import time

# import rclpy

# from rclpy.action import ActionServer

# from rclpy.node import Node


# from pkg_interfaces.action import Progress



# class Action_Server(Node):


#     def __init__(self):

#         super().__init__('progress_action_server')

#         # 创建动作服务端

#         self._action_server = ActionServer(

#             self,

#             Progress,

#             'get_sum',

#             self.execute_callback)

#         self.get_logger().info('动作服务已经启动！')


#     def execute_callback(self, goal_handle):

#         self.get_logger().info('开始执行任务....')



#         # 生成连续反馈；

#         feedback_msg = Progress.Feedback()


#         sum = 0

#         for i in range(1, goal_handle.request.num + 1):

#             sum += i

#             feedback_msg.progress = i / goal_handle.request.num

#             self.get_logger().info('连续反馈: %.2f' % feedback_msg.progress)

#             goal_handle.publish_feedback(feedback_msg)

#             time.sleep(1)


#         # 生成最终响应。

#         goal_handle.succeed()

#         result = Progress.Result()

#         result.sum = sum

#         self.get_logger().info('任务完成！')


#         return result



# def main(args=None):


#     rclpy.init(args=args)

#     # 调用spin函数，并传入节点对象

#     Progress_action_server = Action_Server()

#     rclpy.spin(Progress_action_server)

#     Progress_action_server.destroy_node() 

#     # 释放资源

#     rclpy.shutdown()

 

文件名：setup.py

编码：

from setuptools import find_packages, setup

package_name = 'pkg_action'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='ubuntu',
    maintainer_email='ubuntu@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'action_server_demo = pkg_action.action_server_demo:main',
            'action_client_demo = pkg_action.action_client_demo:main'
        ],
    },
)

 

文件名：package.xml

编码：

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>pkg_action</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
  <license>TODO: License declaration</license>

  <depend>rclpy</depend>
  <depend>pkg_interfaces</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>ament_xmllint</test_depend>
  <test_depend>python3-pytest</test_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>