Esempio n. 1
0
def handle_incoming_car(msg):

    control = Control()
    control.shift_gears = 2

    print('Receiving command', msg)

    args = msg.split(',')
    speed_level = int(args[7])
    x = float(args[5])
    y = float(args[6])
    deg = xy2deg(x, y)

    delta = 15
    if (45 + delta) <= deg <= (135 - delta):  # forward
        control.throttle = 0.2 * speed_level
    elif (225 + delta) <= deg <= (315 - delta):  #backward
        control.brake = 1.0
    elif (135 - delta) <= deg <= (225 + delta):  #left
        control.throttle = 0.2 * speed_level
        control.steer = 0.2 * speed_level
        if deg >= 180:
            control.shift_gears = 3
    else:  #right
        control.throttle = 0.2 * speed_level
        control.steer = -0.2 * speed_level
        if 270 <= deg <= 360:
            control.shift_gears = 3

    vel_pub.publish(control)
def cmd_callback(data):
    global wheelbase
    global ackermann_cmd_topic
    global frame_id
    global pub
    global correct_v
    global correct_yaw
    global v
    global yaw
    global last_recorded_vel
    global last_recorded_ang
    global kp
    global ki
    global kd
    global dt
    global integral
    global last_error
    prius_vel = Control()
    target_vel = data.linear.x
    print("Command Velocity: "),
    if (abs(target_vel) < 0.2):
        target_vel = 0
        print("0.0")
    if (target_vel >= 0.2):
        prius_vel.shift_gears = 2
        print("+"),
        print(target_vel)
    if (target_vel <= -0.2):
        prius_vel.shift_gears = 3
        print(target_vel)

    integral = integral + ki * (target_vel - last_recorded_vel)
    integral = min(3, max(-3, integral))
    v = kp * (target_vel - last_recorded_vel) + integral + kd * (
        target_vel - last_recorded_vel - last_error)
    v = min(1, max(-1, v))
    last_error = (target_vel - last_recorded_vel)
    #v = data.linear.x + correct_v

    yaw = data.angular.z + correct_yaw
    steering = convert_trans_rot_vel_to_steering_angle(v, yaw, wheelbase)
    if (target_vel <= -0.2):
        v = v * -1.0
    # msg = AckermannDriveStamped()
    # msg.header.stamp = rospy.Time.now()
    # msg.header.frame_id = frame_id
    # msg.drive.steering_angle = steering
    # msg.drive.speed = v

    prius_vel.throttle = v
    prius_vel.brake = 0
    prius_vel.steer = steering / 3.14
    if (prius_vel.throttle < 0):
        prius_vel.brake = 1
    #pub.publish(msg)
    pub.publish(prius_vel)
    def forward(self, throttle, steer_angle):
        """forward generates a forward-moving Control msg with throttle and steering angle

        :param throttle: throttle in range [-1.0, 1.0]
        :param steer_angle: steering angle in range [-1.0, 1.0]

        if throttle < 0.0 then apply brakes

        """
        assert steer_angle >= -1.0 and steer_angle <= 1.0, "Require steer angle to be in range [-1.0, 1.0]: {0}".format(
            steer_angle)
        brake = 0.0
        if throttle < 0.0:
            brake = abs(throttle)
            throttle = 0.0
        assert brake >= 0.0 and brake <= 1.0, "Brake must be in range [0.0, 1.0]: {0}".format(
            brake)
        assert throttle >= 0.0 and throttle <= 1.0, "Throttle must be in range [0.0, 1.0]: {0}".format(
            throttle)
        msg = Control()
        self.populate_header(msg)

        msg.throttle = throttle  # throttle as given
        msg.steer = steer_angle  # steering angle as given
        msg.brake = brake  # braking amount
        msg.shift_gears = 2  # forward gearing

        return msg
    def callback(self, message):
        rospy.logdebug("joy_translater received axes %s", message.axes)
        command = Control()
        command.header = message.header
        if message.axes[THROTTLE_AXIS] >= 0:
            command.throttle = message.axes[THROTTLE_AXIS]
            command.brake = 0.0
        else:
            command.brake = message.axes[THROTTLE_AXIS] * -1
            command.throttle = 0.0

        if message.buttons[3]:
            command.shift_gears = Control.FORWARD
        elif message.buttons[1]:
            command.shift_gears = Control.NEUTRAL
        elif message.buttons[0]:
            command.shift_gears = Control.REVERSE
        else:
            command.shift_gears = Control.NO_COMMAND

        command.steer = message.axes[STEERING_AXIS]
        self.last_published = message
        self.pub.publish(command)
Esempio n. 5
0
    def callback(self, message):
        throttle_value = message.linear.x
        gears_value = message.linear.z
        steering_value = message.angular.z

        rospy.logdebug("joy_translater received Throttle value " +
                       str(throttle_value))
        rospy.logdebug("joy_translater received Gears value " +
                       str(gears_value))
        rospy.logdebug("joy_translater received Steering value " +
                       str(steering_value))
        command = Control()

        header = Header()
        header.frame_id = "world"
        header.stamp = rospy.Time.now()

        command.header = header
        if throttle_value >= 0:
            command.throttle = throttle_value
            command.brake = 0.0
        else:
            command.brake = throttle_value * -1
            command.throttle = 0.0

        if gears_value > 0.0:
            command.shift_gears = Control.FORWARD
        elif gears_value == 0.0:
            command.shift_gears = Control.NEUTRAL
        elif gears_value < 0.0:
            command.shift_gears = Control.REVERSE
        else:
            command.shift_gears = Control.NO_COMMAND

        command.steer = steering_value
        self.last_published = message
        self.pub.publish(command)
Esempio n. 6
0
    def sendCtrlCmdsToGazebo(self):
        """
        Send control commands to Gazebo, which updates the robot state according to its inertial model.
        """
        command_msg = Control()
        command_msg.header.stamp = rospy.get_rostime()
        command_msg.throttle = self.throttle
        command_msg.brake = 0.0
        command_msg.shift_gears = Control.FORWARD
        steer = self.steering
        command_msg.steer = steer
        self.control_pub.publish(command_msg)

        position_msg = PointStamped()
        position_msg.header.stamp = rospy.get_rostime()
        position_msg.point.x = self.lin_position
        self.linear_position_pub.publish(position_msg)
        return
Esempio n. 7
0
def control_test():
    pub = rospy.Publisher("/prius", Control, queue_size=1)
    rospy.init_node('sim_control', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    command = Control()
    command.header.stamp = rospy.Time.now()
    command.throttle = 0.2
    command.brake = 0
    command.steer = 0.0
    command.shift_gears = Control.NO_COMMAND   
    
   # while not rospy.is_shutdown():
    for i in range(10):
        pub.publish(command)
        rate.sleep()
    
    for i in range(10):
        command.brake = 0.5
        pub.publish(command)
        rate.sleep()
Esempio n. 8
0
    def joystick_callback(self, message):
        command = Control()

        # Set message header
        command.header = message.header

        # Get steering value
        command.steer = message.axes[self._STEERING_AXIS]
        # Set GUI steering value
        self._steering_scrollbar_signal.emit(
            int(message.axes[self._STEERING_AXIS] * -100))

        # Get cruise control state
        if message.buttons[self._CIRCLE] and self._circle_flag:
            self._circle_flag = False
            self._cruise_control_state = not self._cruise_control_state
            self._cruise_radiobutton_signal.emit(self._cruise_control_state)
        elif not message.buttons[self._CIRCLE]:
            self._circle_flag = True

        # Increment cruise control speed
        if message.buttons[self._R1] and self._R1_flag:
            self._R1_flag = False
            if self._cruise_control_speed < self._MAX_SPEED:
                self._cruise_control_speed += 1
                self._cruise_lcdnumber_signal.emit(self._cruise_control_speed)
        elif not message.buttons[self._R1]:
            self._R1_flag = True

        # Decrement cruise control speed
        if message.buttons[self._L1] and self._L1_flag:
            self._L1_flag = False
            if self._cruise_control_speed > 0:
                self._cruise_control_speed -= 1
                self._cruise_lcdnumber_signal.emit(self._cruise_control_speed)
        elif not message.buttons[self._L1]:
            self._L1_flag = True

        # If cruise control was on then
        if self._cruise_control_state:
            command.shift_gears = Control.FORWARD
            self._gears_lineedit_signal.emit('D')

            # Calculate the safe distance which prevents collision
            safe_velocity = np.sqrt(
                2 * self._AMAX * self._closest_distance) * 3.6
            # Get the minimum of the safe distance or the speed desired by the driver
            desired_velocity = min(self._cruise_control_speed, safe_velocity)

            # PID loop
            t_current = time.time()

            dt = t_current - self._t_previous

            v_current_error = desired_velocity - self._current_velocity
            self._v_total_error += v_current_error * dt
            v_error_rate = (v_current_error - self._v_previous_error) / dt

            p_throttle = self._KP * v_current_error
            i_throttle = self._KI * self._v_total_error
            d_throttle = self._KD * v_error_rate

            longitudinal_output = p_throttle + i_throttle + d_throttle

            if longitudinal_output >= 0:
                command.throttle = np.fmax(np.fmin(longitudinal_output, 1.0),
                                           0.0)
                command.brake = 0.0
                self._throttle_scrollbar_signal.emit(
                    int(command.throttle * -100))
            else:
                command.throttle = 0.0
                command.brake = np.fmax(np.fmin(-longitudinal_output, 1.0),
                                        0.0)
                self._throttle_scrollbar_signal.emit(int(command.brake * 100))

            self._v_previous_error = v_current_error
            self._t_previous = t_current
        else:
            # Reset variables
            self._v_total_error = 0
            self._v_previous_error = 0
            self._t_previous = 0

            self._closest_distance = float('inf')

            # Get throttle/breaking value
            if message.axes[self._THROTTLE_AXIS] >= 0:
                command.throttle = message.axes[self._THROTTLE_AXIS]
                command.brake = 0.0
            else:
                command.brake = message.axes[self._THROTTLE_AXIS] * -1
                command.throttle = 0.0

            # Set GUI throttle value
            self._throttle_scrollbar_signal.emit(
                int(message.axes[self._THROTTLE_AXIS] * -100))

            # Get gears value
            if message.buttons[self._TRIANGLE]:
                command.shift_gears = Control.FORWARD
                self._gears_lineedit_signal.emit('D')
            elif message.buttons[self._CROSS]:
                command.shift_gears = Control.NEUTRAL
                self._gears_lineedit_signal.emit('N')
            elif message.buttons[self._SQUARE]:
                command.shift_gears = Control.REVERSE
                self._gears_lineedit_signal.emit('R')
            else:
                command.shift_gears = Control.NO_COMMAND

        self._t_previous = time.time()

        # Send control message
        try:
            self._prius_publisher.publish(command)
            rospy.loginfo("Published commands")
        except Exceptionrospy.ROSInterruptException as e:
            pass

        self._last_published = message
Esempio n. 9
0
self.pub = rospy.Publisher('car_1/prius', Control, queue_size=1)

stdscr.refresh()

key = ''
while key != ord('q'):
    key = stdscr.getch()
    stdscr.refresh()

    command = Control()

    # fill in the conditions to increment/decrement throttle/steer

    if key == curses.KEY_UP:
        command.throttle = message.axes[THROTTLE_AXIS]
        command.brake = 0.0
        command.shift_gears = Control.FORWARD
    elif key == curses.KEY_DOWN:
        forward = forward - 1
    elif key == curses.KEY_LEFT:
        left = left + 1
    elif key == curses.KEY_RIGHT:
        left = left - 1
    elif key == ord(' '):
        # this key will center the steer and throttle
        left = 0
        forward = 0

    pub.publish(command)

    curses.endwin()
Esempio n. 10
0
    speed = rospy.get_param("~speed", 0.5)
    turn = rospy.get_param("~turn", 1.0)
    status = 0
    command = Control()
    try:
        print(msg)
        print(vels(speed, turn))
        while (1):
            #command = Control()
            key = getKey()
            # if key == 'i': throttle +, steering 0, gear FORWARD, braking 0
            if key == 'i':
                command.throttle = speed
                command.steer = 0.0
                command.shift_gears = Control.FORWARD
                command.brake = 0.0
            # if key == 'u': throttle +, steering +, gear FORWARD, braking 0
            elif key == 'u':
                command.throttle = speed
                command.steer = turn
                command.shift_gears = Control.FORWARD
                command.brake = 0.0
            # if key == 'o': throttle +, steering -, gear FORWARD, braking 0
            elif key == 'o':
                command.throttle = speed
                command.steer = -turn
                command.shift_gears = Control.FORWARD
                command.brake = 0.0
            # if key == 'k': throttle 0, steering 0, gear NEUTRAL, braking +
            elif key == 'k':