#!/usr/bin/env pythonimport rospyimport cv2import numpy as npfrom cv_bridge import CvBridge, CvBridgeErrorfrom geometry_msgs.msg import Twist, PoseStampedfrom sensor_msgs.msg import Imageimport actionlibfrom move_base_msgs.msg import MoveBaseAction, MoveBaseGoalclass OmoveFollowing(): def __init__(self): # 禁止节点自动关闭（关键） rospy.init_node('omove_following', disable_signals=True) self.cv_bridge = CvBridge() self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # 导航组件 self.move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction) self.move_base.wait_for_server() self.initial_pose = None rospy.Subscriber('/amcl_pose', PoseStamped, self.amcl_pose_cb, queue_size=1) # 状态机（新增终端保持状态） self.STATE_TRACKING = 0 self.STATE_NAVIGATING = 1 self.STATE_IDLE = 2 # 新增：导航完成后保持待机 self.current_state = self.STATE_TRACKING self.nav_complete = False # 循迹订阅发布（保留原功能） rospy.Subscriber('/camera/color/image_raw', Image, self.color_img_cb) self.lane_img_pub = rospy.Publisher('/lane_image', Image, queue_size=5) rospy.Subscriber('/camera/depth/image_rect_raw', Image, self.depth_img_cb) self.yaw = 0.0 self.flag = 0 # 仅用于循迹终止检测，不再触发关机 self.blocked = False # 启动非阻塞主循环 self.loop_rate = rospy.Rate(10) self.run() def amcl_pose_cb(self, msg): """记录初始位置（仅循迹阶段有效）""" if self.initial_pose is None and self.current_state == self.STATE_TRACKING: self.initial_pose = msg.pose.pose rospy.loginfo(f"初始位置：x={self.initial_pose.position.x:.2f}, y={self.initial_pose.position.y:.2f}") def send_nav_goal(self): """发送返回初始位置的导航目标""" if not self.initial_pose: rospy.logwarn("未获取初始位置，无法导航") return goal = MoveBaseGoal() goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.header.frame_id = "map" goal.target_pose.pose = self.initial_pose goal.target_pose.pose.orientation = self.initial_pose.orientation # 锁定朝向 self.move_base.send_goal(goal, done_cb=self.nav_callback) self.current_state = self.STATE_NAVIGATING rospy.loginfo("开始导航返回起点") def nav_callback(self, status, result): """导航完成后切换至待机状态""" if status == actionlib.GoalStatus.SUCCEEDED: rospy.loginfo("成功返回起点，进入待机状态") else: rospy.logwarn(f"导航失败：状态码{status}") self.current_state = self.STATE_IDLE # 保持节点运行 self.nav_complete = True def run(self): """主循环（绝对不调用rospy.signal_shutdown）""" while not rospy.is_shutdown(): if self.current_state == self.STATE_TRACKING: self.tracking_loop() elif self.current_state == self.STATE_NAVIGATING: self.navigating_loop() elif self.current_state == self.STATE_IDLE: self.idle_loop() self.loop_rate.sleep() def tracking_loop(self): """循迹状态逻辑（移除所有关机代码）""" vel_cmd = Twist() if self.blocked: self.handle_obstacle(vel_cmd) else: vel_cmd.linear.x = 0.7 vel_cmd.angular.z = -self.yaw * 0.4 # 检测到循迹终止条件（仅切换状态，不关机） if self.flag: rospy.loginfo("循迹结束，准备导航") self.flag = 0 # 重置标志位 self.current_state = self.STATE_TRACKING # 临时保持，等待定位完成 self.send_nav_goal() # 触发导航 return # 立即切换状态 self.cmd_pub.publish(vel_cmd) def handle_obstacle(self, vel_cmd): """原障碍物回避逻辑（无修改）""" for i in range(20): vel_cmd.linear.y = 0.1 self.cmd_pub.publish(vel_cmd) rospy.sleep(0.1) for i in range(30): vel_cmd.linear.x = -0.1 self.cmd_pub.publish(vel_cmd) rospy.sleep(0.1) for i in range(20): vel_cmd.linear.y = -0.1 self.cmd_pub.publish(vel_cmd) rospy.sleep(0.1) self.blocked = False def navigating_loop(self): """导航状态：停止底盘控制""" vel_cmd = Twist() vel_cmd.linear.x = 0.0 vel_cmd.angular.z = 0.0 self.cmd_pub.publish(vel_cmd) def idle_loop(self): """待机状态：保持静止，可添加循环逻辑""" vel_cmd = Twist() self.cmd_pub.publish(vel_cmd) rospy.loginfo("待机中...按Ctrl+C退出") def color_img_cb(self, data): """图像回调（仅保留检测逻辑，移除关机代码）""" try: cv_img = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8') except CvBridgeError as e: print(e) return height, width, channels = cv_img.shape crop_img = cv_img[(height)//2+100:height, :width] # 修正切片语法 lower = np.array([10, 20, 10], dtype="uint8") upper = np.array([40, 50, 40], dtype="uint8") mask = cv2.inRange(crop_img, lower, upper) extraction = cv2.bitwise_and(crop_img, crop_img, mask=mask) m = cv2.moments(mask, False) try: x, y = m['m10']/m['m00'], m['m01']/m['m00'] except ZeroDivisionError: x, y = width//2, (height//2+100)+(height//2-100)//2 self.yaw = (x - width//2)/320.0 cv2.circle(extraction, (int(x), int(y)), 2, (0,255,0), 3) gray = cv2.cvtColor(extraction, cv2.COLOR_BGR2GRAY) edges = cv2.Canny(gray, 50, 150, apertureSize=3) lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=100, minLineLength=100, maxLineGap=10) self.flag = 0 # 重置标志位 if lines is not None: for line in lines: x1, y1, x2, y2 = line[0] slope = abs((y2 - y1)/(x2 - x1 + 1e-6)) if abs(slope) < 0.1: # 水平线检测 self.flag = 1 cv2.line(extraction, (x1, y1), (x2, y2), (0,0,255), 2) # 发布图像（无修改） if not rospy.is_shutdown(): self.lane_img_pub.publish(self.cv_bridge.cv2_to_imgmsg(np.hstack([crop_img, extraction]), "bgr8")) cv2.imshow("Image window", np.hstack([crop_img, extraction])) cv2.waitKey(1) def depth_img_cb(self, data): """深度图像回调（无修改）""" try: depth_img = self.cv_bridge.imgmsg_to_cv2(data, '16UC1') except CvBridgeError as e: print(e) return height, width = depth_img.shape for dis in depth_img[400, :]: # 修正切片语法 if 300 < dis < 350: self.blocked = False breakif __name__ == '__main__': try: node = OmoveFollowing() rospy.spin() # 保持节点运行，直到手动终止 except rospy.ROSInterruptException: rospy.logerr("节点被中断") finally: cv2.destroyAllWindows()为什么该代码，小车在寻迹尽头后，没有执行导航回去而是关闭终端停止运行呢
扫一扫
659
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
