ROS2- 接口-cpp + py实现 机器人移动-10
1.接口
接口其实是一种规范
std_msgs/msg/String
std_msgs/msg/UInt32
ROS2中定义了一个统一的接口叫做sensor_msgs/msg/LaserScan
每个雷达的厂家都会编写程序将自己雷达的数据变成sensor_msgs/msg/LaserScan格式
topic通信时std_msgs功能包是安装ROS2的时候
ROS2自动安装的，除了std_msgs之外，ROS2还定义了很多做机器人常用的接口。
ros2 interface package sensor_msgs 可以查看某一个接口包下所有的接口
打开终端输入：ros2 interface package sensor_msgs
sensor_msgs/msg/JointState #机器人关节数据
sensor_msgs/msg/Temperature #温度数据
sensor_msgs/msg/Imu #加速度传感器
sensor_msgs/msg/Image #图像
sensor_msgs/msg/LaserScan #雷达数据
ROS2为我们定义了大量拿来即用的接口，但有时候还是不能满足，需要掌握自定义接口的方法。
ROS2提供了四种通信方式：
话题-Topics
服务-Services
动作-Action
参数-Parameters
话题、服务和动作(Action)都支持自定义接口 参数不支持
话题接口格式：xxx.msg
int64 num
服务接口格式：xxx.srv
int64 a
int64 b
---
int64 sum
动作接口格式：xxx.action
int32 order
---
int32[] sequence
---
int32[] partial_sequence
接口数据类型
分为基础类型和包装类型
基础类型（后面加上[]可形成数组）
bool
byte
char
float32,float64
int8,uint8
int16,uint16
int32,uint32
int64,uint64
string
包装类型
uint32 id
string image_name
sensor_msgs/Image
接口如何生成代码？
转换的过程：将msg、srv、action文件转换为Python和C++的头文件
msg,srv,action->ROS2-IDL转换器->Python的py,C++的.h头文件
ROS2的IDL模块 产生了头文件，有了头文件，我们就可以在程序里导入并使用这个消息模块
机器人开发中的常见控制场景
设计满足要求的服务接口和话题接口
设计2个节点：
机器人节点，对外提供移动指定距离服务，移动完成后返回当前位置，同时对外发布机器人的位置和状态（是否在移动）。
机器人控制节点，通过服务控制机器人移动指定距离，并实时获取机器人的当前位置和状态。
（假设机器人在坐标轴上，只能前后移动）
服务接口MoveRobot.srv
# 前进后退的距离
float32 distance
---
# 当前的位置
float32 pose
话题接口，采用基础类型 RobotStatus.msg
uint32 STATUS_MOVEING = 1
uint32 STATUS_STOP = 1
uint32 status
float32 pose
话题接口，混合包装类型 RobotPose.msg
uint32 STATUS_MOVEING = 1
uint32 STATUS_STOP = 2
uint32 status
geometry_msgs/Pose pose
创建功能包
cd chapt3_ws/src
ros2 pkg create example_ros2_interfaces --build-type ament_cmake --dependencies rosidl_default_generators geometry_msgs
注：
功能包类型必须为：ament_cmake
依赖rosidl_default_generators必须添加，geometry_msgs视内容情况添加（我们这里有geometry_msgs/Pose pose所以要添加）
创建文件夹和文件将文件写入
├── CMakeLists.txt
├── msg
│ ├── RobotPose.msg
│ └── RobotStatus.msg
├── package.xml
└── srv
└── MoveRobot.srv
修改CMakeLists.txt
find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)
# 添加下面的内容
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/RobotPose.msg"
"msg/RobotStatus.msg"
"srv/MoveRobot.srv"
DEPENDENCIES geometry_msgs
)
修改package.xml
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rosidl_default_generators</depend>
<depend>geometry_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>
编译
cd src/
colcon build --packages-select example_ros2_interfaces
编译完成后
在install/example_ros2_interfaces/include下你应该可以看到C++的头文件
在install/example_ros2_interfaces/local/lib/python3.10/dist-packages下应该可以看到Python版本的头文件。
interface相关命令：
ros2 interface list
ros2 interface show std_msgs/msg/String
2. 接口 开发
利用上一节创建好的消息接口进行代码编写代码，
两个节点：
example_interfaces_robot_01，机器人节点，对外提供控制机器人移动服务并发布机器人的状态。
example_interfaces_control_01，控制节点，发送机器人移动请求，订阅机器人状态话题。
cd chapt3_ws/
ros2 pkg create example_interfaces_rclcpp --build-type ament_cmake --dependencies rclcpp example_ros2_interfaces --destination-directory src --node-name example_interfaces_robot_01
touch src/example_interfaces_rclcpp/src/example_interfaces_control_01.cpp
touch src/example_interfaces_rclcpp/src/example_interfaces_robot_01.cpp
CMakeLists.txt修改
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(example_ros2_interfaces REQUIRED)
add_executable(example_interfaces_robot_01 src/example_interfaces_robot_01.cpp)
target_include_directories(example_interfaces_robot_01 PUBLIC
\(<BUILD_INTERFACE:\){CMAKE_CURRENT_SOURCE_DIR}/include>
\(<INSTALL_INTERFACE:include>)
target_compile_features(example_interfaces_robot_01 PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
ament_target_dependencies(
example_interfaces_robot_01
"rclcpp"
"example_ros2_interfaces"
)
install(TARGETS example_interfaces_robot_01
DESTINATION lib/\){PROJECT_NAME})
add_executable(example_interfaces_control_01 src/example_interfaces_control_01.cpp)
target_include_directories(example_interfaces_control_01 PUBLIC
\(<BUILD_INTERFACE:\){CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_features(example_interfaces_control_01 PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
ament_target_dependencies(
example_interfaces_control_01
"rclcpp"
"example_ros2_interfaces"
)
install(TARGETS example_interfaces_control_01
DESTINATION lib/${PROJECT_NAME})
编写代码 example_interfaces_control_01.cpp
#include "rclcpp/rclcpp.hpp"
class ExampleInterfacesControl : public rclcpp::Node {
public:
// 构造函数,有一个参数为节点名称
ExampleInterfacesControl(std::string name) : Node(name) {
RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
}
private:
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<ExampleInterfacesControl>("example_interfaces_control_01");
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
编写代码 example_interfaces_robot_01.cpp
#include "rclcpp/rclcpp.hpp"
/*创建一个机器人类，模拟真实机器人*/
class Robot {
public:
Robot() = default;
~Robot() = default;
private:
};
class ExampleInterfacesRobot : public rclcpp::Node {
public:
ExampleInterfacesRobot(std::string name) : Node(name) {
RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
}
private:
Robot robot;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<ExampleInterfacesRobot>("example_interfaces_robot_01");
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
cd chapt3_ws/
colcon build
source install/setup.bash
root@ubuntu-16:~/stephen/chapt3/chapt3_ws# vi src/example_interfaces_rclcpp/src/example_interfaces_robot_01.cpp
root@ubuntu-16:~/stephen/chapt3/chapt3_ws# vi src/example_interfaces_rclcpp/src/example_interfaces_control_01.cpp
root@ubuntu-16:~/stephen/chapt3/chapt3_ws#
root@ubuntu-16:~/stephen/chapt3/chapt3_ws#
root@ubuntu-16:~/stephen/chapt3/chapt3_ws#
root@ubuntu-16:~/stephen/chapt3/chapt3_ws# colcon build
Starting >>> example_ros2_interfaces
Starting >>> example_service_rclcpp
Starting >>> example_service_rclpy
Starting >>> example_topic_rclcpp
Starting >>> example_topic_rclpy
Finished <<< example_topic_rclcpp [0.85s]
Finished <<< example_service_rclcpp [0.90s]
Finished <<< example_ros2_interfaces [1.21s]
Starting >>> example_interfaces_rclcpp
Finished <<< example_service_rclpy [1.61s]
Finished <<< example_topic_rclpy [1.63s]
Finished <<< example_interfaces_rclcpp [6.72s]
Summary: 6 packages finished [8.23s]
root@ubuntu-16:~/stephen/chapt3/chapt3_ws# source install/setup.bash
root@ubuntu-16:~/stephen/chapt3/chapt3_ws# ros2 run example_interfaces_rclcpp example_interfaces_robot_01
[INFO] [1749525367.479418628] [example_interfaces_robot_01]: 节点已启动：example_interfaces_robot_01.
^C[INFO] [1749525370.622373196] [rclcpp]: signal_handler(signum=2)
root@ubuntu-16:~/stephen/chapt3/chapt3_ws# ros2 run example_interfaces_rclcpp example_interfaces_control_01
[INFO] [1749525378.786881683] [example_interfaces_control_01]: 节点已启动：example_interfaces_control_01.
^C[INFO] [1749525380.781452000] [rclcpp]: signal_handler(signum=2)
继续丰富 机器人 以及 控制 相关逻辑
src/example_interfaces_rclcpp/src/example_interfaces_control_01.cpp
#include "rclcpp/rclcpp.hpp"
#include "example_ros2_interfaces/srv/move_robot.hpp"
#include "example_ros2_interfaces/msg/robot_status.hpp"
class ExampleInterfacesControl : public rclcpp::Node {
public:
ExampleInterfacesControl(std::string name) : Node(name) {
RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
/*创建move_robot客户端*/
client_ = this->create_client<example_ros2_interfaces::srv::MoveRobot>(
"move_robot");
/*订阅机器人状态话题*/
robot_status_subscribe_ = this->create_subscription<example_ros2_interfaces::msg::RobotStatus>("robot_status", 10, std::bind(&ExampleInterfacesControl::robot_status_callback_, this, std::placeholders::_1));
}
/**
* @brief 发送移动机器人请求函数
* 步骤：1.等待服务上线
* 2.构造发送请求
*
* @param distance
*/
void move_robot(float distance) {
RCLCPP_INFO(this->get_logger(), "请求让机器人移动%f", distance);
/*等待服务端上线*/
while (!client_->wait_for_service(std::chrono::seconds(1))) {
//等待时检测rclcpp的状态
if (!rclcpp::ok()) {
RCLCPP_ERROR(this->get_logger(), "等待服务的过程中被打断...");
return;
}
RCLCPP_INFO(this->get_logger(), "等待服务端上线中");
}
// 构造请求
auto request =
std::make_shared<example_ros2_interfaces::srv::MoveRobot::Request>();
request->distance = distance;
// 发送异步请求，然后等待返回，返回时调用回调函数
client_->async_send_request(
request, std::bind(&ExampleInterfacesControl::result_callback_, this,
std::placeholders::_1));
};
private:
// 声明客户端
rclcpp::Client<example_ros2_interfaces::srv::MoveRobot>::SharedPtr client_;
rclcpp::Subscription<example_ros2_interfaces::msg::RobotStatus>::SharedPtr robot_status_subscribe_;
/* 机器人移动结果回调函数 */
void result_callback_(
rclcpp::Client<example_ros2_interfaces::srv::MoveRobot>::SharedFuture
result_future) {
auto response = result_future.get();
RCLCPP_INFO(this->get_logger(), "收到移动结果：%f", response->pose);
}
/**
* @brief 机器人状态话题接收回调函数
*
* @param msg
*/
void robot_status_callback_(const example_ros2_interfaces::msg::RobotStatus::SharedPtr msg)
{
RCLCPP_INFO(this->get_logger(), "收到状态数据位置：%f 状态：%d", msg->pose ,msg->status);
}
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<ExampleInterfacesControl>("example_interfaces_control_01");
/*这里调用了服务，让机器人向前移动5m*/
node->move_robot(5.0);
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
cat src/example_interfaces_rclcpp/src/example_interfaces_robot_01.cpp
逻辑也比较简单，利用定时器不断发送数据，收到请求后调用机器人类的move_distance接口来移动机器人。
// 导入上一节定义的消息接口
#include "example_ros2_interfaces/msg/robot_status.hpp"
#include "example_ros2_interfaces/srv/move_robot.hpp"
#include "rclcpp/rclcpp.hpp"
/*
* 测试指令：ros2 service call /move_robot example_ros2_interfaces/srv/MoveRobot "{distance: 5}"
*/
class Robot {
public:
Robot() = default;
~Robot() = default;
/**
* @brief 移动指定的距离
*
* @param distance
* @return float
*/
float move_distance(float distance) {
status_ = example_ros2_interfaces::msg::RobotStatus::STATUS_MOVEING;
target_pose_ += distance;
// 当目标距离和当前距离大于0.01则持续向目标移动
while (fabs(target_pose_ - current_pose_) > 0.01) {
// 每一步移动当前到目标距离的1/10
float step = distance / fabs(distance) * fabs(target_pose_ - current_pose_) * 0.1;
current_pose_ += step;
std::cout << "移动了：" << step << "当前位置：" << current_pose_ << std::endl;
// 当前线程休眠500ms
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
status_ = example_ros2_interfaces::msg::RobotStatus::STATUS_STOP;
return current_pose_;
}
/**
* @brief Get the current pose
*
* @return float
*/
float get_current_pose() { return current_pose_; }
/**
* @brief Get the status
*
* @return int
* 1 example_ros2_interfaces::msg::RobotStatus::STATUS_MOVEING
* 2 example_ros2_interfaces::msg::RobotStatus::STATUS_STOP
*/
int get_status() { return status_; }
private:
// 声明当前位置
float current_pose_ = 0.0;
// 目标距离
float target_pose_ = 0.0;
int status_ = example_ros2_interfaces::msg::RobotStatus::STATUS_STOP;
};
class ExampleInterfacesRobot : public rclcpp::Node {
public:
ExampleInterfacesRobot(std::string name) : Node(name) {
RCLCPP_INFO(this->get_logger(), "节点已启动：%s.", name.c_str());
/*创建move_robot服务*/
move_robot_server_ = this->create_service<example_ros2_interfaces::srv::MoveRobot>(
"move_robot", std::bind(&ExampleInterfacesRobot::handle_move_robot, this, std::placeholders::_1, std::placeholders::_2));
/*创建发布者*/
robot_status_publisher_ = this->create_publisher<example_ros2_interfaces::msg::RobotStatus>("robot_status", 10);
/*创建一个周期为500ms的定时器*/
timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&ExampleInterfacesRobot::timer_callback, this));
}
private:
Robot robot;
rclcpp::TimerBase::SharedPtr timer_; /*定时器，用于定时发布机器人位置*/
rclcpp::Service<example_ros2_interfaces::srv::MoveRobot>::SharedPtr move_robot_server_; /*移动机器人服务*/
rclcpp::Publisher<example_ros2_interfaces::msg::RobotStatus>::SharedPtr robot_status_publisher_; /*发布机器人位姿发布者*/
/**
* @brief 500ms 定时回调函数，
*
*/
void timer_callback() {
// 创建消息
example_ros2_interfaces::msg::RobotStatus message;
message.status = robot.get_status();
message.pose = robot.get_current_pose();
RCLCPP_INFO(this->get_logger(), "Publishing: %f", robot.get_current_pose());
// 发布消息
robot_status_publisher_->publish(message);
};
/**
* @brief 收到话题数据的回调函数
*
* @param request 请求共享指针，包含移动距离
* @param response 响应的共享指针，包含当前位置信息
*/
void handle_move_robot(const std::shared_ptr<example_ros2_interfaces::srv::MoveRobot::Request> request,
std::shared_ptr<example_ros2_interfaces::srv::MoveRobot::Response> response) {
RCLCPP_INFO(this->get_logger(), "收到请求移动距离：%f，当前位置：%f", request->distance, robot.get_current_pose());
robot.move_distance(request->distance);
response->pose = robot.get_current_pose();
};
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<ExampleInterfacesRobot>("example_interfaces_robot_01");
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
colcon build --packages-up-to example_interfaces_rclcpp
新的指令--packages-up-to
以先后顺序编译了example_ros2_interfaces再编译example_interfaces_rclcp
4. 测试
服务端
source install/setup.bash
ros2 run example_interfaces_rclcpp example_interfaces_robot_01
控制端
source install/setup.bash
ros2 run example_interfaces_rclcpp example_interfaces_control_01
root@ubuntu-16:~/stephen/chapt3/chapt3_ws# ros2 run example_interfaces_rclcpp example_interfaces_control_01
[INFO] [1749525941.331448781] [example_interfaces_control_01]: 节点已启动：example_interfaces_control_01.
[INFO] [1749525941.333860674] [example_interfaces_control_01]: 请求让机器人移动5.000000
[INFO] [1749525971.234824519] [example_interfaces_control_01]: 收到移动结果：4.990017
[INFO] [1749525971.235004400] [example_interfaces_control_01]: 收到状态数据位置：4.990017 状态：1
[INFO] [1749525971.319335747] [example_interfaces_control_01]: 收到状态数据位置：4.990017 状态：1
[INFO] [1749525971.819438561] [example_interfaces_control_01]: 收到状态数据位置：4.990017 状态：1
[INFO] [1749525972.319419817] [example_interfaces_control_01]: 收到状态数据位置：4.990017 状态：1
[INFO] [1749525972.819410540] [example_interfaces_control_01]: 收到状态数据位置：4.990017 状态：1
[INFO] [1749525973.319369775] [example_interfaces_control_01]: 收到状态数据位置：4.990017 状态：1
[INFO] [1749525973.819398505] [example_interfaces_control_01]: 收到状态数据位置：4.990017 状态：1
[INFO] [1749525974.319351485] [example_interfaces_control_01]: 收到状态数据位置：4.990017 状态：1
[INFO] [1749525974.819402220] [example_interfaces_control_01]: 收到状态数据位置：4.990017 状态：1
root@ubuntu-16:~/stephen/chapt3/chapt3_ws# ros2 run example_interfaces_rclcpp example_interfaces_robot_01
[INFO] [1749525886.318071988] [example_interfaces_robot_01]: 节点已启动：example_interfaces_robot_01.
[INFO] [1749525941.729298188] [example_interfaces_robot_01]: 收到请求移动距离：5.000000，当前位置：0.000000
移动了：0.5当前位置：0.5
移动了：0.00136943当前位置：4.98768
移动了：0.00123248当前位置：4.98891
移动了：0.00110922当前位置：4.99002
[INFO] [1749525971.234663830] [example_interfaces_robot_01]: Publishing: 4.990017
[INFO] [1749525971.319224772] [example_interfaces_robot_01]: Publishing: 4.990017
[INFO] [1749525971.819224244] [example_interfaces_robot_01]: Publishing: 4.990017
[INFO] [1749525972.319216399] [example_interfaces_robot_01]: Publishing: 4.990017
[INFO] [1749525972.819215212] [example_interfaces_robot_01]: Publishing: 4.990017
[INFO] [1749525973.319157594] [example_interfaces_robot_01]: Publishing: 4.990017
[INFO] [1749525973.819201846] [example_interfaces_robot_01]: Publishing: 4.990017
[INFO] [1749525974.319194787] [example_interfaces_robot_01]: Publishing: 4.990017
[INFO] [1749525974.819202810] [example_interfaces_robot_01]: Publishing: 4.990017
[INFO] [1749525975.319181540] [example_interfaces_robot_01]: Publishing: 4.990017
py实现
虽然机器人可以移动了，客户端也可以收到机器人的位置了，但是你应该发现了
机器人移动期间，控制端就收不到了来自机器人端的实时位置信息的话题发布???
服务端调用机器人移动的时候造成了主线程的阻塞和休眠，只有机器人完成移动后才会退出，造成了发布数据的定时器回调无法正常进行。
解决这个问题的方法有很多，比如开个单独给服务开个线程，比如换一种通信方式，带着问题和好奇心，继续学习
上一节使用RCLCPP的API通过自定义接口实现控制节点和机器人节点之间的话题与服务通信。
本节以RCLPY客户端库为例，讲解实现方法。
编写2个节点
example_interfaces_robot_02，机器人节点，对外提供控制机器人移动服务并发布机器人的状态。
example_interfaces_control_02，控制节点，发送机器人移动请求，订阅机器人状态话题。
cd chapt3_ws/
ros2 pkg create example_interfaces_rclpy --build-type ament_python --dependencies rclpy example_ros2_interfaces --destination-directory src --node-name example_interfaces_robot_02 --maintainer-name "fishros" --maintainer-email "fishros@foxmail.com"
touch src/example_interfaces_rclpy/example_interfaces_rclpy/example_interfaces_control_02.py
setup.py
maintainer='fishros',
maintainer_email='fishros@foxmail.com',
entry_points={
'console_scripts': [
'example_interfaces_control_02 = example_interfaces_rclpy.example_interfaces_control_02:main',
'example_interfaces_robot_02 = example_interfaces_rclpy.example_interfaces_robot_02:main'
],
},
example_interfaces_robot_02.py
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
class Robot():
def __init__(self) -> None:
pass
class ExampleInterfacesRobot02(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info("节点已启动：%s!" % name)
def main(args=None):
rclpy.init(args=args) # 初始化rclpy
node = ExampleInterfacesRobot02("example_interfaces_robot_02") # 新建一个节点
rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
rclpy.shutdown() # 关闭rclpy
example_interfaces_control_02.py
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
class ExampleInterfacesControl02(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info("节点已启动：%s!" % name)
def main(args=None):
rclpy.init(args=args) # 初始化rclpy
node = ExampleInterfacesControl02("example_interfaces_control_02") # 新建一个节点
rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
rclpy.shutdown() # 关闭rclpy
测试
# 新终端
colcon build --packages-up-to example_interfaces_rclpy
source install/setup.bash
ros2 run example_interfaces_rclpy example_interfaces_robot_02
# 新终端
source install/setup.bash
ros2 run example_interfaces_rclpy example_interfaces_control_02
继续编写代码
root@ubuntu-16:~/stephen/chapt3/chapt3_ws# source install/setup.bash
ros2 run example_interfaces_rclpy example_interfaces_robot_02
[INFO] [1749527457.482657895] [example_interfaces_robot_02]: 节点已启动：example_interfaces_robot_02!
[INFO] [1749527468.487181129] [example_interfaces_robot_02]: 发布了当前的状态：1 位置：0.0
[INFO] [1749527468.987146895] [example_interfaces_robot_02]: 发布了当前的状态：1 位置：0.0
[INFO] [1749527469.487225748] [example_interfaces_robot_02]: 发布了当前的状态：1 位置：0.0
[INFO] [1749527469.987153475] [example_interfaces_robot_02]: 发布了当前的状态：1 位置：0.0
[INFO] [1749527470.487251192] [example_interfaces_robot_02]: 发布了当前的状态：1 位置：0.0
移动了：0.5当前位置：0.5
移动了：0.45当前位置：0.95
移动了：0.405当前位置：1.355
移动了：0.36450000000000005当前位置：1.7195
移动了：0.32805当前位置：2.04755
移动了：0.295245当前位置：2.342795
移动了：0.26572049999999997当前位置：2.6085155
移动了：0.23914844999999998当前位置：2.8476639500000003
移动了：0.215233605当前位置：3.062897555
移动了：0.1937102445当前位置：3.2566077995000002
移动了：0.17433922005当前位置：3.43094701955
移动了：0.156905298045当前位置：3.587852317595
移动了：0.14121476824050003当前位置：3.7290670858354997
移动了：0.002319198843294057当前位置：4.979127210410353
移动了：0.0020872789589646778当前位置：4.981214489369318
移动了：0.0018785510630682013当前位置：4.9830930404323865
移动了：0.0016906959567613456当前位置：4.984783736389148
移动了：0.0015216263610851933当前位置：4.986305362750233
移动了：0.0013694637249766829当前位置：4.98767482647521
移动了：0.001232517352478979当前位置：4.988907343827689
移动了：0.001109265617231081当前位置：4.99001660944492
[INFO] [1749527500.282982929] [example_interfaces_robot_02]: 发布了当前的状态：1 位置：4.99001660944492
[INFO] [1749527500.487335034] [example_interfaces_robot_02]: 发布了当前的状态：1 位置：4.99001660944492
[INFO] [1749527500.987334437] [example_interfaces_robot_02]: 发布了当前的状态：1 位置：4.99001660944492
[INFO] [1749527501.487550962] [example_interfaces_robot_02]: 发布了当前的状态：1 位置：4.99001660944492
[INFO] [1749527501.987470150] [example_interfaces_robot_02]: 发布了当前的状态：1 位置：4.99001660944492
[INFO] [1749527502.487454332] [example_interfaces_robot_02]: 发布了当前的状态：1 位置：4.99001660944492
root@ubuntu-16:~/stephen/chapt3/chapt3_ws# source install/setup.bash
ros2 run example_interfaces_rclpy example_interfaces_control_02
[INFO] [1749527470.493230762] [example_interfaces_control_02]: 节点已启动：example_interfaces_control_02!
[INFO] [1749527470.747726260] [example_interfaces_control_02]: 请求服务让机器人移动5.0
[INFO] [1749527500.283132715] [example_interfaces_control_02]: 收到返回结果：4.990016460418701
[INFO] [1749527500.284998111] [example_interfaces_control_02]: 收到状态数据位置：4.990016460418701 状态：1
[INFO] [1749527500.487467391] [example_interfaces_control_02]: 收到状态数据位置：4.990016460418701 状态：1
[INFO] [1749527500.987433670] [example_interfaces_control_02]: 收到状态数据位置：4.990016460418701 状态：1
[INFO] [1749527501.487808968] [example_interfaces_control_02]: 收到状态数据位置：4.990016460418701 状态：1
[INFO] [1749527501.987914239] [example_interfaces_control_02]: 收到状态数据位置：4.990016460418701 状态：1
[INFO] [1749527502.487641754] [example_interfaces_control_02]: 收到状态数据位置：4.990016460418701 状态：1
cat src/example_interfaces_rclpy/example_interfaces_rclpy/example_interfaces_robot_02.py
#!/usr/bin/env python3
from example_ros2_interfaces.msg import RobotStatus
import math
from time import sleep
import rclpy
from rclpy.node import Node
class Robot():
def __init__(self) -> None:
self.current_pose_ = 0.0
self.target_pose_ = 0.0
self.status_ = RobotStatus.STATUS_STOP
def get_status(self):
return self.status_
def get_current_pose(self):
return self.current_pose_
def move_distance(self,distance):
self.status_ = RobotStatus.STATUS_MOVEING # 更新状态为移动、
self.target_pose_ += distance # 更新目标位置
while math.fabs(self.target_pose_ - self.current_pose_) > 0.01:
step = distance / math.fabs(distance) * math.fabs(self.target_pose_ - self.current_pose_) * 0.1 # 计算一步移动距离
self.current_pose_ += step # 移动一步
print(f"移动了：{step}当前位置：{self.current_pose_}")
sleep(0.5) #休息0.5s
self.status_ = RobotStatus.STATUS_STOP # 更新状态为停止
return self.current_pose_
from example_ros2_interfaces.srv import MoveRobot
class ExampleInterfacesRobot02(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info("节点已启动：%s!" % name)
self.robot = Robot()
self.move_robot_server_ = self.create_service(MoveRobot,"move_robot", self.handle_move_robot)
self.robot_status_publisher_ = self.create_publisher(RobotStatus,"robot_status", 10)
self.publisher_timer_ = self.create_timer(0.5, self.publisher_timer_callback)
def publisher_timer_callback(self):
"""
定时器回调发布数据函数
"""
msg = RobotStatus() #构造消息
msg.status = self.robot.get_status()
msg.pose = self.robot.get_current_pose()
self.robot_status_publisher_.publish(msg) # 发布消息
self.get_logger().info(f'发布了当前的状态：{msg.status} 位置：{msg.pose}')
def handle_move_robot(self,request, response):
self.robot.move_distance(request.distance)
response.pose = self.robot.get_current_pose()
return response
def main(args=None):
rclpy.init(args=args) # 初始化rclpy
node = ExampleInterfacesRobot02("example_interfaces_robot_02") # 新建一个节点
rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
rclpy.shutdown() # 关闭rclpy
cat src/example_interfaces_rclpy/example_interfaces_rclpy/example_interfaces_control_02.py
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from example_ros2_interfaces.msg import RobotStatus
from example_ros2_interfaces.srv import MoveRobot
class ExampleInterfacesControl02(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info("节点已启动：%s!" % name)
self.client_ = self.create_client(MoveRobot,"move_robot")
self.robot_status_subscribe_ = self.create_subscription(RobotStatus,"robot_status",self.robot_status_callback,10)
def robot_status_callback(self,msg):
self.get_logger().info(f"收到状态数据位置：{msg.pose} 状态：{msg.status}")
def move_result_callback_(self, result_future):
response = result_future.result()
self.get_logger().info(f"收到返回结果：{response.pose}")
def move_robot(self, distance):
while rclpy.ok() and self.client_.wait_for_service(1)==False:
self.get_logger().info(f"等待服务端上线....")
request = MoveRobot.Request()
request.distance = distance
self.get_logger().info(f"请求服务让机器人移动{distance}")
self.client_.call_async(request).add_done_callback(self.move_result_callback_)
def main(args=None):
rclpy.init(args=args) # 初始化rclpy
node = ExampleInterfacesControl02("example_interfaces_control_02") # 新建一个节点
node.move_robot(5.0) #移动5米
rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
rclpy.shutdown() # 关闭rclpy
运行
# 新终端
colcon build --packages-up-to example_interfaces_rclpy
source install/setup.bash
ros2 run example_interfaces_rclpy example_interfaces_robot_02
# 新终端
source install/setup.bash
ros2 run example_interfaces_rclpy example_interfaces_control_02
浙公网安备 33010602011771号