jetson@Transbot:~$ roslaunch transbot_nav transbot_bringup.launch
jetson@Transbot:~$ roslaunch transbot_nav transbot_map.launch map_type:=hector
jetson@Transbot:~$ rosrun map_server map_saver -f ~/transbot_ws/src/transbot_nav/maps/my_map
jetson@Transbot:~$ convert -compress none ~/transbot_ws/src/transbot_nav/maps/my_map.pgm ~/transbot_ws/src/transbot_nav/maps/my_map.tif
######ด้านล่างนี้ไม่ต้องทำ ทำต่อไม่ไหว และเดียวมีปัญหาโดยรวมอีกครับ
checkpoint
jetson@Transbot:~$ python3 -m pip install --upgrade pip
jetson@Transbot:~$ pip install pyzbar opencv-python
jetson@Transbot:~$ sudo apt install ros-melodic-tf
code python
ให้บันทึกเป็นไฟล์ชื่อ: qr_checkpoint_node.py
ด้วยคำสั่ง cd ~/catkin_ws/src/your_package/scripts
nano qr_checkpoint_node.py
ใส่คำสั่งใน ไฟล์
#!/usr/bin/env python3
import rospy
import cv2
import tf
import yaml
import os
from pyzbar import pyzbar
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
class QRCheckpointNode:
def __init__(self):
rospy.init_node('qr_checkpoint_node')
self.bridge = CvBridge()
self.image_pub = rospy.Publisher('/qr_camera/image_raw', Image, queue_size=1)
self.marker_pub = rospy.Publisher('/checkpoint_marker', Marker, queue_size=10)
self.tf_listener = tf.TransformListener()
self.checkpoints = []
self.saved_ids = set()
self.marker_id = 0
# Camera init
self.cap = cv2.VideoCapture(0) # หรือ /dev/video1 ตามกล้องที่ใช้
# Output file
self.output_file = os.path.expanduser("~/checkpoints.yaml")
def publish_marker(self, pos, label):
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.ns = "checkpoints"
marker.id = self.marker_id
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = pos[0]
marker.pose.position.y = pos[1]
marker.pose.position.z = 0.2
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.text = label
self.marker_pub.publish(marker)
self.marker_id += 1
def save_checkpoint(self, qr_data, position):
if qr_data in self.saved_ids:
return
self.saved_ids.add(qr_data)
self.checkpoints.append({
'id': qr_data,
'position': {'x': position[0], 'y': position[1], 'z': position[2]}
})
with open(self.output_file, 'w') as f:
yaml.dump({'checkpoints': self.checkpoints}, f)
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
ret, frame = self.cap.read()
if not ret:
rospy.logwarn("Can't read camera frame")
continue
decoded_objects = pyzbar.decode(frame)
for obj in decoded_objects:
qr_data = obj.data.decode("utf-8")
try:
(trans, rot) = self.tf_listener.lookupTransform('/map', '/base_link', rospy.Time(0))
rospy.loginfo(f"Detected QR: {qr_data} at {trans}")
self.save_checkpoint(qr_data, trans)
self.publish_marker(trans, qr_data)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn("TF error")
# Draw box around QR
(x, y, w, h) = obj.rect
cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
cv2.putText(frame, qr_data, (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
# Convert & publish image
ros_image = self.bridge.cv2_to_imgmsg(frame, "bgr8")
self.image_pub.publish(ros_image)
rate.sleep()
self.cap.release()
if __name__ == '__main__':
try:
node = QRCheckpointNode()
node.run()
except rospy.ROSInterruptException:
pass
วางโค้ด Python ที่ผมให้ไว้ก่อนหน้านี้ลงไปในไฟล์
กด Ctrl + O เพื่อบันทึก
กด Enter เพื่อยืนยันชื่อไฟล์
กด Ctrl + X เพื่อออกจาก nano
chmod +x qr_checkpoint_node.py
เพิ่มสิทธิ์ให้ไฟล์รันได้:
chmod +x qr_checkpoint_node.py
เรียกใช้ node:
rosrun transbop_nav qr_checkpoint_node.py
rosrun your_package qr_checkpoint_node.py
rosrun your_package qr_checkpoint_node.py