safety.py 文件源码

def drive(self):
        while not rospy.is_shutdown():
            if self.received_data is None or self.parsed_data is None:
                rospy.sleep(0.5)
                continue

            if np.any(self.parsed_data['front'][:,0] < MIN_FRONT_DIST):
                rospy.loginfo("stoping!")
                # this is overkill to specify the message, but it doesn't hurt
                # to be overly explicit
                drive_msg_stamped = AckermannDriveStamped()
                drive_msg = AckermannDrive()
                drive_msg.speed = 0.0
                drive_msg.steering_angle = 0.0
                drive_msg.acceleration = 0
                drive_msg.jerk = 0
                drive_msg.steering_angle_velocity = 0
                drive_msg_stamped.drive = drive_msg
                self.pub.publish(drive_msg_stamped)

            # don't spin too fast
            rospy.sleep(.1)