小乌龟控制实现
1. 创建package¶
ros2 pkg create --build-type ament_python turtlesim_servie
可以修改
package.xml文件如下信息<description>C++ client server tutorial</description> <maintainer email="you@email.com">Your Name</maintainer> <license>Apache License 2.0</license>
2. 编写代码¶
2.1 节点代码¶
- 在package的
turtlesim_service文件夹下创建turtlesim_service.py文件,代码如下
import sys
from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QHBoxLayout, QPushButton, QLineEdit, QCheckBox
import rclpy
from rclpy.node import Node
from std_srvs.srv import Empty
from turtlesim.srv import Kill, Spawn, SetPen, TeleportAbsolute, TeleportRelative
class MainWindow(QWidget):
def __init__(self, node):
super(MainWindow, self).__init__()
self.node = node
self.clear_ser = self.node.create_client(Empty, '/clear')
self.reset_ser = self.node.create_client(Empty, '/reset')
self.spawn_ser = self.node.create_client(Spawn, '/spawn')
self.kill_ser = self.node.create_client(Kill, '/kill')
layout = QVBoxLayout()
self.setLayout(layout)
# part1
clear_btn = QPushButton('清理画布')
# part2
reset_btn = QPushButton('重置')
# part3
layout1 = QHBoxLayout()
self.create_x = QLineEdit()
self.create_y = QLineEdit()
self.create_theta = QLineEdit()
self.create_name = QLineEdit()
self.create_x.setPlaceholderText('x坐标')
self.create_y.setPlaceholderText('y坐标')
self.create_theta.setPlaceholderText('角度')
self.create_name.setPlaceholderText('名字')
create_btn = QPushButton('创建小乌龟')
layout1.addWidget(self.create_x)
layout1.addWidget(self.create_y)
layout1.addWidget(self.create_theta)
layout1.addWidget(self.create_name)
layout1.addWidget(create_btn)
# part4
layout2 = QHBoxLayout()
self.kill_name = QLineEdit()
self.kill_name.setPlaceholderText('名字')
kill_btn = QPushButton('杀死小乌龟')
layout2.addWidget(self.kill_name)
layout2.addWidget(kill_btn)
# part5
layout3 = QHBoxLayout()
self.pen_name = QLineEdit()
self.pen_red = QLineEdit()
self.pen_green = QLineEdit()
self.pen_blue = QLineEdit()
self.pen_width = QLineEdit()
self.pen_close = QCheckBox('关闭')
self.pen_name.setPlaceholderText('名字')
self.pen_red.setPlaceholderText('红')
self.pen_green.setPlaceholderText('绿')
self.pen_blue.setPlaceholderText('蓝')
self.pen_width.setPlaceholderText('粗细')
pen_btn = QPushButton('设置画笔')
layout3.addWidget(self.pen_name)
layout3.addWidget(self.pen_red)
layout3.addWidget(self.pen_green)
layout3.addWidget(self.pen_blue)
layout3.addWidget(self.pen_width)
layout3.addWidget(self.pen_close)
layout3.addWidget(pen_btn)
# part6
layout4 = QHBoxLayout()
self.abs_name = QLineEdit()
self.abs_x = QLineEdit()
self.abs_y = QLineEdit()
self.abs_theta = QLineEdit()
self.abs_x.setPlaceholderText('x坐标')
self.abs_y.setPlaceholderText('y坐标')
self.abs_theta.setPlaceholderText('角度')
self.abs_name.setPlaceholderText('名字')
abs_btn = QPushButton('设置绝对位置')
layout4.addWidget(self.abs_name)
layout4.addWidget(self.abs_x)
layout4.addWidget(self.abs_y)
layout4.addWidget(self.abs_theta)
layout4.addWidget(self.abs_theta)
layout4.addWidget(abs_btn)
# part7
layout5 = QHBoxLayout()
self.rel_name = QLineEdit()
self.rel_linear = QLineEdit()
self.rel_angular = QLineEdit()
self.rel_name.setPlaceholderText('名字')
self.rel_linear.setPlaceholderText('线速度')
self.rel_angular.setPlaceholderText('角速度')
rel_btn = QPushButton('设置相对位置')
layout5.addWidget(self.rel_name)
layout5.addWidget(self.rel_linear)
layout5.addWidget(self.rel_angular)
layout5.addWidget(rel_btn)
# all
layout.addWidget(clear_btn)
layout.addWidget(reset_btn)
layout.addLayout(layout1)
layout.addLayout(layout2)
layout.addLayout(layout3)
layout.addLayout(layout4)
layout.addLayout(layout5)
# click event
clear_btn.clicked.connect(self.clear)
reset_btn.clicked.connect(self.reset)
create_btn.clicked.connect(self.create_turtle)
kill_btn.clicked.connect(self.kill_turtle)
pen_btn.clicked.connect(self.set_pen)
abs_btn.clicked.connect(self.set_abs_pos)
rel_btn.clicked.connect(self.set_rel_pos)
def clear(self):
req = Empty.Request()
self.clear_ser.call_async(req)
def reset(self):
req = Empty.Request()
self.reset_ser.call_async(req)
def create_turtle(self):
req = Spawn.Request()
req.name = self.create_name.text()
req.x = float(self.create_x.text())
req.y = float(self.create_y.text())
req.theta = float(self.create_theta.text())
self.spawn_ser.call_async(req)
def kill_turtle(self):
req = Kill.Request()
req.name = self.create_name.text()
self.kill_ser.call_async(req)
def set_pen(self):
self.set_pen_ser = self.node.create_client(SetPen, f'/{self.pen_name.text()}/set_pen')
req = SetPen.Request()
req.r = int(self.pen_red.text())
req.g = int(self.pen_green.text())
req.b = int(self.pen_blue.text())
req.width = int(self.pen_width.text())
req.off = 1 if self.pen_close.isChecked() else 0
self.set_pen_ser.call_async(req)
def set_abs_pos(self):
self.abs_pos_ser = self.node.create_client(TeleportAbsolute, f'/{self.abs_name.text()}/teleport_absolute')
req = TeleportAbsolute.Request()
req.x = float(self.abs_x.text())
req.y = float(self.abs_y.text())
req.theta = float(self.abs_theta.text())
self.abs_pos_ser.call_async(req)
def set_rel_pos(self):
self.rel_pos_ser = self.node.create_client(TeleportRelative, f'/{self.rel_name.text()}/teleport_relative')
req = TeleportRelative.Request()
req.linear = float(self.rel_linear.text())
req.angular = float(self.rel_angular.text())
self.rel_pos_ser.call_async(req)
def main():
rclpy.init()
node = Node('turtlesim_control')
app = QApplication(sys.argv)
window = MainWindow(node)
window.show()
sys.exit(app.exec_())
if __name__ == '__main__':
main()
2.2 添加配置¶
- 打开
setup.py文件,在console_scripts节点下添加配置
'turtlesim_service = turtlesim_service.turtlesim_service:main',
3. 编译工程¶
colcon build
4. 运行节点¶
source install/setup.bash
ros2 run turtlesim_service turtlesim_service