def main(): rospy.init_node('ekf_node') ekf = ekf_class.ExtendedKalmanFilter() publisher_position = rospy.Publisher('estimated_pose', PoseStamped, queue_size=1) publisher_mavros = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1) #publisher_particles = rospy.Publisher('particle_poses', PoseArray, queue_size=1) publisher_marker = rospy.Publisher('Sphere', MarkerArray, queue_size=1) broadcaster = tf.TransformBroadcaster() rospy.Subscriber("/tag_detections", AprilTagDetectionArray, callback, [ ekf, publisher_position, publisher_mavros, broadcaster, publisher_marker ], queue_size=1) while not rospy.is_shutdown(): pass
def listener(): rospy.init_node('publisher', anonymous=True) ekf = ekf_class.ExtendedKalmanFilter() pub = rospy.Publisher("point_cloud2", PointCloud2, queue_size=2) publisher_marker = rospy.Publisher('detection_net_plane', MarkerArray, queue_size=1) rospy.Subscriber("/d435i/depth/color/points", PointCloud2, callback, [pub, ekf, publisher_marker]) rospy.spin() # video.release() cv2.destroyAllWindows()
def listener(): geschwindigkeit = 0.5 # tracker = tracking_red_dots(308,410) # tracker = tracking_red_dots(960, 1280,350,900,400,960) global ekf, publisher_distance_net, rviz ekf = ekf_class.ExtendedKalmanFilter() rospy.init_node('publisher', anonymous=True) publisher_distance_net = rospy.Publisher('/estimated_distance_to_net', PoseStamped, queue_size=1) rospy.Subscriber("/multisense_sl/camera/left/image_raw", Image, callback) if rviz: rospy.Subscriber("/gazebo/model_states", ModelStates, draw_net_rviz) rospy.spin() # video.release() cv2.destroyAllWindows()
def main(): global tags rospy.init_node('ekf_node') try: #which_calibration=rospy.get_param('~calibration') which_calibration = "water_tank" except KeyError: print( "################## You have to set a calibration parameter ###########################" ) exit(-1) if which_calibration == "gazebo": print("using gazebo calibration") data_path = rospack.get_path( "mu_auv_localization" ) + '/scripts/calibration_ground_truth_gazebo.csv' # in gazebo else: if which_calibration == "water_tank": print("using real calibration") data_path = rospack.get_path( "mu_auv_localization" ) + '/scripts/calibration_tank.csv' # in real tank else: print("could not find correct parameter for calibration ") exit(-1) tags = genfromtxt(data_path, delimiter=',') # home PC tags = tags[:, 0:4] tags[:, 3] += 0.0 ekf = ekf_class.ExtendedKalmanFilter() publisher_position = rospy.Publisher('estimated_pose', PoseStamped, queue_size=1) publisher_twist = rospy.Publisher('estimated_twist', TwistStamped, queue_size=1) publisher_mavros = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1) # publisher_particles = rospy.Publisher('particle_poses', PoseArray, queue_size=1) publisher_marker = rospy.Publisher('Sphere', MarkerArray, queue_size=1) broadcaster = tf.TransformBroadcaster() rospy.Subscriber("/tag_detections", AprilTagDetectionArray, callback, [ ekf, publisher_position, publisher_mavros, broadcaster, publisher_marker, publisher_twist ], queue_size=1) rospy.Subscriber("/mavros/imu/data", Imu, callback_imu, [ ekf, publisher_position, publisher_mavros, broadcaster, publisher_marker, publisher_twist ], queue_size=1) rospy.Subscriber("/mavros/local_position/pose_NED", PoseStamped, callback_orientation, ekf, queue_size=1) rospy.spin()
import ekf_class import numpy as np ekf = ekf_class.ExtendedKalmanFilter() ekf.prediction() A=np.asarray([[1,1,0],[0,3,2],[3,2,1]]) for i in range(5000): print(i) ekf.prediction() ekf.update(A)