ROS导航仿真和多点导航2——在Map中获取机器人位置

  本博客通过地图与坐标的关系，来获取机器人在地图中的实时坐标。为了获取地图上的任一一点坐标信息，本博客使用Python QT构建一个简单的界面。

0.代码下载

代码下载：
链接：https://pan.baidu.com/s/1x9o7nCnl-TdK0Y6cEqfRWg
提取码：oi7u

1.代码结构讲解

  为了方便大家理解，此处介绍代码结构：

class  LocationRecorder(QWidget):
      def __init__(self):
            super(LocationRecorder,self).__init__()
            self.init_ui()									#通过python qt构建uil界面，此处代码中包括获得机器人位置和记录机器人位置
            self.current_robot_pose = None
            self.robot_record_pose = None
            self.package_path = None
            self.init_rospack()								#获取ROS功能包的路径
            self.tf_listener = tf.TransformListener()

            self.show()
            
     def init_ui(self):
            self.layout = QVBoxLayout()
            self.order_layout = QHBoxLayout()
            self.order_layout.addWidget(QLabel("位置编号："))
            self.order_edit = QLineEdit("")
            self.order_layout.addWidget(self.order_edit)

            self.text_content = QTextEdit()
            self.text_content.setEnabled(False)

            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)		//获取机器人当前位置

if  __name__ == "__main__":
      app = QApplication(sys.argv)
      rospy.init_node("record_location")
      lr = LocationRecorder()
      sys.exit(app.exec_())
      

  UI界面的结构：

下面的功能函数都是class LocationRecorder(QWidget)类的函数。

2.获取当前功能包的路径

  为了存储机器人的位置，本函数获得当前功能包的路径：

     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)

2.获取机器人位置信息

  通过base_link与map坐标的tf，就可以得到机器人实时位置，获取函数如下：

      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)

3.用json的数据格式记录机器人位置

  将获得的机器人位置存储在当前功能包的data目录下：

      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 {}/data".format(self.package_path))
            with open("{}/data/{}.json".format(self.package_path,order),'w+') as out:
                  json.dump(new_record,out,indent=4)

4.运行方式

rosrun auto_nav2d_point src/record_location.py