def cmd_callback(cmd):
    """ Handle velocity commands from the navigation controllers """
    left_motor = cmd.linear.x - cmd.angular.z
    right_motor = cmd.linear.x + cmd.angular.z
    motors_cmd = motors(left=left_motor, right=right_motor)

    motors_pub.publish(motors_cmd)
def manual_control_callback(data):
    #http://wiki.ros.org/joy#Microsoft_Xbox_360_Wireless_Controller_for_Linux

    axes = data.axes

    drivetrain_msg = motors()

    # RT
    if (1 - axes[5]) < 0.1:
        throttle = 0
    else:
        throttle = (1 - axes[5]) * max_speed

    # LT
    if (1 - axes[2]) > 0.1:
        throttle = -(1 - axes[2]) * max_speed

    turning = axes[0] * max_speed

    drivetrain_msg.left = clamp(throttle - turning * 0.6, -max_speed,
                                max_speed)
    drivetrain_msg.right = clamp(throttle + turning * 0.6, -max_speed,
                                 max_speed)

    # Corrections
    drivetrain_msg.right = -drivetrain_msg.right

    # rospy.loginfo("manual_control callback.")
    manual_control_pub.publish(drivetrain_msg)
def manual_control_callback(data):
    #http://wiki.ros.org/joy#Microsoft_Xbox_360_Wireless_Controller_for_Linux

    axes = data.axes

    drivetrain_msg = motors()

    drivetrain_msg.right = axes[4]**3 * 2.2
    if abs(axes[4]) < 0.1:
        drivetrain_msg.right = 0

    drivetrain_msg.left = axes[1]**3 * 2.2
    if abs(axes[1]) < 0.1:
        drivetrain_msg.left = 0

    # rospy.loginfo("manual_control callback.")
    manual_control_pub.publish(drivetrain_msg)
def timer_callback(event):
    if pos is None or heading is None:
        return

    cur_pos = (pos[0], pos[1])

    lookahead = None
    radius = 0.3 # Start with a radius of 0.3 meters

    while lookahead is None and radius <= 3: # Look until we hit 3 meters max
        lookahead = pp.get_lookahead_point(cur_pos[0], cur_pos[1], radius)
        radius *= 1.2

    if SHOW_PLOTS:
        draw_pp(cur_pos, lookahead, pp.path)

    if lookahead is not None:
        # Get heading to to lookahead from current position
        heading_to_lookahead = math.degrees(math.atan2(lookahead[1] - cur_pos[1], lookahead[0] - cur_pos[0]))
        if heading_to_lookahead < 0:
            heading_to_lookahead += 360

        # Get difference in our heading vs heading to lookahead
        heading_error = heading_to_lookahead - heading
        heading_error = (heading_error + 180) % 360 - 180

        # Normalize error to -1 to 1 scale
        error = heading_error/180

        # Base forward velocity for both wheels
        forward_speed = 0.8 * (1 - abs(error))**5

        # Define wheel linear velocities
        # Add proprtional error for turning.
        # TODO: PID instead of just P
        motor_pkt = motors()
        motor_pkt.left = (forward_speed - 0.4 * error) / WHEEL_RADIUS
        motor_pkt.right = (forward_speed + 0.4 * error) / WHEEL_RADIUS

        publy.publish(motor_pkt)
Exemplo n.º 5
0
def timer_callback(event):
    if pos is None or heading is None:
        return

    cur_pos = (pos[0], pos[1])

    lookahead = None
    radius = 0.8  # Starting radius

    while lookahead is None and radius <= 3:  # Look until we hit 3 meters max
        lookahead = pp.get_lookahead_point(cur_pos[0], cur_pos[1], radius)
        radius *= 1.2

    if SHOW_PLOTS:
        draw_pp(cur_pos, lookahead, pp.path)

    if lookahead is not None and mspeed is not None and (
        (lookahead[1] - cur_pos[1])**2 + (lookahead[0] - cur_pos[0])**2) > 0.1:
        # Get heading to to lookahead from current position
        heading_to_lookahead = math.degrees(
            math.atan2(lookahead[1] - cur_pos[1], lookahead[0] - cur_pos[0]))
        if heading_to_lookahead < 0:
            heading_to_lookahead += 360

        # Get difference in our heading vs heading to lookahead
        # Normalize error to -1 to 1 scale
        error = get_angle_diff(heading, heading_to_lookahead) / 180

        # print(f"am at {cur_pos[0]:0.02f},{cur_pos[1]:0.02f}, want to go to {lookahead[0]:0.02f},{lookahead[1]:0.02f}")
        # print(f"angle delta: {error * 180:0.01f}")

        # print(f"error is {error}")
        if abs(error) < 2.0 / 180:
            error = 0

        # Base forward velocity for both wheels
        forward_speed = 0.65 * (1 - abs(error))**3

        # Define wheel linear velocities
        # Add proprtional error for turning.
        # TODO: PID instead of just P
        motor_pkt = motors()
        motor_pkt.left = (forward_speed - clamp(0.3 * error, -0.2, 0.2))
        motor_pkt.right = (forward_speed + clamp(0.3 * error, -0.2, 0.2))

        if motor_pkt.left < mspeed[0] - 0.1:
            motor_pkt.left = mspeed[0] - 0.1

        if motor_pkt.left > mspeed[0] + 0.1:
            motor_pkt.left = mspeed[0] + 0.1

        if motor_pkt.right < mspeed[1] - 0.1:
            motor_pkt.right = mspeed[1] - 0.1

        if motor_pkt.right > mspeed[1] + 0.1:
            motor_pkt.right = mspeed[1] + 0.1

        publy.publish(motor_pkt)
    else:
        # We couldn't find a suitable direction to head, stop the robot.
        motor_pkt = motors()
        motor_pkt.left = -0.25
        motor_pkt.right = -0.25

        publy.publish(motor_pkt)