四轮转向键盘控制改进版 ros2（python）

目录

  • 写在前面的话
  • 核心代码
    • 键盘输入
    • 发布车子速度和车子转向
    • 发布控制模式
    • 函数调用
  • 完整代码
  • 运行演示

写在前面的话

上一篇博客：键盘控制车子四轮转向，原代码把键盘控制和车轮速度发布绑定到一起了，不适合后续的分布式独立开发，所以打算将键盘控制进行独立出来。

核心代码

键盘输入

符合 linux 系统的键盘监听

import sys, select, termios, tty

self.old_settings = termios.tcgetattr(sys.stdin)

 def getKey(self):
     tty.setraw(sys.stdin.fileno())
     select.select([sys.stdin], [], [], 0)
     key = sys.stdin.read(1)
     termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)
     return key

发布车子速度和车子转向

发布/keyboard_vel_msg话题
采用 Twist 数据类型，需要设置 linear 线速度和 angular 角速度

   from geometry_msgs.msg import Quaternion, TransformStamped, Twist, Vector3
   
   self.vel_pub = self.create_publisher(Twist,'/keyboard_vel_msg', 10)

   self.vel_msg.linear.x = self.x * self.speed
   self.vel_msg.linear.y = self.y * self.speed
   self.vel_msg.angular.x = 0.0 
   self.vel_msg.angular.y = 0.0 
   self.vel_msg.angular.z = self.th * self.turn
   print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.vel_msg.linear.x}) \tturn({self.vel_msg.angular.z})")                
   self.vel_pub.publish(self.vel_msg)

发布控制模式

发布/keyboard_control_mode话题
控制模式有3种，1（反相转弯）、2（同相变向）、3（原地旋转）
采用 Int32 数据类型，赋值要给data属性

   from std_msgs.msg import Int32
   
   self.mode_pub = self.create_publisher(Int32, '/keyboard_control_mode', 10)

   int32_msg = Int32()
   int32_msg.data = self.mode_selection
   self.mode_pub.publish(int32_msg)

函数调用

通过 rclpy.spin 进行 keyboard_subscriber.listener_keyboard() 循环调用

def main(args=None):
    rclpy.init(args=sys.argv)
    
    keyboard_subscriber = Keyboard_subscriber()
    keyboard_subscriber.listener_keyboard()
    rclpy.spin(keyboard_subscriber) 
    rclpy.shutdown()

完整代码

 按键功能提示

	Moving around:
    q   w   e (opposite-phase)
    a   s   d   f   g   h   j (in-phase)
    z       c

    anything else : stop

    f/g : run to left_front/right_front (in-phase)
    h/j : run to left_back/right_back (in-phase)
    ,/. : increase/decrease max speed by 10%
    v/b : increase/decrease only linear speed by 10%
    n/m : increase/decrease only angular speed by 10%

    mode_selection
    0 or other_number: 静止(stop 没速度控制)
    1: 低速小角度转弯(oppostise-phase)
    2: 高速变道(in-phase)需要y轴速度
    3: 原地转弯(pviot-turn)


    CTRL-C to quit

#!/usr/bin/python3
import math
import threading
import rclpy
import numpy as np
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
import sys, select, termios, tty
from nav_msgs.msg import Odometry
import pty
from geometry_msgs.msg import Quaternion, TransformStamped, Twist, Vector3
from nav_msgs.msg import Odometry
from tf2_ros import TransformBroadcaster
from tf2_ros import TransformStamped
import math
from std_msgs.msg import Int32

class Keyboard_subscriber(Node):
    def __init__(self):
        super().__init__('keyboard_subscriber')
        self.msg = """
        Reading from the keyboard  and Publishing to Twist!
        ---------------------------
        注意：r是停止，角速度不能设置太大会导致转弯不稳车身摆动,
            要想实现反相效果需要线速度大于角速度，否则车轮转角会不正确。

        Moving around:
        q   w   e (opposite-phase)
        a   s   d   f   g   h   j (in-phase)
        z       c

        anything else : stop

        f/g : run to left_front/right_front (in-phase)
        h/j : run to left_back/right_back (in-phase)
        ,/. : increase/decrease max speed by 10%
        v/b : increase/decrease only linear speed by 10%
        n/m : increase/decrease only angular speed by 10%

        mode_selection
        0 or other_number: 静止(stop 没速度控制)
        1: 低速小角度转弯(oppostise-phase)
        2: 高速变道(in-phase)需要y轴速度
        3: 原地转弯(pviot-turn)


        CTRL-C to quit
        """
        
        self.moveBindings = {
                'w':(1,0,0,0),
                's':(-1,0,0,0),
                'q':(1,0,0,1),
                'e':(1,0,0,-1),
                'a':(0,0,0,1),
                'd':(0,0,0,-1),
                'f':(1,1,0,0),
                'g':(1,-1,0,0),
                'h':(-1,1,0,0),
                'j':(-1,-1,0,0),                   
                'z':(-1,0,0,1),
                'c':(-1,0,0,-1),
                'r':(0,0,0,0),
            }
        
        self.speedBindings={
                ',':(1.1,1.1),
                '.':(0.9,0.9),
                'v':(1.1,1),
                'b':(0.9,1),
                'n':(1,1.1),
		        'm':(1,.9),
            }
        
        self.modeBindings={
                '1':'opposite-phase',# 低速转弯
                '2':'in-phase',             
                '3':'pivot-turn',#原地旋转
            }

        self.old_settings = termios.tcgetattr(sys.stdin)
        print(self.old_settings)
        self.vel_msg = Twist()  # robot velocity
        self.mode_selection = 1 # 1:opposite phase, 2:in-phase, 3:pivot turn 4: none
        self.speed = 0.5
        self.turn = 0.5 #30度左右
        self.x = 0.0
        self.y = 0.0
        self.th = 0.0
        self.status = 0.0
        self.vel_pub = self.create_publisher(Twist,'/keyboard_vel_msg', 10)
        self.mode_pub = self.create_publisher(Int32, '/keyboard_control_mode', 10)


    def getKey(self):
        tty.setraw(sys.stdin.fileno())
        select.select([sys.stdin], [], [], 0)
        key = sys.stdin.read(1)
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)
        return key
    
    
    def listener_keyboard(self):

        try:
            print(self.msg)
            print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.vel_msg.linear.x}) \tturn({self.vel_msg.angular.z})")
            while True:
                key = self.getKey()#获取按键
                # print(f'Press {key}')
                if key in self.moveBindings.keys():
                    self.x = float(self.moveBindings[key][0])
                    self.y = float(self.moveBindings[key][1])
                    self.th = float(self.moveBindings[key][3])
                    print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.x * self.speed}) \tturn({self.th * self.turn})")

                
                elif key in self.speedBindings.keys():
                    self.speed = self.speed * self.speedBindings[key][0]
                    self.turn = self.turn * self.speedBindings[key][1]
                    print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.x * self.speed}) \tturn({self.th * self.turn})")

                elif key in self.modeBindings.keys():
                    if self.modeBindings[key] == 'opposite-phase':
                        self.mode_selection = 1
                    elif self.modeBindings[key] == 'in-phase':
                        self.mode_selection = 2
                    elif self.modeBindings[key] == 'pivot-turn':
                        self.mode_selection = 3
                    else:
                        self.mode_selection = 0
                    print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.vel_msg.linear.x}) \tturn({self.vel_msg.angular.z})")
                    print(self.msg)
                else:
                    self.x = 0.0
                    self.y = 0.0
                    self.th = 0.0

                    if (key == '\x03'):
                        break

                if (self.status == 8):
                    self.status = 0   
                    print(self.msg)
                # print(self.status)
                self.status = self.status + 1
                
                self.vel_msg.linear.x = self.x * self.speed
                self.vel_msg.linear.y = self.y * self.speed
                self.vel_msg.angular.x = 0.0 
                self.vel_msg.angular.y = 0.0 
                self.vel_msg.angular.z = self.th * self.turn
                
                print(f"currently: mode_selection({self.mode_selection}) \tspeed({self.vel_msg.linear.x}) \tturn({self.vel_msg.angular.z})")                
                self.vel_pub.publish(self.vel_msg)
                int32_msg = Int32()
                int32_msg.data = self.mode_selection
                self.mode_pub.publish(int32_msg)
        
        except Exception as e:
            print('sss',e)
        
        finally:
            self.vel_msg.linear.x = 0.0; 
            self.vel_msg.linear.y = 0.0; 
            self.vel_msg.linear.z = 0.0
            self.vel_msg.angular.x = 0.0; 
            self.vel_msg.angular.y = 0.0; 
            self.vel_msg.angular.z = 0.0
            self.vel_pub.publish(self.vel_msg)
            print(f'{self.mode_selection}')
            int32_msg = Int32()
            int32_msg.data = self.mode_selection
            self.mode_pub.publish(int32_msg)

            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)


def main(args=None):
    rclpy.init(args=sys.argv)
    
    keyboard_subscriber = Keyboard_subscriber()
    keyboard_subscriber.listener_keyboard()
    rclpy.spin(keyboard_subscriber) 
    rclpy.shutdown()

if __name__ == '__main__':
    main()

运行演示

在这里插入图片描述
在这里插入图片描述

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

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

电阻的基本应用

电阻的基本应用

从使用数量的角度来看，电阻在电子元器件中的数量要占到30%以上，电阻可以在电路中用于分压、分流、限流、负载、反馈、阻抗匹配、RC充放电电路、上下拉、运算放大器外围电路、兼容设计电路、电流转电压等，下面介绍一下电阻的基本应用 在集总参…
阅读更多...
在Windows下编译支持https的wsdl2h

在Windows下编译支持https的wsdl2h

下载源码 在官网下载源码 安装Openssl 下载OpenSSL并安装，安装完成后需要将OpenSSL的路径添加到环境变量中 配置VS 1、打开工程 2、因为前面安装的OpenSLL是64位的，因此需要创建一个X64的配置 打开配置管理器，然后选择新建&#xff0…
阅读更多...
基于SpringBoot + Vue + redis 的医院挂号管理系统的设计与实现

基于SpringBoot + Vue + redis 的医院挂号管理系统的设计与实现

第6章 系统实现 本章节展示了公共模块，管理员模块，医生模块以及患者模块的部分功能截图。 6.1 登录注册模块 登录页面，用户在浏览器中输入网址后进入到系统的登录页面，用户可以选择角色进行登录，如图6-1所示。 图6-1…
阅读更多...
【机器学习】支持向量机SVR、SVC分析简明教程

【机器学习】支持向量机SVR、SVC分析简明教程

关于使用SVM进行回归分析的介绍很少，在这里，我们讨论一下SVR的理论知识，并对该方法有一个简明的理解。 1. SVC简单介绍 SVR全称是support vector regression，是SVM（支持向量机support vector machine）对回…
阅读更多...
大数据期末笔记

大数据期末笔记

第一章、大数据概述 人类的行为及产生的事件的一种记录称之为数据。 1、大数据时代的特征，并结合生活实例谈谈带来的影响。 （一）特征 1、Volume 规模性：数据量大。 2、Velocity高速性：处理速度快。数据的生成和响…
阅读更多...
贝锐自研智慧网关系统OrayOS升级，适配Banana PI开发板BPI-R3 Mini

贝锐自研智慧网关系统OrayOS升级，适配Banana PI开发板BPI-R3 Mini

为了满足多元化的应用场景，贝锐与Banana PI携手合作，贝锐自研新一代云智慧网关系统OrayOS不仅已成功适配BPI-R3，还进一步扩展至BPI-R3 Mini，提供了更丰富的选择。在全球工业物联网、视频监控管理以及企业级办公存储等领域&#xf…
阅读更多...
Android Studio使用soundtouch实现变声，AudioRecord,AudioTrack录音和播放，转换pcm文件为wav文件

Android Studio使用soundtouch实现变声，AudioRecord,AudioTrack录音和播放，转换pcm文件为wav文件

1.目标概要 分步骤实现为 1.0 集成使用soundtouch so文件 done 1.1 audiorecord和audiotrack 录音（pcm文件）并播放 done 1.2 把录音后文件转成wav文件 并播放 done 1.3 soundtouch变音后播放 done 2.实现 2.1 集成使用soundtouch so文件 编译和使用so…
阅读更多...
SpringBoot 架构的新冠密接者跟踪系统：安全防护体系深度解读

SpringBoot 架构的新冠密接者跟踪系统：安全防护体系深度解读

第3章 系统分析 在进行系统分析之前，需要从网络上或者是图书馆的开发类书籍中收集大量的资料，因为这个环节也是帮助即将开发的程序软件制定一套最优的方案，一旦确定了程序软件需要具备的功能，就意味着接下来的工作和任务都是围绕着…
阅读更多...
Opencv+ROS实现颜色识别应用

Opencv+ROS实现颜色识别应用

目录 一、工具 二、原理 概念 本质 三、实践 添加发布话题 主要代码 四、成果 五、总结 一、工具 opencvros ubuntu18.04 摄像头 二、原理 概念 彩色图像：RGB（红，绿，蓝） HSV图像：H&#xff0…
阅读更多...
图解人工智能：从规则到深度学习的全景解析

图解人工智能：从规则到深度学习的全景解析

🌟作者简介：热爱数据分析，学习Python、Stata、SPSS等统计语言的小高同学~🍊个人主页：小高要坚强的博客🍓当前专栏：Python之机器学习🍎本文内容：图解人工智能：…
阅读更多...
【FPGA开发】Vivado自定义封装IP核，绑定总线

【FPGA开发】Vivado自定义封装IP核，绑定总线

支持单个文件的封装、整个工程的封装，这里用单个文件举例。 在文件工程目录下，自建一个文件夹，里面放上需要封装的verilog文件。 选择第三个，指定路径封装，找到文件所在目录 取个名，选择封装IP的路径 会…
阅读更多...
fiddler安卓雷电模拟器配置踩坑篇

fiddler安卓雷电模拟器配置踩坑篇

一、fiddler端配置 和网页版fiddler一样，需要首先再本机安装证书，可以参考我之前的fiddler浏览器配置文章，前期操作一致： 此处需要注意的是connections里面需要勾选allow remote这个选项，这个主要是为了后来再安卓模拟…
阅读更多...
六、文本搜索工具（grep）和正则表达式

六、文本搜索工具（grep）和正则表达式

一、grep工具的使用 1、概念 grep： 是 linux 系统中的一个强大的文本搜索工具，可以按照 正则表达式 搜索文本，并把匹配到的行打印出来（匹配到的内容标红）。 2、语法 grep [options]…… pattern [file]…… 工作方式…
阅读更多...
时序论文28｜CycleNet：通过对周期模式进行建模增强时间序列预测

时序论文28｜CycleNet：通过对周期模式进行建模增强时间序列预测

论文标题：CycleNet: Enhancing Time Series Forecasting through Modeling Periodic Patterns 论文链接：https://arxiv.org/abs/2409.18479v1 代码链接：https://github.com/ACAT-SCUT/CycleNet 前言 这是今年NIPS的一篇时序论文&#xff…
阅读更多...
LuaForWindows_v5.1.5-52.exe

LuaForWindows_v5.1.5-52.exe

Releases rjpcomputing/luaforwindows GitHub #lua C:\Users\Administrator\Desktop\test.lua print("Hello lua！") print("ZengWenFeng 13805029595")
阅读更多...
Spring Boot教程之十一：获取Request 请求 和 Put请求

Spring Boot教程之十一：获取Request 请求 和 Put请求

如何在 Spring Boot 中获取Request Body？ Java 语言是所有编程语言中最流行的语言之一。使用 Java 编程语言有几个优点，无论是出于安全目的还是构建大型分发项目。使用 Java 的优点之一是 Java 试图借助类、继承、多态等概念将语言中的每个概念与现实世…
阅读更多...
【力扣热题100】[Java版] 刷题笔记-3. 无重复字符的最长子串

【力扣热题100】[Java版] 刷题笔记-3. 无重复字符的最长子串

题目:3. 无重复字符的最长子串 给定一个字符串 s ，请你找出其中不含有重复字符的 最长子串 的长度。 解题思路 根据题目，只需要返回无重复字符串的最长子串的长度，所以我们不需要知道知道字符串内容是什么，在整个字符串 s 中&…
阅读更多...
如何监控Elasticsearch集群状态？

如何监控Elasticsearch集群状态？

大家好，我是锋哥。今天分享关于【如何监控Elasticsearch集群状态？】面试题。希望对大家有帮助； 如何监控Elasticsearch集群状态？ 1000道 互联网大厂Java工程师 精选面试题-Java资源分享网 监控 Elasticsearch 集群的状态对于确保…
阅读更多...
Y20030018基于Java+Springboot+mysql+jsp+layui的家政服务系统的设计与实现 源代码 文档

Y20030018基于Java+Springboot+mysql+jsp+layui的家政服务系统的设计与实现 源代码 文档

家政服务系统的设计与实现 1.摘要2.开发目的和意义3.系统功能设计4.系统界面截图5.源码获取 1.摘要 随着人们生活水平的提高，老龄化、少子化等多重因素影响，我国对家政服务人群的需求与日俱增。家政服务行业对我国的就业和社会效益贡献也与日俱增&#…
阅读更多...
FreeRTOS——列表及列表项

FreeRTOS——列表及列表项

目录 一、概念及其应用 1.1列表List_t 1.2列表项ListItem_t 1.3迷你列表项MiniListItem_t 二、相关API 三、实现原理 3.1列表初始化 3.2列表项初始化 3.3插入列表项 3.4尾插列表项 3.5列表项的删除 3.6列表的遍历 一、概念及其应用 作为多任务的核心，列…
阅读更多...
最新文章

Copyright @ 2022~2023