ROS2 學(xué)習(xí)
ROS（機(jī)器人操作系統(tǒng)）
按照官方案例做了一遍，然后實(shí)現(xiàn)了一個(gè)小烏龜跟隨運(yùn)動(dòng)康聂，并且調(diào)用服務(wù)的案例贰健，算是個(gè)綜合性的練習(xí)吧。(手動(dòng)計(jì)算坐標(biāo)恬汁，沒(méi)有用到tf2相關(guān)模塊）
啥都不說(shuō)伶椿，看效果和代碼。
GIF 2020-9-2 13-56-07.gif
先啟動(dòng)官方的小烏龜
ros2 run turtlesim turtlesim_node
在啟動(dòng)編寫(xiě)的節(jié)點(diǎn)
ros2 run py_pubsub my_test_node2
代碼放在了 py_pubsub中氓侧，代碼目錄結(jié)構(gòu)如下：
└── src
├── py_pubsub
│ ├── launch
│ │ ├── my_test.launch.py
│ ├── package.xml
│ ├── py_pubsub
│ │ ├── __init__.py
│ │ ├── my_test_node2.py
my_test_node2.py 代碼如下所示：
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import math
import random
from turtlesim.srv import Spawn, Kill
class Target:
def __init__(self):
self.name = None
self.is_spawn = False
self.pose = None
class MinimalPublisher(Node):
def __init__(self):
super(MinimalPublisher, self).__init__('my_test_node2')
self.publisher_ = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)
self.subscription_ = self.create_subscription(Pose, 'turtle1/pose', self.subscription_callback, 10)
# 生成海龜 服務(wù)調(diào)用
self.client_spawn = self.create_client(Spawn, 'spawn', )
while not self.client_spawn.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service /spawn not available, waiting again...')
# 生成海龜 服務(wù)調(diào)用
self.client_kill = self.create_client(Kill, 'kill', )
while not self.client_spawn.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service /kill not available, waiting again...')
self.index_spawn = 10 # 生成烏龜序號(hào) turtle10 開(kāi)始 ... turtle 101
self.req_spawn = Spawn.Request()
self.req_kill = Kill.Request()
self.pose_ = Pose() # 第一個(gè)烏龜?shù)牡漠?dāng)前姿勢(shì)
self.msg = Twist() # 第一個(gè)烏龜?shù)牡?速度脊另，需要發(fā)布
self.target: Target = Target() # 第一個(gè)烏龜?shù)牡漠?dāng)前目標(biāo)
# 新生成一個(gè)海龜
self.spawn_target()
# for publish
self.timer = self.create_timer(1 / 100, self.time_callback)
def time_callback(self):
if self.target and self.target.is_spawn:
if math.sqrt(math.fabs((self.pose_.x - self.target.pose.x) ** 2) +
math.fabs((self.pose_.y - self.target.pose.y) ** 2)) < 0.1:
# 如果追到目標(biāo)，服務(wù)調(diào)用kill掉
self.req_kill.name = self.target.name
self.client_kill.call_async(self.req_kill).add_done_callback(
self.future_kill_callback)
self.target = None
else:
# 計(jì)算速度方向约巷，并且發(fā)布
length = math.sqrt(math.fabs((self.target.pose.x - self.pose_.x) ** 2) +
math.fabs((self.target.pose.y - self.pose_.y) ** 2))
speed = 5.0
self.msg.linear.x = (self.target.pose.x - self.pose_.x) / length * speed
self.msg.linear.y = (self.target.pose.y - self.pose_.y) / length * speed
self.publisher_.publish(self.msg)
else:
# 如果沒(méi)有目標(biāo)就不動(dòng)偎痛。
self.msg.linear.x = 0.0
self.msg.linear.y = 0.0
self.publisher_.publish(self.msg)
def subscription_callback(self, pose: Pose):
""" 獲取海龜位置
:param pose:
:return:
"""
self.pose_ = pose
def spawn_target(self):
p = Pose(x=float(random.randint(1, 10)), y=float(random.randint(1, 10)))
self.req_spawn.x = p.x
self.req_spawn.y = p.y
self.req_spawn.name = 'turtle' + str(self.index_spawn)
self.index_spawn += 1
self.target = Target()
self.target.is_spawn = False # 等待異步調(diào)用完成
self.target.name = self.req_spawn.name
self.target.pose = p
self.client_spawn.call_async(self.req_spawn).add_done_callback(self.future_spawn_callback)
def future_spawn_callback(self, args: rclpy.Future):
result: Spawn.Response = args.result()
self.target.is_spawn = True
def future_kill_callback(self, args: rclpy.Future):
self.spawn_target()
def main(args=None):
rclpy.init(args=args)
minimal_publisher = MinimalPublisher()
try:
rclpy.spin(minimal_publisher)
minimal_publisher.destroy_node()
rclpy.shutdown()
except KeyboardInterrupt as e:
print('exit normal .. ')
if __name__ == '__main__':
main()
