ros2笔记-4.3 用C++做一个巡逻海龟

需求: zidingy让小海龟在海龟模拟器中随机游走进行巡逻

分析:

1，控制海龟到达目标点我知道，怎么改成动态接收?服务

2用什么接口?自定义的

3随机游走?客户端来产生随机点，请求巡逻服务

4.3.1 自定义服务接口

在src/chapt4_interfaces/srv下新建文件Patrol.srv，代码如下：

复制代码

float32 target_x 	# 目标x值
float32 target_y 	# 目标y值
---
int8 SUCCESS = 1    # 定义常量，表示成功
int8 FAIL = 0		    # 定义常量，表示失败
int8 result  	      # 处理结果:SUCCESS / FAIL

在CMakeLists.txt 增加对该接口注册，

复制代码

cmake_minimum_required(VERSION 3.8)
project(chapt4_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)
find_package(rosidl_default_generators REQUIRED)
find_package(sensor_msgs REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/FaceDetector.srv"
  "srv/Patrol.srv"
  DEPENDENCIES sensor_msgs
)


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()

构建接口功能包并进行测试。

复制代码

bohu@bohu-TM1701:~/chapt4/chapt4_ws$ colcon build
Starting >>> chapt4_interfaces
Finished <<< chapt4_interfaces [5.79s]                     
Starting >>> demo_python_service
Finished <<< demo_python_service [1.08s]          

Summary: 2 packages finished [7.16s]
bohu@bohu-TM1701:~/chapt4/chapt4_ws$ source install/setup.bash 
bohu@bohu-TM1701:~/chapt4/chapt4_ws$ ros2 interface show chapt4_interfaces/srv/Patrol 
float32 target_x        # 目标x值
float32 target_y        # 目标y值
---
int8 SUCCESS = 1    # 定义常量，表示成功
int8 FAIL = 0               # 定义常量，表示失败
int8 result           # 处理结果:SUCCESS / FAIL

4.3.2 服务端代码实现

在src下创建服务端功能包demo_cpp_service。

ros2 pkg create demo_cpp_service --build-type ament_cmake --dependencies chapt4_interfaces rclcpp geometry_msgs turtlesim --license Apache-2.0

新建文件turtle_control.cpp,代码如下

cpp 复制代码

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "turtlesim/msg/pose.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
using Patrol = chapt4_interfaces::srv::Patrol;
using namespace std;


class TurtleControlNode : public rclcpp::Node
{
public:
    TurtleControlNode() : Node("turtle_controller")
    {   
        patrol_service_ =  this->create_service<Patrol>(
        "patrol",
        [&](const std::shared_ptr<Patrol::Request> request,
            std::shared_ptr<Patrol::Response> response) -> void {
          // 判断巡逻点是否在模拟器边界内
          if ((0 < request->target_x && request->target_x < 12.0f)
           && (0 < request->target_y && request->target_y < 12.0f)) {
            target_x_ = request->target_x;
            target_y_ = request->target_y;
            response->result = Patrol::Response::SUCCESS;
          }else{
            response->result = Patrol::Response::FAIL;
          }
        });

        //调用继承来父类来创建发布者  
        publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel",10);
        //订阅者
        subscriber_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose",10,std::bind(&
        TurtleControlNode::on_pose_received_,this,placeholders::_1));
    }
private:
    void on_pose_received_(const turtlesim::msg::Pose::SharedPtr pose) {
    //1获取当前位置
    auto current_x = pose->x;
    auto current_y = pose->y;
    RCLCPP_INFO(get_logger(),"当前：x=%f,y=%f",current_x,current_y);
    //2计算与目标距离，以及当前海龟的角度差
    auto distance = sqrt(
        (target_x_-current_x)*(target_x_-current_x)+
        (target_y_-current_y)*(target_y_-current_y)
    );
    auto angle = atan2((target_y_-current_y),(target_x_-current_x))-pose->theta;

    //3控制策略：
    auto msg = geometry_msgs::msg::Twist();
    if(distance>0.1){
        if(fabs(angle)>0.2)
        {
            msg.angular.z = fabs(angle);
        }else{
            msg.linear.x = k_*distance;
        }
    }

    //4 限制最大速度
    if(msg.linear.x>max_speed_){
        msg.linear.x = max_speed_;
    }
    publisher_->publish(msg);
  }
private:
    rclcpp::Service<Patrol>::SharedPtr patrol_service_;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr  subscriber_; //订阅者智能指针
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;   //发布者智能指针  
    double target_x_{1.0}; //目标位置X,
    double target_y_{1.0}; 目标位置Y
    double k_{1.0};//比例系数
    double max_speed_{2.0}; //最大速度    
};

int main(int argc,char *argv[])
{
    rclcpp::init(argc,argv);
    auto node = std::make_shared<TurtleControlNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

这个代码，实在之前3.3.2节小海龟模拟器控制复制过来，加了一段创建服务create_service，使用了lambda表达式作为回调函数。参数是请求跟响应对象的共享指针。注意：视频里面跟书上命名上有点差别。

接着在CMakeLists.txt对turtle_contorl节点进行注册。构建，运行之前先运行小海龟模拟器。

在启动节点:ros2 run demo_cpp_service turtle_control

测试呢：可以在终端打开rqt,直接传参数控制小海龟运动轨迹，效果如下。

4.3.3 客户端代码实现

在src/demo_cpp_service/src 新建文件：patrol_client.cpp

客户端只需要生成随机坐标点，发送请求即可。代码如下：

cpp 复制代码

#include <chrono>
#include <cstdlib>
#include <ctime>
#include "rclcpp/rclcpp.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
using Patrol = chapt4_interfaces::srv::Patrol;
using namespace std;
using namespace std::chrono_literals;//可以使用10s


class PatrolClient : public rclcpp::Node
{
public:
    PatrolClient() : Node("patrol_client")
    {   
        patrol_client_ = this->create_client<Patrol>("patrol");
        timer_ = this->create_wall_timer(10s,[&]()->void{
            //1检测服务端是否上线
            while(!patrol_client_->wait_for_service(1s)){
                RCLCPP_ERROR(this->get_logger(),"等待服务上线过程中，rclcpp异常，退出");
                return;
            }
            RCLCPP_INFO(this->get_logger(),"等待服务上线....");
            //2 构造请求对象
            auto request = std::make_shared<Patrol::Request>();
            request->target_x = rand()%15;
            request->target_y = rand()%15;
            RCLCPP_INFO(this->get_logger(),"已准备好目标点%f,%f",request->target_x ,request->target_y );
            //3 发送请求
            this->patrol_client_->async_send_request(request,[&](rclcpp::Client<Patrol>::SharedFuture result_future)->void{
                auto response = result_future.get();
                if(response->result==Patrol::Response::SUCCESS){
                    RCLCPP_INFO(this->get_logger(),"请求目标点处理成功");
                }else if(response->result==Patrol::Response::FAIL){
                    RCLCPP_INFO(this->get_logger(),"请求目标点处理失败");
                }

            });

        });
    }
private:
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Client<Patrol>::SharedPtr patrol_client_;
};

int main(int argc,char *argv[])
{
    rclcpp::init(argc,argv);
    auto node = std::make_shared<PatrolClient>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

PatrolClient() 继承了Node节点，有两个成员变量patrol_client_、timer_,定时器用来定时异步请求服务端。再构造函数里面进行初始化。

接着在CMakeLists.txt对节点patrol_client进行注册。构建，运行之前先运行小海龟模拟器。

在运行服务端：ros2 run demo_cpp_service turtle_control

再运行客户端：ros2 run demo_cpp_service patrol_client

效果如下