Exemplo n.º 1
0
def main_auto():
    # initialize ROS node
    init_node('auto_mode', anonymous=True)
    nh = Publisher('ecu', ECU, queue_size=10)

    # set node rate
    rateHz = get_param("controller/rate")
    rate = Rate(rateHz)
    dt = 1.0 / rateHz
    t_i = 0

    # get experiment parameters
    t_0 = get_param("controller/t_0")  # time to start test
    t_f = get_param("controller/t_f")  # time to end test
    FxR_target = get_param("controller/FxR_target")
    d_f_target = pi / 180 * get_param("controller/d_f_target")

    # main loop
    while not is_shutdown():
        # get command signal
        (FxR, d_f) = circular(t_i, t_0, t_f, d_f_target, FxR_target)

        # send command signal
        ecu_cmd = ECU(FxR, d_f)
        nh.publish(ecu_cmd)

        # wait
        t_i += dt
        rate.sleep()
Exemplo n.º 2
0
    def run(self):

        publishRate = 30  # Hz
        rosRate = Rate(publishRate)
        while not is_shutdown():
            if not self._multiArray:
                continue

            covMat = self._convertToMatrix(self._multiArray)
            covMatNpTp = np.transpose(np.array(covMat))

            (eigValues, eigVectors) = np.linalg.eig(covMatNpTp)
            loginfo(eigVectors)

            quaternion = self._getRotation(eigVectors)
            scaledEigenValues = self._getScaledEigenvalues(
                eigValues, 0.05, 0.25)

            ellipsoid = self._createEllipsoidMsg([1, 1, 1], quaternion,
                                                 scaledEigenValues)

            self._publisher.publish(ellipsoid)
            # loginfo(ellipsoid)

            loginfo(10 * "---")
            rosRate.sleep()
Exemplo n.º 3
0
def main_auto():
    # initialize ROS node
    init_node('auto_mode', anonymous=True)
    nh = Publisher('ecu', ECU, queue_size = 10)

	# set node rate
    rateHz  = get_param("controller/rate")
    rate 	= Rate(rateHz)
    dt   	= 1.0 / rateHz
    t_i     = 0

    # get experiment parameters 
    t_0             = get_param("controller/t_0")     # time to start test
    t_f             = get_param("controller/t_f")     # time to end test
    FxR_target      = get_param("controller/FxR_target")
    d_f_target      = pi/180*get_param("controller/d_f_target")
 
    # main loop
    while not is_shutdown():
        # get command signal
        (FxR, d_f) = circular(t_i, t_0, t_f, d_f_target, FxR_target)
			
        # send command signal 
        ecu_cmd = ECU(FxR, d_f)
        nh.publish(ecu_cmd)
	
        # wait
        t_i += dt
        rate.sleep()
Exemplo n.º 4
0
 def local_robot_state_publisher(self, publish_rate=20):
     if self.publish_robot_state:
         rate = Rate(publish_rate)  # 20hz
         t = currentThread()
         while not is_shutdown() and getattr(t, "do_run", True):
             local_transformer.update_robot_state()
             rate.sleep()
         return True
    def run(self):
        rate = Rate(1)
        while not rospy.is_shutdown():

            cameraMessage = self._createCameraInfoMessage()
            self._cameraInfoPublisher.publish(cameraMessage)

            rate.sleep()
Exemplo n.º 6
0
 def run(self):
     rosRate = Rate(30)
     while not is_shutdown():
         
         message = self._setForces()
         loginfo(message)
         self._publisher.publish(message)
         rosRate.sleep()
Exemplo n.º 7
0
    def run(self):
        rosRate = Rate(30)
        while not is_shutdown():

            self._broadcastTransform(self._markerPose, self._parentFrame,
                                     self._childFrame)  #map2center

        rosRate.sleep()
Exemplo n.º 8
0
 def local_robot_pose_publisher(self, publish_rate=20):
     "Publishes the base_frame of the robot in reference to the odom frame to tf."
     if self.publish_robot_base_pose:
         rate = Rate(publish_rate)  # 20hz
         t = currentThread()
         while not is_shutdown() and getattr(t, "do_run", True):
             local_transformer.update_robot_pose()
             rate.sleep()
         return True
Exemplo n.º 9
0
def do_sample():
    pub = Publisher('turtle1/cmd_vel', Twist, queue_size=10)
    init_node('sample_talker')
    rate = Rate(10)
    print_instruction()
    while not is_shutdown():
        move = get_move()
        if move is not None:
            pub.publish(move)
        rate.sleep()
Exemplo n.º 10
0
def tank_drive(leftFrontVel, leftBackVel, rightFrontVel, rightBackVel):
    vel_publisher = Publisher("/robot_drive_controller/command", Float64MultiArray, queue_size=10)
    rospy.init_node("drive_velocity_setpoint", anonymous=True)
    vel_array = Float64MultiArray()
    # left front, left_back, right_front right_back
    vel_array.data = [leftFrontVel, leftBackVel, rightFrontVel, rightBackVel]
    rate = Rate(10)
    while not rospy.is_shutdown():
        rospy.loginfo(vel_array)
        vel_publisher.publish(vel_array)
        rate.sleep()
Exemplo n.º 11
0
    def run(self):
        rosRate = Rate(60)

        while not is_shutdown():
            buttonMsg = self._setButton()
            positionMsg = self._setLockState()

            self._pubPosition.publish(positionMsg)
            self._pubButton.publish(buttonMsg)

            rosRate.sleep()
Exemplo n.º 12
0
def main(args):
    init()
    ihm = Ihm()
    rospy.init_node('ihm', anonymous=True)
    try:
        rate = Rate(1)
        rate.sleep()
        rospy.spin()

    except KeyboardInterrupt:
        print("Shutting down")
    cv2.destroyAllWindows()
Exemplo n.º 13
0
        def local_object_pose_publisher(self, publish_rate=20):
            """
            Publishes the given pose of the object of given name in reference to the map frame to tf.

            :type name: str
            :type pose: list or Pose
            """
            if self.publish_objects_poses:
                rate = Rate(publish_rate)
                t = currentThread()
                while not is_shutdown() and getattr(t, "do_run", True):
                    local_transformer.update_objects()
                    rate.sleep()
                return True
def wifi_signal_monitor():
    init_node('bthere_wifi_signal_monitor', anonymous=False)
    pub = Publisher('/bthere/wifi_signal', WifiData, queue_size=10)
    loginfo('Outputting to /bthere/wifi_signal')
    test_output = get_param('~test_output', False)
    update_period = get_param('~update_period', 15.0)
    quiet = get_param('~quiet', False)

    rate = Rate(1 / float(update_period))
    loginfo('Publishing rate: ' + str(1 / float(update_period)) + 'hz')

    while not is_shutdown():
        if (test_output):
            output_test_data(rate, pub, quiet)
        else:
            output_wifi(rate, pub, quiet)
        rate.sleep()
Exemplo n.º 15
0
    def run(self):

        publishRate = 30  # Hz
        rosRate = Rate(publishRate)

        # initialize stiffness feedback class
        CalcOmniFeedbackForce = CalcHDFeedbackForce([0, 1000], [-3.3, 3.3],
                                                    [-60, 60])
        # [0.5, 0.5, 0.5], [5, 5, 5],
        # [-3.3,-3.3,-3.3], [3.3,3.3,3.3],
        # [-60,-60,-60],[60,60,60])

        while not is_shutdown():
            if not (self._stiffnessMessage and self._omniPositionMessage):
                continue

            # Get/Set stiffness matrix
            loginfo(self._omniPositionMessage)
            stiffnessMatrix = self._convertArrayToMatrix(
                self._stiffnessMessage)
            CalcOmniFeedbackForce.setCurrentRobotStiffness(stiffnessMatrix)

            # Get/Set omni current_position
            currentPosition = self._omniPositionMessage.current_position
            CalcOmniFeedbackForce.setCurrentHDPosition(
                [currentPosition.x, currentPosition.y, currentPosition.z])

            # Get/Set omni lock_position position
            lockPosition = self._omniPositionMessage.lock_position
            CalcOmniFeedbackForce.setLockPositionHD(
                [lockPosition.x, lockPosition.y, lockPosition.z])

            # Do the calculations
            CalcOmniFeedbackForce.calcForce()

            # get the feedbackForces
            feedBackForce = CalcOmniFeedbackForce.getHDForces()

            # Set feedback force message
            message = self._setOmniFeedbackMessage(feedBackForce)

            # publish feedback force message
            self._publisher.publish(message)

            loginfo(10 * "---")
            rosRate.sleep()
    def run(self):
        rosRate = Rate(30)
        broadcaster = StaticTransformBroadcaster()
        while not is_shutdown():

            rot = Rotation(self._rotMatrixArray[0], self._rotMatrixArray[1],
                           self._rotMatrixArray[2], self._rotMatrixArray[3],
                           self._rotMatrixArray[4], self._rotMatrixArray[5],
                           self._rotMatrixArray[6], self._rotMatrixArray[7],
                           self._rotMatrixArray[8])
            quat = rot.GetQuaternion()

            staticTransform = self._setTransform(self._robotBaseFrame,
                                                 self._HDFrame, quat)
            broadcaster.sendTransform(staticTransform)

            rosRate.sleep()
Exemplo n.º 17
0
 def local_static_odom_publisher(self, publish_rate=20):
     if self.publish_static_odom:
         rate = Rate(publish_rate)  # 20hz
         t = currentThread()
         while not is_shutdown() and getattr(t, "do_run", True):
             published = local_transformer.publish_pose(
                 local_transformer.map_frame,
                 local_transformer.projection_namespace + '/' +
                 local_transformer.odom_frame,
                 [[0, 0, 0], [0, 0, 0, 1]],
                 static=True)
             if not published:
                 logerr(
                     "(publisher) Could not publish static map to odom tf."
                 )
             rate.sleep()
         return True
Exemplo n.º 18
0
def main_auto():
    global read_yaw0, yaw_local

    # initialize ROS node
    init_node('auto_mode', anonymous=True)
    ecu_pub = Publisher('ecu', ECU, queue_size=10)
    se_sub = Subscriber('imu', Imu, imu_callback)

    # set node rate
    rateHz = get_param("controller/rate")
    rate = Rate(rateHz)
    dt = 1.0 / rateHz

    # get PID parameters
    p = get_param("controller/p")
    i = get_param("controller/i")
    d = get_param("controller/d")
    pid = PID(P=p, I=i, D=d)
    setReference = False

    # get experiment parameters
    t_0 = get_param("controller/t_0")  # time to start test
    t_f = get_param("controller/t_f")  # time to end test
    FxR_target = get_param("controller/FxR_target")
    t_params = (t_0, t_f, dt)

    while not is_shutdown():

        # OPEN LOOP
        if read_yaw0:
            # set reference angle
            if not setReference:
                pid.setPoint(yaw_local)
                setReference = True
                t_i = 0.0
                print('yaw_local', yaw_local)
            # apply open loop command
            else:
                (FxR, d_f) = straight(t_i, pid, t_params, FxR_target)
                ecu_cmd = ECU(FxR, -d_f)
                ecu_pub.publish(ecu_cmd)
                t_i += dt

        # wait
        rate.sleep()
Exemplo n.º 19
0
def motorMaster():
    '''
    The Master Node defined as a function. Using a multiarray message, the 4 angles are calculated
    by the functions brought in from motorControls and then published to a single topic. Each motor
    pulls its respective command from that multiarray.
    '''
    angPub = Publisher('motAngs', DOFArray, queue_size=10)
    init_node('master', anonymous=True)

    while not is_shutdown():
        for i in dataFrame.values:
            angles = DOFArray()
            angles.baseAng, angles.mainAng, \
                angles.secAng, angles.toolAng = genAngles(list(i))
            rate = Rate(.5)
            loginfo("\n" + str(angles))
            angPub.publish(angles)
            rate.sleep()
Exemplo n.º 20
0
def imu():
        #define publisher
        pub0 = Publisher('imu_data', Imu, queue_size=10)
        pub1 = Publisher('yaw_rate', Float64WithHeader, queue_size=10)
        pub2 = Publisher('yaw_rate_control', Float64, queue_size=10)
        init_node('imu', anonymous=True)
        rate = Rate(10) # 10hz
        print('Running IMU node.')
        while not is_shutdown():
            # 
            # Read IMU data.
            data  = imu_t.get_data()
            imu_data.header.stamp = Time.now()
            imu_data.header.stamp = get_rostime()
            # 
            # Get quaternion.
            roll = 180 * math.atan(data[3] / math.sqrt(data[4]**2 + data[5]**2)) / math.pi
            pitch = 180 * math.atan(data[4] / math.sqrt(data[3]**2 + data[5]**2)) / math.pi
            yaw = 180 * math.atan(data[5] / math.sqrt(data[3]**2 + data[5]**2)) / math.pi
            quaternion = quaternion_from_euler(roll,pitch,yaw)
            # 
            # Fill message.
            imu_data.orientation.w = quaternion[0]
            imu_data.orientation.x = quaternion[1]
            imu_data.orientation.y = quaternion[2]
            imu_data.orientation.z = quaternion[3]
            imu_data.linear_acceleration.x = data[3]
            imu_data.linear_acceleration.y = data[4]
            imu_data.linear_acceleration.z = data[5]
            imu_data.linear_acceleration_covariance[0] = -1
            imu_data.angular_velocity.x = data[0]
            imu_data.angular_velocity.y = data[1]
            imu_data.angular_velocity.z = data[2]
            imu_data.angular_velocity_covariance[0] = -1
            pub0.publish(imu_data)

            yaw_rate = Float64WithHeader()
            yaw_rate.header.stamp = get_rostime()
            yaw_rate.float.data = data[2]
            pub1.publish(yaw_rate)
            pub2.publish(data[2]/250.0) #data2 is yaw rate, 1 is pitch rate, 0 roll, have to fix the imudata
            # loginfo(imu_data)
            # loginfo(yaw_rate)
            rate.sleep()
Exemplo n.º 21
0
def main_auto():
    global read_yaw0, yaw_local

    # initialize ROS node
    init_node('auto_mode', anonymous=True)
    ecu_pub = Publisher('ecu', ECU, queue_size = 10)
    se_sub = Subscriber('imu/data', Imu, imu_callback)

	# set node rate
    rateHz      = get_param("controller/rate") 
    rate 	    = Rate(rateHz)
    dt          = 1.0 / rateHz

    # get PID parameters
    p 		= get_param("controller/p")
    i 		= get_param("controller/i")
    d 		= get_param("controller/d")
    pid     = PID(P=p, I=i, D=d)
    setReference    = False
    
    # get experiment parameters 
    t_0             = get_param("controller/t_0")     # time to start test
    t_f             = get_param("controller/t_f")     # time to end test
    FxR_target      = get_param("controller/FxR_target")
    t_params        = (t_0, t_f, dt)

    while not is_shutdown():

        # OPEN LOOP 
        if read_yaw0:
            # set reference angle
            if not setReference:
                pid.setPoint(yaw_local)
                setReference    = True
                t_i             = 0.0
            # apply open loop command
            else:
                (FxR, d_f)      = straight(t_i, pid, t_params, FxR_target)
                ecu_cmd             = ECU(FxR, d_f)
                ecu_pub.publish(ecu_cmd)
                t_i += dt
	
        # wait
        rate.sleep()
Exemplo n.º 22
0
def step():
    publisher = Publisher('turtle1/cmd_vel', Twist, queue_size=10)
    init_node('control')
    rate = Rate(100)

    while not is_shutdown():
        keys = {
            "fwd": get_param('fwd'),
            "bwd": get_param('bwd'),
            "rht": get_param('rht'),
            "lft": get_param('lft')
        }
        system('clear')
        display(keys)
        move = getMove(keys)
        if move == 'quit':
            return True
            break
        if move is not None:
            publisher.publish(move)
        rate.sleep()
Exemplo n.º 23
0
def main_auto():

    # initialize ROS node
    init_node('auto_mode', anonymous=True)
    ecu_pub = Publisher('ecu', ECU, queue_size=10)

    # set node rate
    rateHz = get_param("controller/rate")
    rate = Rate(rateHz)
    dt = 1.0 / rateHz

    # get experiment parameters
    FxR_target = get_param("controller/FxR_target")

    while not is_shutdown():

        # OPEN LOOP
        ecu_cmd = ECU(FxR_target, 0)
        ecu_pub.publish(ecu_cmd)
        # wait
        rate.sleep()
Exemplo n.º 24
0
def main_auto():
    global throttle, steering

    # initialize the ROS node
    init_node('manual_control', anonymous=True)
    Subscriber('rc_inputs', ECU, rc_inputs_callback)
    nh = Publisher('ecu_pwm', ECU, queue_size = 10)

    # set node rate
    rateHz = 50
    dt = 1.0/rateHz
    rate = Rate(rateHz)

    throttle = 90
    steering = 90

    # main loop
    while not is_shutdown():
        ecu_cmd = ECU(throttle, steering)
        nh.publish(ecu_cmd)
        rate.sleep()
Exemplo n.º 25
0
def main_auto():
    global throttle, steering

    # initialize the ROS node
    init_node('manual_control', anonymous=True)
    Subscriber('rc_inputs', ECU, rc_inputs_callback)
    nh = Publisher('ecu_pwm', ECU, queue_size=10)

    # set node rate
    rateHz = 50
    dt = 1.0 / rateHz
    rate = Rate(rateHz)

    throttle = 90
    steering = 90

    # main loop
    while not is_shutdown():
        ecu_cmd = ECU(throttle, steering)
        nh.publish(ecu_cmd)
        rate.sleep()
Exemplo n.º 26
0
def sonar():
    pub = Publisher('sonar_meas', Float64WithHeader, queue_size=10)
    pub1 = Publisher('sonar_meas_control', Float64, queue_size=10)
    init_node('sonar', anonymous=True)
    rate = Rate(20)  # 10hz
    while not is_shutdown():
        GPIO.output(TRIG, True)
        time.sleep(0.00001)
        GPIO.output(TRIG, False)
        count = 0
        while GPIO.input(ECHO) == 0:
            if count > 1000:
                break
            pulse_start = time.time()
            count += 1

        count = 0
        while GPIO.input(ECHO) == 1:
            if count > 1000:
                break
            pulse_end = time.time()
            count += 1

        pulse_duration = pulse_end - pulse_start

        distance = pulse_duration * 17150
        distance = distance / 100.0

        sonar_data = Float64WithHeader()
        sonar_data.header.stamp = get_rostime()
        if abs(distance) < 2.0:
            sonar_data.float.data = distance
            pub1.publish(distance)
        else:
            sonar_data.float.data = 0.0
            pub1.publish(0.0)
        pub.publish(sonar_data)
        rate.sleep()
def cpu_monitor():
    """Publishes CPU data to /bthere/cpu_data."""

    architecture = uname()[
        4]  # This will return 'x86_64', 'aarc64' (for 64 bit arm), etc.
    if (not architecture in SUPPORTED_ARCHITECTURES):
        logerr(
            "This architecture doesn't appear to be one that is supported. Consider adding it and openning"
            + " a pull request on github!")
        exit()

    init_node("bthere_cpu_monitor", anonymous=False)
    pub = Publisher("/bthere/cpu_data", CPUData, queue_size=10)
    loginfo("Outputting to /bthere/cpu_data")

    #update period should to be somewhat small since the cpu load data is average since you last checked,
    #a slower update rate will be less accurate for bursty loads and may introduce more lag than expected
    #if a load is added later in the time between updates for example.
    update_period = get_param('~update_period', 1.0)
    rate = Rate(1 / float(update_period))
    loginfo("Publishing rate: " + str(1.0 / update_period) + " hz")

    quiet = get_param("~quiet", False)

    #since the temperature-getting seems likely to be failure prone, try it once to check.
    able_to_get_temps = True

    if (isnan(get_cpu_temps(architecture)[0])):
        logwarn("Unable to get CPU temperatures")
        able_to_get_temps = False

    last_cpu_times = []
    while not is_shutdown():
        data = CPUData()
        gated_loginfo(quiet, "------ CPU Data ------")
        if (able_to_get_temps):
            # If temperature data can be collected, add it to the CPUData to be published and log
            package_temp, core_temps = get_cpu_temps(architecture)
            gated_loginfo(quiet, "CPU Package temp. (C): " + str(package_temp))
            data.package_temp = package_temp
            if (len(core_temps) > 0):
                for core in range(len(core_temps)):
                    gated_loginfo(
                        quiet, "CPU Core " + str(core) + "temp. (C): " +
                        str(core_temps[core]))
            data.core_temps = core_temps
        else:
            # If the data is unavailable just publish NaN and log
            gated_loginfo(quiet, "CPU temperatures unavailable")
            data.package_temp = float("NaN")
            data.core_temps = [float("NaN")]
        if (len(last_cpu_times) == 0):
            # If this hasn't been initialized, we just won't publish this info yet and init.
            # last_cpu_times can't just be initialized before the loop because it should (for consistency) be the same
            # time between data collections and getting the initial data before the loop would make the time between
            # data collections small and potentially make the data misleading due to burst loads.
            last_cpu_times = get_load_data()
            gated_loginfo(quiet, "CPU load not yet available")
        else:
            overall_load, per_cores, last_cpu_times = get_cpu_load(
                last_cpu_times)
            gated_loginfo(
                quiet,
                "Overall CPU load: " + str(round(overall_load * 100, 1)) + "%")
            data.overall_cpu_load = overall_load
            if (len(per_cores) > 0):
                for core in range(len(per_cores)):
                    gated_loginfo(
                        quiet, "CPU core " + str(core) + " load: " +
                        str(round(per_cores[core] * 100, 1)) + "%")
            data.core_loads = per_cores

        # Add the header information:
        header = Header(stamp=Time.now())
        # The frame_id property seems to be to do with tf frames of reference. That isn't useful for something like
        # this, so just leave it empty. (this might be the wrong way to do this, but I don't have any other info.)
        # The sequential id is apparently set by the publisher.
        data.header = header

        pub.publish(data)
        rate.sleep()
Exemplo n.º 28
0
def sonar():
    init_node('sonar', anonymous=True)
    rate = Rate(20) # 10hz
    while not is_shutdown():
        pi.gpio_trigger(TRIGGER, 10)
        rate.sleep()
Exemplo n.º 29
0
class Node:
    def __init__(self):

        init_node(NODE_NAME, anonymous=True)
        Service(SERVICE_NAME, Planning, self.callback)
        rospy.Subscriber("pose", Float32MultiArray, self.pose_callback)

        self.planner = Planner()
        self.navigator = Navigator()
        self.position = Position()
        self.status = IDLE
        self.publisher = Publisher(PUBLISH_TO,
                                   Float32MultiArray,
                                   queue_size=10)
        self.rate = Rate(10)
        self.goal = None
        self.calculating = False

    def callback(self, request):
        commands = loads(request.req)
        response = {'status': 'type error'}
        if commands['type'] == SET_GOAL:
            self.planner.update_goal(
                (commands['goal'][0], commands['goal'][1]))
            self.goal = commands['goal']
            self.calculating = True
            self.planner.plan()
            self.position.update_plan(self.planner.planned)
            self.calculating = False
            response['status'] = 'ok'
        elif commands['type'] == MOVE:
            if commands['status'] == COMMAND_PAUSE:
                self.status = PAUSED
                response['status'] = 'ok'
            elif commands['status'] == COMMAND_GO:
                self.status = WALKING
                response['status'] = 'ok'
            elif commands['status'] == COMMAND_CANCEL:
                self.planner.clear()
                self.navigator.clear()
                self.status = IDLE
                response['status'] = 'ok'
        elif commands['type'] == SET_MAP:
            self.planner.update_map(commands['map'])
            response['state'] = self.status
            response['target'] = self.goal
            response['path'] = self.planner.get_path()
            response['status'] = 'ok'
        return dumps(response)

    def operate(self):
        if self.calculating:
            self.publisher.publish(Float32MultiArray(data=[0, 0]))
            return
        planned_position = self.position.get_position(
            *self.planner.current_position)
        if self.position.threshold_check(POSITION_ERROR_THRES):
            self.publisher.publish(Float32MultiArray(data=[0, 0]))
            print('Node : Hm... where am I?')
            self.planner.plan()
            self.position.update_plan(self.planner.planned)
            planned_position = self.position.get_position(
                *self.planner.current_position)
            return
        if not planned_position:
            self.status = IDLE
            self.publisher.publish(Float32MultiArray(data=[0, 0]))
            return
        self.publisher.publish(
            Float32MultiArray(data=self.navigator.get_motor_speed(
                self.planner.current_position, planned_position, time())))

    def pose_callback(self, input):
        new_pose = (input.data[3], input.data[4],
                    (input.data[2] * 180 / np.pi + 360) % 360)
        self.planner.update_position(new_pose)

    def publish(self):
        while not is_shutdown():
            if self.status == IDLE or self.status == PAUSED:
                self.publisher.publish(Float32MultiArray(data=[0, 0]))
            elif self.status == WALKING:
                if not validate_planned(self.planner.planned,
                                        self.planner.map):
                    self.planner.plan()
                    self.position.update_plan(self.planner.planned)
                self.operate()
            self.rate.sleep()

    def run(self):
        self.publish()
Exemplo n.º 30
0
            elif key in vSpeedBindings.keys():
                fixedV = fixedV*vSpeedBindings[key][0]
            elif key in wSpeedBindings.keys():
                fixedW = fixedW*wSpeedBindings[key][0]
            elif key in altitudeBindings.keys():
                z += altitudeBindings[key]
                z = max(min(1.75,z),0.2)
            else:
                v = 0.0
                w = 0.0
                if (key == '\x03'):
                    break

            print 'v %f w %f z %f m \n'%(v,w,z)
            pub_a.publish(z)
            pub_v.publish(v)
            pub_w.publish(w)
            rate.sleep()

    except ROSInterruptException:
        pass

    finally:
        print 'IM SHOOKETH'
        pub_a.publish(z)
        pub_v.publish(v)
        pub_w.publish(w)

        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

Exemplo n.º 31
0
def control():
    global read_yaw0, yaw_local, est_yaw
    # global location

    # initialize ROS node
    init_node('adas', anonymous=True)
    Subscriber('imu/data', Imu, imu_callback)
    Subscriber('scan', LaserScan, laser_callback)
    # Subscriber('vel_est', Encoder, vel_callback)
    # Subscriber('range', Float32MultiArray, range_callback)
    Subscriber('pose_estimate', Vector3, pose_callback)
    ecu_pub = Publisher('ecu', ECU, queue_size=1)
    encoder_pub = Publisher('filtered_vel', Float32, queue_size=1)
    yaw_pub = Publisher('filtered_yaw', Float32, queue_size=1)
    scenario = (get_param("adas/scenario"))
    (p, i, d) = (get_param("adas/PID")[0], get_param("adas/PID")[1],
                 get_param("adas/PID")[2])
    stime = 0.02  # simulation time step
    rate = Rate(1 / stime)
    aeb = False
    setReference = False
    setReference1 = False
    trigger = False
    pid = PID(P=p, I=i, D=d)
    t_i = 0.0
    (t0, tf, ttc) = (0, 0, 0)
    while not is_shutdown():
        t0 = rospy.get_time()
        if scenario == "AEB":
            # if 0.0 < location < 15.0:     # for camera based detection
            if dist < 1:
                if not aeb:
                    aeb = True
            else:
                aeb = False
            if aeb:
                if not setReference:
                    pid.setPoint(0)
                    setReference = True
                (F, dx) = (-1 / dist * 3, 0)
            else:
                setReference = False
                (F, dx) = (2.0, 0.0)
        elif scenario == "Lane Change":
            if t_i >= 50:
                (F, dx) = (0, 0)
            else:
                # if 0.0 < location < 15.0:  #  for camera based detection
                if dist < 1:
                    if not trigger:
                        trigger = True
                        x1 = dist
                        ttc = vel_avg * x1
                        net_time = ttc + t_i
                if trigger:
                    if net_time >= t_i:
                        if not setReference:
                            pid.setPoint(30 / 57.3)
                            setReference = True
                            setReference1 = False
                        (F, dx) = (3, pid.update(est_yaw, stime))
                    else:
                        if not setReference1:
                            pid.setPoint(0)
                            setReference1 = True
                            setReference = False
                        (F, dx) = (3, pid.update(est_yaw, stime))
                else:
                    (F, dx) = (3, 0)
                    trigger = False
                    setReference1 = False
                    setReference = False

        ecu_pub.publish(ECU(F, dx))
        encoder_pub.publish(Float32(vel_avg))
        yaw_pub.publish(Float32(yaw_local))
        rate.sleep()
        tf = rospy.get_time()
        t_elapsed = tf - t0
        t_i += t_elapsed
        print('time elapsed=' + str(t_i) + '\nloop time=' + str(t_elapsed))
        print('ttc=' + str(ttc))
        print('vel avg=' + str(vel_avg))
        print('range=' + str(dist))