Ejemplo n.º 1
0
def _handle_vision_robot_position(msg):
    robo_msg = RobotState()

    robo_msg.vision_x = msg.x
    robo_msg.vision_y = msg.y
    robo_msg.vision_theta = msg.theta
    robo_msg.correction = True

    robo_msg.xhat = msg.x
    robo_msg.yhat = msg.y
    robo_msg.thetahat = msg.theta

    robopub.publish(robo_msg)
Ejemplo n.º 2
0
def _handle_vision_position(msg):
    global _measured, _last_time
    _measured = (msg.x, msg.y, msg.theta)

    # If the estimator isn't on, just pass
    # vision data through to the rest
    if not _robot_estimator_on:
        msg = RobotState()
        msg.xhat = msg.vision_x = msg.xhat_future = _measured[0]
        msg.yhat = msg.vision_y = msg.yhat_future = _measured[1]
        msg.thetahat = msg.vision_theta = msg.thetahat_future = _measured[2]
        msg.correction = True
        _state_pub.publish(msg)

    # Timestamp this msg so the ball updater knows
    # how much time elapsed since last measurement
    _last_time = time.time()
Ejemplo n.º 3
0
def _handle_vision_position(msg):
    global _measured, _last_time
    _measured = (msg.x, msg.y, msg.theta)

    # If the estimator isn't on, just pass
    # vision data through to the rest
    if not _robot_estimator_on:
        msg = RobotState()
        msg.xhat = msg.vision_x = msg.xhat_future = _measured[0]
        msg.yhat = msg.vision_y = msg.yhat_future = _measured[1]
        msg.thetahat = msg.vision_theta = msg.thetahat_future = _measured[2]
        msg.correction = True
        _state_pub.publish(msg)

    # Timestamp this msg so the ball updater knows
    # how much time elapsed since last measurement
    _last_time = time.time()
Ejemplo n.º 4
0
def _handle_vision_robot_position(msg):
    robo_msg = RobotState()

    robo_msg.vision_x = msg.x
    robo_msg.vision_y = msg.y
    robo_msg.vision_theta = msg.theta
    robo_msg.correction = True

    robo_msg.xhat = msg.x
    robo_msg.yhat = msg.y
    robo_msg.thetahat = msg.theta

    robopub.publish(robo_msg)
Ejemplo n.º 5
0
def main():
    rospy.init_node('robot_estimator', anonymous=False)

    global _measured, _robot_estimator_on, _state_pub

    # Sub/Pub
    rospy.Subscriber('vision_position', Pose2D, _handle_vision_position)
    rospy.Subscriber('vel_cmds', Twist, _handle_vel_cmds)
    # Use remap in roslaunch file to create separate channels per robot
    _state_pub = rospy.Publisher('robot_state', RobotState, queue_size=10)

    _robot_estimator_on = rospy.get_param('robot_estimator_on', 'true')

    rate = rospy.Rate(_estimator_rate)
    while not rospy.is_shutdown():

        if not _robot_estimator_on:
            break

        xhat = yhat = thetahat = xhat_future = yhat_future = thetahat_future = 0

        # LPF
        Ts = (time.time() - _last_time)
        (xhat, yhat, thetahat) = _robot.update(Ts, measurement=_measured)

        # # KF
        # Ts = (time.time() - _last_time)
        # (xhat, yhat, thetahat) = _robot.update(measurement=_measured, vel_cmd=_velocities)

        if _predictor_on:
            (xhat_future, yhat_future, thetahat_future) = _robot.predict(_predict_forward_seconds)

        # Grab the estimated velocities of the ball
        # (vx, vy) = _ball.get_velocities()
        (vx, vy, w) = (0, 0, 0)

        # Construct ball_state message, BallState
        msg = RobotState()
        msg.vision_x = _measured[0] if _measured[0] is not None else 0
        msg.vision_y = _measured[1] if _measured[1] is not None else 0
        msg.vision_theta = _measured[2] if _measured[2] is not None else 0
        msg.xhat = xhat
        msg.yhat = yhat
        msg.thetahat = thetahat
        msg.vx = vx
        msg.vy = vy
        msg.w = w
        msg.xhat_future = xhat_future
        msg.yhat_future = yhat_future
        msg.thetahat_future = thetahat_future
        msg.predict_forward_seconds = _predict_forward_seconds if _predictor_on else 0
        msg.correction = _measured[0] is not None and _measured[1] is not None and _measured[2] is not None
        _state_pub.publish(msg)

        # Set measured to None so the robot updater 
        # knows to predict instead of correcting
        _measured = (None, None, None)


        rate.sleep()

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
Ejemplo n.º 6
0
def main():
    rospy.init_node('robot_estimator', anonymous=False)

    global _measured, _robot_estimator_on, _state_pub

    # Sub/Pub
    rospy.Subscriber('vision_position', Pose2D, _handle_vision_position)
    rospy.Subscriber('vel_cmds', Twist, _handle_vel_cmds)
    # Use remap in roslaunch file to create separate channels per robot
    _state_pub = rospy.Publisher('robot_state', RobotState, queue_size=10)

    _robot_estimator_on = rospy.get_param('robot_estimator_on', 'true')

    rate = rospy.Rate(_estimator_rate)
    while not rospy.is_shutdown():

        if not _robot_estimator_on:
            break

        xhat = yhat = thetahat = xhat_future = yhat_future = thetahat_future = 0

        # LPF
        Ts = (time.time() - _last_time)
        (xhat, yhat, thetahat) = _robot.update(Ts, measurement=_measured)

        # # KF
        # Ts = (time.time() - _last_time)
        # (xhat, yhat, thetahat) = _robot.update(measurement=_measured, vel_cmd=_velocities)

        if _predictor_on:
            (xhat_future, yhat_future,
             thetahat_future) = _robot.predict(_predict_forward_seconds)

        # Grab the estimated velocities of the ball
        # (vx, vy) = _ball.get_velocities()
        (vx, vy, w) = (0, 0, 0)

        # Construct ball_state message, BallState
        msg = RobotState()
        msg.vision_x = _measured[0] if _measured[0] is not None else 0
        msg.vision_y = _measured[1] if _measured[1] is not None else 0
        msg.vision_theta = _measured[2] if _measured[2] is not None else 0
        msg.xhat = xhat
        msg.yhat = yhat
        msg.thetahat = thetahat
        msg.vx = vx
        msg.vy = vy
        msg.w = w
        msg.xhat_future = xhat_future
        msg.yhat_future = yhat_future
        msg.thetahat_future = thetahat_future
        msg.predict_forward_seconds = _predict_forward_seconds if _predictor_on else 0
        msg.correction = _measured[0] is not None and _measured[
            1] is not None and _measured[2] is not None
        _state_pub.publish(msg)

        # Set measured to None so the robot updater
        # knows to predict instead of correcting
        _measured = (None, None, None)

        rate.sleep()

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()