Esempio n. 1
0
    def __init__(self):
        self.waypoint_x = []
        self.waypoint_y = []
        self.waypoint_seq = []
        self.waypoint_total_seq = 0
        self.x = 0
        self.y = 0
        self.q = np.empty(4)
        self.yaw = np.pi / 2
        self.pre_steering_ang = 0
        self.mission_start = False

        rospy.init_node('look_ahead_following')
        rospy.on_shutdown(self.shutdown)

        # ROS callback function, receive /odom mesage
        rospy.Subscriber('/gnss_odom',
                         Odometry,
                         self.odom_callback,
                         queue_size=1)
        #rospy.Subscriber('/gazebo/model_states', ModelStates, self.truth_callback)
        rospy.Subscriber('/mav/mission', MAV_Mission, self.load_waypoint)
        rospy.Subscriber('/mav/modes', MAV_Modes, self.mav_modes)
        self.ajk_pub = rospy.Publisher('/ajk_auto', AJK_value, queue_size=1)
        self.ajk_value = AJK_value()
        self.auto_log_pub = rospy.Publisher('/auto_log',
                                            Auto_Log,
                                            queue_size=1)
        self.auto_log = Auto_Log()
        self.cmdvel_pub = rospy.Publisher(
            '/sim_ajk/diff_drive_controller/cmd_vel', Twist, queue_size=1)
        self.cmdvel = Twist()
    def __init__(self):
        self.waypoint_x = []
        self.waypoint_y = []
        self.last_waypoint_x = 0
        self.last_waypoint_y = 0
        self.waypoint_seq = []
        self.waypoint_total_seq = 0
        self.x = 0
        self.y = 0
        self.q = np.empty(4)
        self.yaw = np.pi / 2
        self.pre_steering_ang = 0
        self.bool_start_point = True

        # mav_modes
        self.mission_start = False
        self.base_mode = 0
        self.custom_mode = 0

        # related to GNSS
        self.iTOW = 0
        self.rtk_status = 0
        self.movingbase_status = 0
        self.latitude = 0
        self.longitude = 0

        rospy.init_node('look_ahead_following')
        rospy.on_shutdown(self.shutdown)

        # ROS callback function, receive /odom mesage
        rospy.Subscriber('/gnss_odom',
                         Odometry,
                         self.odom_callback,
                         queue_size=1)
        #rospy.Subscriber('/gazebo/model_states', ModelStates, self.truth_callback)
        rospy.Subscriber('/mav/mission', MAV_Mission, self.load_waypoint)
        rospy.Subscriber('/mav/modes', MAV_Modes, self.mav_modes)
        rospy.Subscriber('/navpvt', NavPVT, self.navpvt_callback, queue_size=1)
        rospy.Subscriber('/relposned',
                         RELPOSNED,
                         self.relposned_callback,
                         queue_size=1)
        self.ajk_pub = rospy.Publisher('/ajk_auto', AJK_value, queue_size=1)
        self.ajk_value = AJK_value()
        self.auto_log_pub = rospy.Publisher('/auto_log',
                                            Auto_Log,
                                            queue_size=1)
        self.auto_log = Auto_Log()
        self.cmdvel_pub = rospy.Publisher('/roboteq_driver/cmd',
                                          Twist,
                                          queue_size=1)
        self.sim_cmdvel_pub = rospy.Publisher(
            '/sim_ajk/diff_drive_controller/cmd_vel', Twist, queue_size=1)
        self.cmdvel = Twist()
Esempio n. 3
0
    def __init__(self):
        self.waypoint_x = []
        self.waypoint_y = []
        self.waypoint_goal = []
        self.x = 0
        self.y = 0
        self.q = np.empty(4)
        self.yaw = np.pi / 2
        self.pre_steering_ang = 0

        rospy.init_node('look_ahead_following')
        rospy.on_shutdown(self.shutdown)

        # ROS callback function, receive /odom mesage
        #rospy.Subscriber('/gnss_odom', Odometry, self.odom_callback, queue_size = 1)
        rospy.Subscriber('/gazebo/model_states', ModelStates,
                         self.truth_callback)
        self.ajk_pub = rospy.Publisher('/ajk_auto', AJK_value, queue_size=1)
        self.ajk_value = AJK_value()
        self.auto_log_pub = rospy.Publisher('/auto_log',
                                            Auto_Log,
                                            queue_size=1)
        self.auto_log = Auto_Log()