ROS2小乌龟案例动作通信

无边界ROS2

于 2024-07-18 08:52:01 发布

阅读量264

点赞数 11

分类专栏： ROS2 文章标签：前端机器人 ROS2 c++ python 算法

本文链接：https://blog.csdn.net/weixin_47378530/article/details/140512061

版权

ROS2 专栏收录该内容

42 篇文章 0 订阅

订阅专栏

1.案例需求

需求：处理请求发送的目标点，控制乌龟向该目标点运动，并连续反馈乌龟与目标点之间的剩余距离。

2.案例分析

在上述案例与服务通信案例类似，需要关注的问题有两个：

如何在指定位置生成一只新乌龟？
控制原生乌龟向目标乌龟运动并连续反馈剩余距离应该使用何种通信模式又如何实现？

思路：

问题1可以通过调用turtlesim_node内置的名称为/spawn的服务功能来实现新乌龟的创建；
问题2可以通过动作通信来实现，动作客户端发送新生成的乌龟的位姿到动作服务端，服务端根据该坐标以及原生乌龟的坐标计计算出二者距离，计算速度并控制原生乌龟运动。当然如果使用动作通信，还需要自定义动作接口。
最后，整个案例涉及到多个节点，我们可以通过launch文件集成这些节点。

3.流程简介

主要步骤如下：

编写动作接口文件；
编写动作服务端实现；
编写动作客户端实现；
编写launch文件；
编辑配置文件；
编译；
执行。

4.动作接口文件

功能包base_interfaces_demo的action目录下，新建action文件Nav.action，并编辑文件，输入如下内容：

float32 goal_x
float32 goal_y
float32 goal_theta
---
float32 turtle_x
float32 turtle_y
float32 turtle_theta
---
float32 distance

5.动作服务端实现

功能包cpp07_exercise的src目录下，新建C++文件exe04_action_server.cpp，并编辑文件，输入如下内容：

/*
   需求：处理请求发送的目标点，控制乌龟向该目标点运动，并连续反馈乌龟与目标点之间的剩余距离。
   步骤：
       1.包含头文件；
       2.初始化 ROS2 客户端；
       3.定义节点类；
            3-1.创建原生乌龟位姿订阅方，回调函数中获取乌龟位姿；
            3-2.创建原生乌龟速度发布方；
            3-3.创建动作服务端；
            3-4.解析动作客户端发送的请求；
            3-5.处理动作客户端发送的取消请求；
            3-6.创建新线程处理请求；
            3-7.新线程产生连续反馈并响应最终结果。
       4.调用spin函数，并传入节点对象指针；
       5.释放资源。
*/
/*
    处理客户端发送的请求数据(目标点)，控制乌龟向目标点移动，并连续反馈剩余距离
*/
#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "base_interfaces_demo/action/nav.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

using base_interfaces_demo::action::Nav;
using std::placeholders::_1;
using std::placeholders::_2;

class Exe04ActionServer: public rclcpp::Node
{
public:
    Exe04ActionServer():Node("exe04_action_server_node_cpp"),x(0.0),y(0.0)
    {
        RCLCPP_INFO(this->get_logger(),"动作服务端创建！");
        
        //创建原生乌龟位姿订阅方，获取当前乌龟坐标
        sub_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose", 10, std::bind(&Exe04ActionServer::pose_cb,this,_1));

        //创建一个速度指令发布方，控制乌龟运动
        cmd_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel",10);

        //创建一个动作服务端
        /*
            NodeT node, 
            const std::string &name, 
            rclcpp_action::Server<ActionT>::GoalCallback handle_goal, 
            rclcpp_action::Server<ActionT>::CancelCallback handle_cancel, 
            rclcpp_action::Server<ActionT>::AcceptedCallback handle_accepted
        */
        action_server_ = rclcpp_action::create_server<Nav>(
            this,
            "nav",
            std::bind(&Exe04ActionServer::handle_goal,this,_1,_2),
            std::bind(&Exe04ActionServer::handle_cancel,this,_1),
            std::bind(&Exe04ActionServer::handle_accepted,this,_1)
            );
    }

private:
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr sub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
    rclcpp_action::Server<Nav>::SharedPtr action_server_;
    float x,y;
    void pose_cb(const turtlesim::msg::Pose::SharedPtr pose)
    {
        x = pose->x;
        y = pose->y;
    }

    //请求目标处理
    /*
        GoalResponse(const GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)
    */
    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const Nav::Goal> goal)
    {
        (void)uuid;
        
        if(goal->goal_x < 0 || goal->goal_x > 11.08 || goal->goal_y < 0 || goal->goal_y > 11.08)
        {
            //取出目标点中的x,y,判断是否超出范围[0,11.08],如果超出，则不接收，否则接收
            RCLCPP_INFO(this->get_logger(),"输入的坐标超出图像可显示范围！");
            return rclcpp_action::GoalResponse::REJECT;
        }
        RCLCPP_INFO(this->get_logger(),"目标点合法！");
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    //请求取消处理
    /*
        CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)
    */
    rclcpp_action::CancelResponse handle_cancel(std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle)
    {
        (void)goal_handle;

        RCLCPP_INFO(this->get_logger(),"取消任务！");
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    //主逻辑实现
    void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle)
    {
        //子线程处理主要逻辑
        RCLCPP_INFO(this->get_logger(),"主逻辑开始执行！");
        //最终结果
        auto result = std::make_shared<Nav::Result>();
        auto feedback = std::make_shared<Nav::Feedback>();
        geometry_msgs::msg::Twist twist;
        //1.生成连续反馈
        rclcpp::Rate rate(1.0);
        while(true)
        {
            //如果要取消任务，需要特殊处理
            if(goal_handle->is_canceling())
            {
                goal_handle->canceled(result);
                return;
            }

            //解析目标点坐标与原生乌龟实时坐标
            float goal_x = goal_handle->get_goal()->goal_x;
            float goal_y = goal_handle->get_goal()->goal_y;

            //计算剩余距离并发布
            float distance_x = goal_x - x;
            float distance_y = goal_y - y;
            float distance = std::sqrt(distance_x * distance_x + distance_y * distance_y);
            feedback->distance = distance;
            goal_handle->publish_feedback(feedback);

            //2.发布乌龟运动指令
            //根据剩余距离计算速度指令并发布
            float scale = 0.5;
            float linear_x = scale * distance_x;
            float linear_y = scale * distance_y;
            twist.linear.x = linear_x;
            twist.linear.y = linear_y;
            cmd_pub_->publish(twist);

            //循环结束条件
            if(distance <= 0.05)
            {
                //如果两个王八之间的距离小于0.05,就结束循环
                RCLCPP_INFO(this->get_logger(),"王八已经导航至终点！");
                break;
            }
            rate.sleep();
        }
       
        //3.生成最终反馈
        if(rclcpp::ok())
        {
            result->turtle_x = x;
            result->turtle_y = y;
            goal_handle->succeed(result);
        }
    }

    /*
        void (std::shared_ptr<ServerGoalHandle<ActionT>>)
    */
    void handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<Nav>> goal_handle)
    {
        std::thread(&Exe04ActionServer::execute,this,goal_handle).detach();
        
    }
};

int main(int argc,char* argv[])
{
    rclcpp::init(argc,argv);

    rclcpp::spin(std::make_shared<Exe04ActionServer>());

    rclcpp::shutdown();

    return 0;
}

6.动作客户端实现

功能包cpp07_exercise的src目录下，新建C++文件exe05_action_client.cpp，并编辑文件，输入如下内容：

/*
   需求：向动作服务端发送目标点数据，并处理服务端的响应数据。
   步骤：
       1.包含头文件；
       2.初始化 ROS2 客户端；
       3.定义节点类；
            3-1.创建动作客户端；
            3-2.发送请求数据，并处理服务端响应；
            3-3.处理目标响应；
            3-4.处理响应的连续反馈；
            3-5.处理最终响应。
       4.调用spin函数，并传入节点对象指针；
       5.释放资源。
*/
/*
    需求：向动作服务端发送目标点数据，并处理响应结果
*/
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "base_interfaces_demo/action/nav.hpp"

using base_interfaces_demo::action::Nav;
using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_2;

class Exe05ActionClient: public rclcpp::Node
{
public:
    Exe05ActionClient():Node("exe05_action_client_node_cpp")
    {
        RCLCPP_INFO(this->get_logger(),"动作客户端创建！");
        //创建动作客户端
        client_ = rclcpp_action::create_client<Nav>(this,"nav");
    }

    //连接服务端，发送请求
    void send_goal(float x, float y, float theta)
    {
        //连接服务器
        if(!client_->wait_for_action_server(10s))
        {
            RCLCPP_INFO(this->get_logger(),"服务连接超时！");
            return;
        }

        //组织并发送
        Nav::Goal goal;
        goal.goal_x = x;
        goal.goal_y = y;
        goal.goal_theta = theta;

        rclcpp_action::Client<Nav>::SendGoalOptions options;

        //std::function<void (std::shared_ptr<rclcpp_action::ClientGoalHandle<base_interfaces_demo::action::Nav>>, std::shared_ptr<const base_interfaces_demo::action::Nav_Feedback>)
        options.feedback_callback = std::bind(&Exe05ActionClient::feedback_callback,this,_1,_2);
        //std::function<void (std::shared_ptr<rclcpp_action::ClientGoalHandle<base_interfaces_demo::action::Nav>>)
        options.goal_response_callback = std::bind(&Exe05ActionClient::goal_response_callback,this,_1);
        //std::function<void (const rclcpp_action::ClientGoalHandle<base_interfaces_demo::action::Nav>::WrappedResult &result)
        options.result_callback = std::bind(&Exe05ActionClient::result_callback,this,_1);

        /*
            const base_interfaces_demo::action::Nav::Goal &goal, 
            const rclcpp_action::Client<base_interfaces_demo::action::Nav>::SendGoalOptions &options
        */
        client_->async_send_goal(goal,options);
    }
    //处理目标值相关响应结果
    void goal_response_callback(std::shared_ptr<rclcpp_action::ClientGoalHandle<Nav>> goal_handle)
    {
        if(goal_handle)
        {
            RCLCPP_INFO(this->get_logger(),"请求目标值被接收！");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(),"目标值非法！");
        }
    }

    //处理连续反馈
    void feedback_callback(std::shared_ptr<rclcpp_action::ClientGoalHandle<Nav>> goal_handle, std::shared_ptr<const Nav::Feedback> feedback)
    {
        (void)goal_handle;
        RCLCPP_INFO(this->get_logger(),"两只王八剩余距离%.2f米",feedback->distance);
    }

    //处理最终响应
    void result_callback(const rclcpp_action::ClientGoalHandle<Nav>::WrappedResult &result)
    {
        if(result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
            RCLCPP_INFO(this->get_logger(),"王八的最终坐标为(%.2f,%.2f)，航向为:%.2f",result.result->turtle_x, result.result->turtle_y, result.result->turtle_theta);
        }
        else if(result.code == rclcpp_action::ResultCode::UNKNOWN)
        {
            RCLCPP_INFO(this->get_logger(),"最终信息无法读取，读取失败！");
        }
        else if(result.code == rclcpp_action::ResultCode::CANCELED)
        {
            RCLCPP_INFO(this->get_logger(),"最终响应被取消！");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(),"响应失败！");
        }
    }

private:
    rclcpp_action::Client<Nav>::SharedPtr client_;

};

int main(int argc,char* argv[])
{
    //解析launch文件传入的参数
    if(argc != 5)
    {
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"请输入王八的参数！");
        return 1;
    }

    rclcpp::init(argc,argv);

    auto client = std::make_shared<Exe05ActionClient>();
    client->send_goal(atof(argv[1]),atof(argv[2]),atof(argv[3]));
    rclcpp::spin(client);
    
    rclcpp::shutdown();

    return 0;
}

7.launch文件

该案例需要分别为动作服务端和动作客户端创建launch文件。

功能包cpp07_exercise的launch目录下，首先新建动作服务端launch文件exe04_action_server.launch.py，编辑文件，输入如下内容：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    # 创建turtlesim_node节点
    turtle = Node(package="turtlesim",executable="turtlesim_node")
    # 创建动作服务端节点
    server = Node(package="cpp07_exercise",executable="exe04_action_server")

    return LaunchDescription([turtle,server])

然后新建动作客户端launch文件exe05_action_client.launch.py，编辑文件，输入如下内容：

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess

def generate_launch_description():
    # 设置目标点的坐标，以及目标点乌龟的名称
    x = 8.54
    y = 9.54
    theta = 0.0
    name = "t3"
    # 生成新的乌龟
    spawn = ExecuteProcess(
        cmd=["ros2 service call /spawn turtlesim/srv/Spawn \"{'x': "
                + str(x) + ",'y': " + str(y) + ",'theta': " + str(theta) + ",'name': '" + name + "'}\""],
        # output="both",
        shell=True
    )
    # 创建动作客户端节点
    client = Node(package="cpp07_exercise",
                executable="exe05_action_client",
                arguments=[str(x),str(y),str(theta)])
    return LaunchDescription([spawn,client])

8.编辑配置文件

此处需要编辑base_interfaces_demo和cpp07_exercise两个功能包下的配置文件。

1.base_interfaces_demo下的CMakeLists.txt

和前面服务通信一样，只需要修改CMakeLists.txt中的rosidl_generate_interfaces 函数即可，修改后的内容如下：

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Student.msg"
  "srv/AddInts.srv"
  "srv/Distance.srv"
  "action/Progress.action"
  "action/Nav.action"
)

2.cpp07_exercise下的CMakeLists.txt

CMakeLists.txt 文件需要添加如下内容：

add_executable(exe04_action_server src/exe04_action_server.cpp)
ament_target_dependencies(
  exe04_action_server
  "rclcpp"
  "turtlesim"
  "base_interfaces_demo"
  "geometry_msgs"
  "rclcpp_action"
)

add_executable(exe05_action_client src/exe05_action_client.cpp)
ament_target_dependencies(
  exe05_action_client
  "rclcpp"
  "turtlesim"
  "base_interfaces_demo"
  "geometry_msgs"
  "rclcpp_action"
)

文件中 install 修改为如下内容：

install(TARGETS 
  exe01_pub_sub
  exe02_server
  exe03_client
  exe04_action_server
  exe05_action_client  
  DESTINATION lib/${PROJECT_NAME})

9.编译

终端中进入当前工作空间，编译功能包：

colcon build --packages-select base_interfaces_demo cpp07_exercise

10.执行

当前工作空间下，启动两个终端。

终端1输入如下指令：

. install/setup.bash
ros2 launch cpp07_exercise exe04_action_server.launch.py

指令执行后，将生成turtlesim_node节点对应的窗口，并且会启动乌龟导航的动作服务端。

终端2输入如下指令：

. install/setup.bash
ros2 launch cpp07_exercise exe05_action_client.launch.py

指令执行后，会生成一只新的乌龟，并且原生乌龟会以新乌龟为目标点向其运动，运动过程中，动作客户端会接收服务端连续反馈的剩余距离消息，最终运行结果与演示案例类似。