通过代码获取机器人位置坐标
本博客通过地图与坐标的关系，来获取机器人在地图中的实时坐标。为了获取地图上的任一一点坐标信息，本博客使用Python QT构建一个简单的界面。
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)
代码下载：
链接：https://pan.baidu.com/s/1x9o7nCnl-TdK0Y6cEqfRWg
提取码：oi7u