ROS导航小车6 记录充电点实位置发送命令现前往回充点（仅作个人纪录）

记录当前机器人充电点位Python1参考链接

#!/usr/bin/env python3
#coding: utf-8
 
import sys
import rospy
from PyQt5.QtWidgets import QWidget,QApplication,QLabel,QHBoxLayout,QVBoxLayout,QPushButton,QLineEdit,QTextEdit,QCheckBox
from PyQt5.QtCore import *
from geometry_msgs.msg import PoseStamped
import json
import os
import tf
import rospkg
 
class  LocationRecorder(QWidget):
      def __init__(self):
            super(LocationRecorder,self).__init__()
            self.init_ui()
            self.current_robot_pose = None
            self.robot_record_pose = None
            self.package_path = None
            self.init_rospack()
            self.tf_listener = tf.TransformListener()
            self.setGeometry(300, 300, 800, 800)         # 确定窗口位置大小
            self.setWindowTitle('路径节点保存器')          # 设置窗口标题
            quit = QPushButton('Close', self)            # button 对象
            quit.setGeometry(680, 650, 90, 90)           # 设置按钮的位置 和 大小
            quit.setStyleSheet("background-color: red")  # 设置按钮的风格和颜色
            quit.clicked.connect(self.close)             # 点击按钮之后关闭窗口            
 
            self.show()
      
      def init_rospack(self):
            rospack = rospkg.RosPack()
            rospack.list()
            try:
                  self.package_path = rospack.get_path('auto_nav2d_point')
            except:
                  print("Could not find package auto_nav2d_point")
                  exit(1)
            print(self.package_path)
            
 
      def init_ui(self):
            self.layout = QVBoxLayout()                       #创建实例UI
            self.order_layout = QHBoxLayout()
            self.order_layout.addWidget(QLabel("位置编号："))   #设置UI界面
            self.order_edit = QLineEdit("6")                  #这里是输入框中的具体内容
            self.order_layout.addWidget(self.order_edit)      #将控件放入布局widget->控件窗口或控件
 
            self.text_content = QTextEdit()       #这里是下方信息输出窗口的地方
            self.text_content.setEnabled(False)   #setEnabled(False)设置窗口不可点击 true则可以点击
 
            self.record_layout = QHBoxLayout()                 #创建实例
            self.receive_button = QPushButton("获取位点")       #创建按钮和按钮名称
            self.record_button = QPushButton("记录位点")        #创建按钮和按钮名称
            self.record_layout.addWidget(self.receive_button)  #将按钮放入实际的工作界面
            self.record_layout.addWidget(self.record_button)   #将按钮放入实际的工作界面
 
            self.layout.addLayout(self.order_layout)           #将创建的模块放入分层中
            self.layout.addWidget(self.text_content)
            self.layout.addLayout(self.record_layout)
            self.setLayout(self.layout)
            
            self.record_button.clicked.connect(self.record)   #点击按钮就记录
            self.receive_button.clicked.connect(self.receive) #点击按钮就保存
            
           
 
      def record(self):
            order = self.order_edit.text()
            try:
                  order = int(order)
            except:
                  return
            
            new_record = {
                  "order" : order,
                  "robot_pose" : self.robot_record_pose
            }
 
            os.system("mkdir -p {}/data2".format(self.package_path))
            with open("{}/data2/{}.json".format(self.package_path,order),'w+') as out:
                  json.dump(new_record,out,indent=4)
 
      def listen_tf(self):
            try:
                  (pos,ori) = self.tf_listener.lookupTransform("map","base_link",rospy.Duration(0.0))
                  msg_dict = {
                        "pos_x" : pos[0],
                        "pos_y" : pos[1],
                        "pos_z" : pos[2],
                        "ori_x" : ori[0],
                        "ori_y" : ori[1],
                        "ori_z" : ori[2],
                        "ori_w": ori[3]
                  }
                  self.current_robot_pose = msg_dict
                  return True
            except tf.Exception as e:
                  print ("listen to tf failed")
                  return False
            
 
      def receive(self):
            while not rospy.is_shutdown() and not self.listen_tf():
                  rospy.sleep(1.0)
            self.robot_record_pose = self.current_robot_pose
            display_msg = "Robot:\n" + json.dumps(self.robot_record_pose,indent=4) + "\n"
 
            self.text_content.setText(display_msg)
 
 
 
if  __name__ == "__main__":
      app = QApplication(sys.argv)
      rospy.init_node("record_location")
      lr = LocationRecorder()

记录当前机器人充电点位Python2 参考链接

#!/usr/bin/env python
#coding: utf-8


import sys
import rospy
from PyQt5.QtWidgets import QWidget,QApplication,QLabel,QHBoxLayout,QVBoxLayout,QPushButton,QLineEdit,QTextEdit,QCheckBox
from geometry_msgs.msg import PoseStamped
import json
import os
import tf
import rospkg

class  LocationRecorder(QWidget):
      def __init__(self):
            super(LocationRecorder,self).__init__()
            self.init_ui()
            self.current_robot_pose = None
            self.robot_record_pose = None
            self.package_path = None
            self.init_rospack()
            self.setGeometry(300, 300, 800, 800) 
            self.setWindowTitle('charging pile location') 
            self.tf_listener = tf.TransformListener()
            quit = QPushButton('Close', self)            
            quit.setGeometry(680, 650, 90, 90)           
            quit.setStyleSheet("background-color: red")  
            quit.clicked.connect(self.close)             
            quit.clicked.connect(self.close) 

            self.show()
      
      def init_rospack(self):
            rospack = rospkg.RosPack()
            rospack.list()