Ejemplo n.º 1
0
    def __init__(self):
        # Control states
        self.NO_CONTROL = 0
        self.EMERGENCY_BRAKE = 1
        self.NORMAL_TRACKING = 2
        self.ESTIMATE_TRACKING = 3

        self.HIGH_SPEED = 3.
        self.LOW_SPEED = 1.

        # Control variable
        self.look_ahead_distance = 50
        self.look_ahead_point = PoseStamped()

        # Input manage
        self.vehicle_state = VehicleState()

        self.estimated_path = Path()

        self.latest_generated_path = Path()
        self.current_path = Path()
        self.update_path = False

        self.latest_velocity_level = VelocityLevel()
        self.velocity_level = VelocityLevel()
        self.update_velocity_level = False

        self.latest_motion_state = MotionState()
        self.motion_state = MotionState()
        self.update_motion_state = False
        self.motion_state_buff = [self.motion_state]
        self.motion_state_buff_size = 20

        self.curvature = Curvature()
        self.curvature.gear = 0
        self.curvature_buff = [self.curvature]
        self.curvature_time_buff = [rospy.get_rostime().to_sec()]
        self.curvature_buff_size = 200

        self.latest_vehicle_pose = Pose()
        self.update_vehicle_pose = False
        self.vehicle_pose = Pose()

        self.temp_control_mode = 0

        # Variable for parking
        self.is_PARKING = False
        self.init_parking_path = Path()
Ejemplo n.º 2
0
    def __init__(self):
        # Control constants
        self.MAX_speed = 5.5  # [m/s]
        self.MIN_speed = 0.  # [m/s]
        #self.MAX_steer = 2000/float(71)      # [degree]
        #self.MIN_steer = -2000/float(71)     # [degree]
        self.MAX_steer = 180.
        self.MIN_steer = -180.
        self.MAX_brake = 150
        self.MIN_brake = 1

        # Control states
        self.NO_CONTROL = 0
        self.EMERGENCY_BRAKE = 1
        self.NORMAL_TRACKING = 2
        self.ESTIMATE_TRACKING = 3

        # Platform constants
        self.wheelbase = 1.02  #[m] distance between two wheel axis
        self.cm2rear = 0.51  #[m] distance between center of mass and rear axis

        # Control variable
        self.control = Control()
        self.control_mode = self.NO_CONTROL
        self.control_write = 0
        self.control_count = 0
        self.control_buff = []
        self.control_time_buff = []
        self.control_buff_size = 200
        self.look_ahead_distance = 20
        self.look_ahead_point = PoseStamped()

        # Input manage
        self.vehicle_state = Control()

        self.latest_generated_path = Path()
        self.current_path = Path()
        self.update_path = False

        self.latest_velocity_level = VelocityLevel()
        self.velocity_level = VelocityLevel()
        self.update_velocity_level = False

        self.curvature = Curvature()
Ejemplo n.º 3
0
        def __init__ (self) :
                # Control states
                self.NO_CONTROL = 0
                self.EMERGENCY_BRAKE = 1
                self.NORMAL_TRACKING = 2
                self.ESTIMATE_TRACKING = 3
        
                # Control variable
                self.look_ahead_distance = 10
                self.look_ahead_point= PoseStamped()

                # Input manage
                self.vehicle_state = VehicleState()

                self.latest_generated_path = Path()
                self.current_path = Path()
                self.update_path = False
                self.first_path = False

                self.latest_velocity_level = VelocityLevel()
                self.velocity_level = VelocityLevel()
                self.update_velocity_level = False

                self.latest_motion_state = MotionState()
                self.motion_state = MotionState()
                self.update_motion_state = False
                self.motion_state_buff = []
                self.motion_state_buff_size = 20

                self.curvature = Curvature()
                self.curvature.gear = 1
                self.curvature_write = 0
                self.curvature_count = 0
                self.curvature_buff = []
                self.curvature_time_buff = []
                self.curvature_buff_size = 200

                self.temp_control_mode = 0

                # Variable for parking
                self.is_PARKING = False
                self.init_parking_path = Path()
Ejemplo n.º 4
0
 def __init__(self):
     self.lane_map = None
     self.lidar_map = None
     self.merged_map = None
     self.left_cor = [0, 0]  #upper bound of left lane
     self.right_cor = [0, 0]  #upper bound if right lane
     self.mid_cor = [0, 0]  #medium of two coors
     self.turn_count = 0
     self.count = 0
     self.pose = PoseStamped()
     self.velocity = VelocityLevel()
Ejemplo n.º 5
0
def mainloop():
    '''
    code for activate and deactivate the node
    '''
    nodename = 'local_map_generator'
    mainloop.active = True

    def signalResponse(data):
        mainloop.active
        if 'zero_monitor' in data.active_nodes:
            if nodename in data.active_nodes:
                mainloop.active = True
            else:
                mainloop.active = False
        else:
            rospy.signal_shutdown('no monitor')

    rospy.Subscriber('/active_nodes', ActiveNode, signalResponse)
    '''
    ...
    '''
    rospy.Subscriber("/motion_state", MotionState, mcb)
    rospy.Subscriber("/raw_local_map", Image, rlmcb)
    pubgoal = rospy.Publisher('/goal_pose', PoseStamped, queue_size=10)
    pubmap = rospy.Publisher('/local_map', OccupancyGrid, queue_size=10)
    pubtf = rospy.Publisher('/tf', TFMessage, queue_size=10)
    pubvel = rospy.Publisher('/velocity_level', VelocityLevel, queue_size=10)
    rospy.init_node(nodename, anonymous=True)
    rate = rospy.Rate(10)  # 10hz

    goal = PoseStamped()
    goal.pose.position.y = 2
    goal.pose.orientation.w = 1
    goal.header.frame_id = "car_frame"

    vellv = VelocityLevel()
    vellv.header.frame_id = "car_frame"

    qmap = tf_conversions.transformations.quaternion_from_euler(0, 0, 0)
    simplemap = OccupancyGrid()
    simplemap.header.frame_id = "car_frame"
    simplemap.info.resolution = 0.03  #0.5
    simplemap.info.width = 200  #100
    simplemap.info.height = 200  #100
    simplemap.info.origin.position.x = -3  #-25
    simplemap.info.origin.position.y = -0.5  #-25
    simplemap.info.origin.orientation.x = qmap[0]
    simplemap.info.origin.orientation.y = qmap[1]
    simplemap.info.origin.orientation.z = qmap[2]
    simplemap.info.origin.orientation.w = qmap[3]
    simplemap.data = makemap(simplemap.info.height, simplemap.info.width)

    qframe = tf_conversions.transformations.quaternion_from_euler(0, 0, 0)
    simpletf = TFMessage()
    simpletf.transforms.append(TransformStamped())
    simpletf.transforms.append(TransformStamped())
    simpletf.transforms[0].header.frame_id = "global_ref"
    simpletf.transforms[0].child_frame_id = "car_frame"
    simpletf.transforms[0].transform.rotation.x = qframe[0]
    simpletf.transforms[0].transform.rotation.y = qframe[1]
    simpletf.transforms[0].transform.rotation.z = qframe[2]
    simpletf.transforms[0].transform.rotation.w = qframe[3]
    simpletf.transforms[0].transform.translation.x = 0
    simpletf.transforms[0].transform.translation.y = 0
    simpletf.transforms[1].header.frame_id = "car_frame"
    simpletf.transforms[1].child_frame_id = "camera"
    simpletf.transforms[1].transform.rotation.x = qframe[0]
    simpletf.transforms[1].transform.rotation.y = qframe[1]
    simpletf.transforms[1].transform.rotation.z = qframe[2]
    simpletf.transforms[1].transform.rotation.w = qframe[3]
    simpletf.transforms[1].transform.translation.x = 1
    simpletf.transforms[1].transform.translation.y = 2
    i = 0
    while not rospy.is_shutdown():
        simplemap.header.stamp = rospy.Time.now()
        simplemap.header.seq = i
        goal.header.stamp = rospy.Time.now()
        goal.header.seq = i
        vellv.header.stamp = rospy.Time.now()
        vellv.header.seq = i
        simpletf.transforms[0].header.stamp = rospy.Time.now()
        simpletf.transforms[0].header.seq = i
        simpletf.transforms[1].header.stamp = rospy.Time.now()
        simpletf.transforms[1].header.seq = i
        i = i + 1
        if mainloop.active:
            pubgoal.publish(goal)
            pubmap.publish(simplemap)
            pubtf.publish(simpletf)
            pubvel.publish(vellv)
        rate.sleep()
Ejemplo n.º 6
0
                np.flipud(merged_arr).ravel().astype(np.int8))
            occupancy.header.stamp = rospy.Time.now()
            occupancy.header.frame_id = "car_frame"
            occupancy.info.resolution = 0.03
            occupancy.info.width = merged_arr.shape[0]
            occupancy.info.height = merged_arr.shape[1]
            occupancy.info.origin.position.x = -occupancy.info.width / 2 * occupancy.info.resolution
            occupancy.info.origin.position.y = 0
            occupancy.info.origin.position.z = 0
            occupancy.info.origin.orientation.z = 0
            occupancy.info.origin.orientation.w = 1
            if isactive:
                obstacle_map_pub.publish(merged_img)
            if isactive:
                local_map_pub.publish(occupancy)

            pose = PoseStamped()
            pose.header.frame_id = "car_frame"
            if isactive:
                goal_pose_pub.publish(pose)

            velocity = VelocityLevel()
            velocity.header.frame_id = "car_frame"
            if isactive:
                velocity_level_pub.publish(velocity)

        rate.sleep()
    #rospy.spin()
    #except TypeError:
    #    rospy.loginfo("None type is in either lane_map or lidar_map")