ROS2贪吃龟练习工程

news2024/11/13 3:34:49

本文是ROS2基础知识的综合小应用,练习如何创建工作包,创建Node,定义Topic和Service,以及通过LaunchFile启动多个节点。基础知识可以参考:ROS2基础编程,ROS2 Topics和Services,ROS2 LaunchFile和Parameter
更多内容,访问专栏目录获取实时更新。

创建工作包

ros2 pkg create turtlesim_greedy_turtle --build-type ament_python

创建节点

touch turtle_controller.py
touch turtle_spawner.py
chmod +x turtle_controller.py
chmod +x turtle_spawner.py

添加依赖

<depend>rclpy</depend>
<depend>turtlesim</depend>
<depend>geometry_msgs</depend>
<depend>my_robot_interfaces</depend>

创建Msg
Turtle.msg

string name
float64 x
float64 y
float64 theta

TurtleArray.msg

Turtle[] turtles

创建Srv
CatchTurtle.srv

string name
---
bool success

添加节点控制Turtle
turtle_controller.py

#!/usr/bin/env python3
import rclpy
import math
from functools import partial
from rclpy.node import Node
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from my_robot_interfaces.msg import Turtle
from my_robot_interfaces.msg import TurtleArray
from my_robot_interfaces.srv import CatchTurtle


class TurtleControllerNode(Node):
  def __init__(self):
    super().__init__("turtle_controller")

    self.pose = None
    self.turtle_to_catch = None

    self.pose_subscriber = self.create_subscription(Pose, "turtle1/pose", self.callback_turtle_pose, 10)
    self.cmd_vel_publisher = self.create_publisher(Twist, "turtle1/cmd_vel", 10)
    self.alive_turtles_subscriber = self.create_subscription(TurtleArray, "alive_turtles", self.callback_alive_turtles, 10)

    self.control_loop_timer = self.create_timer(0.01, self.control_loop)
  
  def callback_turtle_pose(self, msg):
    self.pose = msg

  def control_loop(self):
    if self.pose == None or self.turtle_to_catch == None:
      return
    # calcu the distance between current position and target position
    dist_x = self.turtle_to_catch.x - self.pose.x
    dist_y = self.turtle_to_catch.y - self.pose.y
    distance  = math.sqrt(dist_x * dist_x + dist_y * dist_y)
    # construct cmd_vel msg to control the turtle
    msg = Twist()
    if distance <= 0.5:  # target reached
      msg.linear.x = 0.0   # stop the turtle
      msg.angular.z = 0.0
      self.call_catch_turtle_server(self.turtle_to_catch.name)
      self.turtle_to_catch = None
    else:                # move to the target position
      msg.linear.x = 2 * distance  # optimize
      goal_theta = math.atan2(dist_y, dist_x)  # orientation
      diff = goal_theta - self.pose.theta
      if diff > math.pi:
        diff -= 2 * math.pi
      elif diff < -math.pi:
        diff += 2 * math.pi
      msg.angular.z = 6 * diff  # optimize

    self.cmd_vel_publisher.publish(msg)

  def callback_alive_turtles(self, msg):
    if len(msg.turtles) > 0:
      self.turtle_to_catch = msg.turtles[0]

  def call_catch_turtle_server(self, turtle_name):
    client = self.create_client(CatchTurtle, "catch_turtle")
    while not client.wait_for_service(1.0):
      self.get_logger().warn("Waiting for server...")
    
    request = CatchTurtle.Request()
    request.name = turtle_name
    future = client.call_async(request)
    future.add_done_callback(partial(self.callback_call_catch_turtle, turtle_name=turtle_name))

  def callback_call_catch_turtle(self, future, turtle_name):
    try:
      response = future.result()
      if not response.success:
        self.get_logger().error("Turtle " + str(turtle_name) + " could not be caught")
    except Exception as e:
      self.get_logger().error("Service call failed %r" & (e,))
  

def main(args=None):
  rclpy.init(args=args)
  node = TurtleControllerNode()
  rclpy.spin(node)
  rclpy.shutdown()


if __name__ == "__main__":
  main()

添加节点产生或移除Turtle

#!/usr/bin/env python3
import rclpy
import random
import math
from functools import partial
from rclpy.node import Node
from turtlesim.srv import Spawn
from turtlesim.srv import Kill
from my_robot_interfaces.msg import Turtle
from my_robot_interfaces.msg import TurtleArray
from my_robot_interfaces.srv import CatchTurtle


class TurtleSpawnerNode(Node):
 def __init__(self):
    super().__init__("turtle_spawner")
    self.declare_parameter("spawn_frequency", 1.0)
    self.declare_parameter("turtle_name_prefix", "turtle")

    self.spawn_frequency = self.get_parameter("spawn_frequency").value
    self.turtle_name_prefix = self.get_parameter("turtle_name_prefix").value
    self.turtle_counter = 0
    self.alive_turtles = []

    self.spawn_turtle_timer = self.create_timer(1.0 / self.spawn_frequency, self.spawn_new_turtle)
    self.alive_turtles_publisher = self.create_publisher(TurtleArray, "alive_turtles", 10)
    self.catch_turtle_service = self.create_service(CatchTurtle, "catch_turtle", self.callback_catch_turtle)

  def spawn_new_turtle(self):
    self.turtle_counter += 1
    name = self.turtle_name_prefix + "_" + str(self.turtle_counter)
    x = random.uniform(0.0, 10.0)
    y = random.uniform(0.0, 10.0)
    theta = random.uniform(0.0, 2 * math.pi)
    self.call_spawn_server(name, x, y, theta)
  
  def call_spawn_server(self, turtle_name, x, y, theta):
    client = self.create_client(Spawn, "spawn")
    while not client.wait_for_service(1.0):
      self.get_logger().warn("Waiting for server...")
    
    request = Spawn.Request()
    request.name = turtle_name
    request.x = x
    request.y = y
    request.theta = theta

    future = client.call_async(request)
    future.add_done_callback(partial(self.callback_call_spawn, turtle_name=turtle_name, x=x, y=y, theta=theta))

  def callback_call_spawn(self, future, turtle_name, x, y, theta):
    try:
      response = future.result()
      if response.name != "":
        self.get_logger().info("Turtle " + response.name + " is now alive")
        new_turtle = Turtle()
        new_turtle.name = turtle_name
        new_turtle.x = x
        new_turtle.y = y
        new_turtle.theta = theta
        self.alive_turtles.append(new_turtle)
        self.publish_alive_turtles()
    except Exception as e:
      self.get_logger().error("Service call failed %r" % (e,))

  def publish_alive_turtles(self):
    msg = TurtleArray()
    msg.turtles = self.alive_turtles
    self.alive_turtles_publisher.publish(msg)

  def callback_catch_turtle(self, request, response):
    self.call_kill_server(request.name)
    response.success = True
    return response

  def call_kill_server(self, turtle_name):
    client = self.create_client(Kill, "kill")
    while not client.wait_for_service(1.0):
      self.get_logger().warn("Waiting for server...")
    
    request = Kill.Request()
    request.name = turtle_name
    future = client.call_async(request)
    future.add_done_callback(partial(self.callback_call_kill, turtle_name=turtle_name))

  def callback_call_kill(self, future, turtle_name):
    try:
      future.result()
      for (i, turtle) in enumerate(self.alive_turtles):
        if turtle.name == turtle_name:
          del self.alive_turtles[i]
          self.publish_alive_turtles()
          break
    except Exception as e:
      self.get_logger().error("Service call failed %r" & (e,))


def main(args=None):
  rclpy.init(args=args)
  node = TurtleSpawnerNode()
  rclpy.spin(node)
  rclpy.shutdown()


if __name__ == "__main__":
  main()

创建LaunchFile
在这里插入图片描述

不要忘记更新my_robot_bringup工作包的package.xml添加依赖

通过LaunchFile启动

ros2 launch my_robot_bringup turtlesim_greedy_turtle.launch.py

最终的效果:
在这里插入图片描述

如有错误,欢迎留言或来信指正:hbin6358@163.com

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/1714475.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

模拟集成电路(5)----单级放大器(共栅级)

模拟集成电路(5)----单级放大器&#xff08;共栅级&#xff09; 有一些场合需要一些小的输入电阻&#xff08;电流放大器&#xff09; 大信号分析 − W h e n V i n ≥ V B − V T H ∙ M 1 i s o f f , V o u t V D D − F o r L o w e r V i n I d 1 2 μ n C o x W L ( V…

matlab安装及破解

一、如何下载 软件下载链接&#xff0c;密码&#xff1a;98ai 本来我想自己生成一个永久百度网盘链接的&#xff0c;但是&#xff1a; 等不住了&#xff0c;所以大家就用上面的链接吧。 二、下载花絮 百度网盘下载速度比上载速度还慢&#xff0c;我给充了个会员&#xff0c…

java调用远程接口下载文件

在postman中这样下载文件 有时下载文件太大postman会闪退&#xff0c;可以通过代码下载&#xff0c;使用hutool的http包

中华活页文选高中版投稿发表

《中华活页文选&#xff08;高中版&#xff09;》创刊于1960年&#xff0c;是中宣部所属中国出版传媒股份有限公司主管、中华书局主办的国家级基础教育期刊&#xff0c;曾获得“中国期刊方阵双效期刊”、国家新闻出版广电总局推荐的“百种优秀报刊”等荣誉称号。本刊以高中学科…

WAMP无法启动mysql

一种原因是原来安装过mysql,mysql默认是自启动的&#xff0c;而WAMP内置mysql会发生冲突&#xff0c;所以 解决方法&#xff1a; winR 输入 services.msc 将mysql关闭&#xff0c;并设为手动模式

扒出秦L三个槽点,我不考虑买它了

文 | Auto芯球 作者 | 雷慢 比亚迪的有一个王炸“秦L”&#xff0c;再一次吸引了我注意力&#xff0c; 我上一辆车刚卖不久&#xff0c;最近打算买第二辆车&#xff0c; 二手车和新车都有在看&#xff0c; 我又是一个坚定的实用主义者&#xff0c; 特别是现在的经济环境不…

深入解析 JSONPath:从入门到精通

码到三十五 &#xff1a; 个人主页 在数据处理和交换领域&#xff0c;JSON已经成为了一种广泛使用的数据格式&#xff0c; 如何有效地查询和操作这些数据也变得越来越重要。在这种情况下&#xff0c;JSONPath 应运而生&#xff0c;成为了一种在JSON数据中定位和提取信息的强大工…

老师如何对付挑事儿的家长?

身为老师&#xff0c;你有没有遇到过这样的家长&#xff1a;孩子在学校里闹点小矛盾&#xff0c;或者作业分数有点争议&#xff0c;他们就气势汹汹地来找你&#xff0c;说你偏心&#xff0c;甚至在其他家长面前说三道四&#xff1f;面对这种爱“挑事”的家长&#xff0c;老师们…

Nacos 2.x 系列【12】配置加密插件

文章目录 1. 前言2. 安装插件2.1 编译2.2 客户端2.3 服务端 3. 测试 1. 前言 为保证用户敏感配置数据的安全&#xff0c;Nacos提供了配置加密的新特性。降低了用户使用的风险&#xff0c;也不需要再对配置进行单独的加密处理。 前提条件&#xff1a; 版本:老版本暂时不兼容&…

AI在肿瘤学临床决策中的应用:一种多模态方法

在临床肿瘤学领域&#xff0c;多模态人工智能&#xff08;AI&#xff09;系统通过解读各类医学数据&#xff0c;展现出提升临床决策的潜力。然而&#xff0c;这些模型在所有医学领域中的有效性尚未确定。本文介绍了一种新型的多模态医疗AI方法&#xff0c;该方法利用大型语言模…

maven自建的两个模块怎么相互引用

【背景】 我们平时用Maven仓库都是引用外部别人写好的jar包&#xff0c;今天我需要自己在同一个Project下&#xff0c;在一个模块引用另一个模块中的类。案例展示如下&#xff1a; 【操作】 每个模块都有个自己的pom.xml文件&#xff0c;项目下也有个自己的pom.xml文件&#…

20240529瑞芯微官方Toybrick TB-RK3588开发板的Debian11安装iperf并测试网速

20240529瑞芯微官方Toybrick TB-RK3588开发板的Debian11安装iperf并测试网速 2024/5/29 15:09 服务器端&#xff1a;瑞芯微官方Toybrick TB-RK3588开发板&#xff1a;Debian11 客户端&#xff1a;笔记本电脑&#xff1a;Ubuntu20.04 两者通过千兆交换机连接&#xff01; toybr…

使用Java Swing制作一个飞翔的小鸟游戏

文章目录 一、需求分析二、技术介绍2.1相关技术2.2开发环境 三、功能实现1、开始2、运动3、死亡 四、部分代码实现获取源码 文章最下方获取源码&#xff01;&#xff01;&#xff01; 文章最下方获取源码&#xff01;&#xff01;&#xff01; 文章最下方获取源码&#xff01;&…

芝加哥大学最新研究:GPT-4与财务预测,重塑财务分析的未来

最近&#xff0c;芝加哥大学的研究团队发表了一篇突破性的研究&#xff0c;展示了大型语言模型&#xff08;LLM&#xff09;&#xff0c;特别是 OpenAI 开发的 GPT-4&#xff0c;如何在财务报表分析领域取得了与专业分析师相匹配甚至超越的表现。这项研究不仅凸显了人工智能在高…

系统与软件工程软件测试过程

系统与软件工程 软件测试 测试过程 &#xff1b;对应的国标是GB/T 38634.4 2020 &#xff0c;该标准的范围规定适应用于治理、管理和实施任何组织,项目或较小规模测试活动的软件测试的测试过程,定义了软件测试通用过程,给出了描述过程的支持信息图表。 一 术语和定义 1.1实测…

《庆余年算法番外篇》:范闲通过最短路径算法在阻止黑骑截杀林相

剧情背景 在《庆余年 2》22集中,林相跟大宝交代完为人处世的人生哲理之后,就要跟大宝告别了 在《庆余年 2》23集中,林相在告老还乡的路上与婉儿和大宝告别后 范闲也在与婉儿的对话中知道黑骑调动是绝密,并把最近一次告老还乡梅执礼被马匪截杀与黑骑调动日期关联在一起,…

B/S架构+java语言+Mysqladr数 据 库ADR药物不良反应监测系统源码 ADR药物不良反应监测系统有哪些作用?

B/S架构&#xff0b;java语言&#xff0b;Mysqladr数 据 库ADR药物不良反应监测系统源码 ADR药物不良反应监测系统有哪些作用&#xff1f; 药物不良反应(ADR)是指在合格药物以正常用量和用法用于预防、诊断、治疗疾病或调节生理功能时所发生的意外的、与防治目的无关的、不利或…

系统架构设计师【第2章】: 计算机系统基础知识 (核心总结)

文章目录 2.1 计算机系统概述2.2 计算机硬件2.2.1 计算机硬件组成2.2.2 处理器2.2.3 存储器2.2.4 总线2.2.5 接口2.2.6 外部设备 2.3 计算机软件2.3.1 计算机软件概述2.3.2 操作系统2.3.3 数据库2.3.4 文件系统2.3.5 网络协议2.3.6 中间件2.3.7 软件构件2.3.8 …

C++语言·list链表(下)

还是之前说的&#xff0c;因为要写模板&#xff0c;为了避免链接出现问题&#xff0c;我们将所有内容都写到一个文件中去。首先就是画出链表的框架 链表本身只需要一个头节点就足以找到整条链表&#xff0c;而需要它拼接的节点我们再写一个模板。而我们知道list是一个带头双向循…

JAVA云HIS医院管理系统源码 云HIS系统源码 云HIS的优势 云HIS的发展

JAVA云HIS医院管理系统源码 云HIS系统源码 云HIS的优势 云HIS的发展 HIS系统&#xff0c;即医院信息系统&#xff08;Hospital Information System&#xff09;&#xff0c;在医院的运营和管理中扮演着至关重要的角色。关于HIS系统的溯源&#xff0c;简单从以下几个方面进行讲…