def __init__(self):
        # defines
        self.update_rate = 20  # set update frequency [Hz]
        self.automode = False
        self.automode_prev = False
        self.status = 0
        self.wpt = False
        self.prev_wpt = False
        self.linear_speed = 0.0
        self.angular_speed = 0.0

        rospy.loginfo(rospy.get_name() + ": Start")
        self.quaternion = np.empty((4, ), dtype=np.float64)
        self.wii_a = False
        self.wii_a_changed = False
        self.wii_home = False
        self.wii_home_changed = False
        self.wii_up = False
        self.wii_up_changed = False
        self.wii_down = False
        self.wii_down_changed = False

        # get parameters
        self.debug = rospy.get_param("~print_debug_information", 'true')
        if self.debug:
            rospy.loginfo(rospy.get_name() + ": Debug enabled")
        self.status_publish_interval = rospy.get_param(
            "~status_publish_interval", '0')

        # get topic names
        self.automode_topic = rospy.get_param("~automode_sub",
                                              '/fmDecisionMakers/automode')
        self.pose_topic = rospy.get_param("~pose_sub", '/fmKnowledge/pose')
        self.joy_topic = rospy.get_param("~joy_sub", '/joy')
        self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",
                                            '/fmCommand/cmd_vel')
        self.wptnav_status_topic = rospy.get_param("~status_pub",
                                                   '/fmData/wptnav_status')

        # setup subscription topic callbacks
        rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
        rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
        rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)

        # setup publish topics
        self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
        self.twist = TwistStamped()
        self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic,
                                                 waypoint_navigation_status)
        self.wptnav_status = waypoint_navigation_status()
        self.status_publish_count = 0

        # configure waypoint navigation
        self.wptlist = waypoint_list()
        self.wptnav = waypoint_navigation(self.update_rate, self.debug)
        self.wptlist_loaded = False

        # call updater function
        self.r = rospy.Rate(self.update_rate)
        self.updater()
예제 #2
0
    def __init__(self):
        # defines
        self.update_rate = 20  # set update frequency [Hz]
        self.automode = False
        self.automode_prev = False
        self.status = 0
        self.wpt = False
        self.prev_wpt = False
        self.linear_speed = 0.0
        self.angular_speed = 0.0

        rospy.loginfo(rospy.get_name() + ": Start")
        self.quaternion = np.empty((4,), dtype=np.float64)
        self.wii_a = False
        self.wii_a_changed = False
        self.wii_home = False
        self.wii_home_changed = False
        self.wii_up = False
        self.wii_up_changed = False

        self.boom_state_pub = rospy.Publisher("/fmData/boom_state", Int8)
        self.boom_state_msg = Int8()
        self.boom_state_msg.data = 0

        # get parameters
        self.debug = rospy.get_param("~print_debug_information", "true")
        if self.debug:
            rospy.loginfo(rospy.get_name() + ": Debug enabled")
        self.status_publish_interval = rospy.get_param("~status_publish_interval", "0")

        # get topic names
        self.automode_topic = rospy.get_param("~automode_sub", "/fmDecisionMakers/automode")
        self.pose_topic = rospy.get_param("~pose_sub", "/fmKnowledge/pose")
        self.joy_topic = rospy.get_param("~joy_sub", "/joy")
        self.cmdvel_topic = rospy.get_param("~cmd_vel_pub", "/fmCommand/cmd_vel")
        self.wptnav_status_topic = rospy.get_param("~status_pub", "/fmData/wptnav_status")

        # setup subscription topic callbacks
        rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
        rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
        rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)

        # setup publish topics
        self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
        self.twist = TwistStamped()
        self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status)
        self.wptnav_status = waypoint_navigation_status()
        self.status_publish_count = 0

        # configure waypoint navigation
        self.wptlist = waypoint_list()
        self.wptnav = waypoint_navigation(self.update_rate, self.debug)
        self.wptlist_loaded = False

        # call updater function
        self.r = rospy.Rate(self.update_rate)
        self.updater()
    def __init__(self):
        # defines
        self.update_rate = 20  # set update frequency [Hz]
        self.IMPLEMENT_INVALID = -10000000.0
        self.STATE_IDLE = 0
        self.STATE_NAVIGATE = 1
        self.STATE_WAIT = 2
        self.state = self.STATE_IDLE
        self.state_prev = self.state
        self.automode_warn = False
        self.no_route_plan_warn = False
        self.wait_after_arrival = 0.0
        self.wait_timeout = 0.0
        self.status = 0
        self.wpt = False
        self.prev_wpt = False
        self.linear_vel = 0.0
        self.angular_vel = 0.0
        self.pos = False
        self.bearing = False

        rospy.loginfo(rospy.get_name() + ": Start")
        self.quaternion = np.empty((4,), dtype=np.float64)

        # get parameters
        self.debug = rospy.get_param("~print_debug_information", "true")
        if self.debug:
            rospy.loginfo(rospy.get_name() + ": Debug enabled")
        self.status_publish_interval = rospy.get_param("~status_publish_interval", 0)
        self.pid_publish_interval = rospy.get_param("~pid_publish_interval", 0)

        # get topic names
        self.pose_topic = rospy.get_param("~pose_sub", "/fmKnowledge/pose")
        self.automode_topic = rospy.get_param("~automode_sub", "/fmDecision/automode")
        HMI_sub_topic = rospy.get_param("~hmi_sub", "/fmDecision/hmi")
        ROUTEPT_topic = rospy.get_param("~routept_sub", "/fmPlan/route_point")
        self.cmdvel_topic = rospy.get_param("~cmd_vel_pub", "/fmCommand/cmd_vel")
        self.implement_topic = rospy.get_param("~implement_pub", "/fmCommand/implement")
        self.wptnav_status_topic = rospy.get_param("~status_pub", "/fmInformation/wptnav_status")
        self.pid_topic = rospy.get_param("~pid_pub", "/fmInformation/wptnav_pid")

        # setup publish topics
        self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
        self.twist = TwistStamped()
        self.implement_pub = rospy.Publisher(self.implement_topic, FloatStamped)
        self.implement = FloatStamped()
        self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status)
        self.wptnav_status = waypoint_navigation_status()
        self.status_publish_count = 0
        self.pid_pub = rospy.Publisher(self.pid_topic, FloatArrayStamped)
        self.pid = FloatArrayStamped()
        self.pid_publish_count = 0

        # configure waypoint navigation
        self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2)  # [m]
        drive_kp = rospy.get_param("~drive_kp", 1.0)
        drive_ki = rospy.get_param("~drive_ki", 0.0)
        drive_kd = rospy.get_param("~drive_kd", 0.0)
        drive_ff = rospy.get_param("~drive_feed_forward", 0.0)
        drive_max_output = rospy.get_param("~drive_max_output", 0.3)
        turn_kp = rospy.get_param("~turn_kp", 1.0)
        turn_ki = rospy.get_param("~turn_ki", 0.0)
        turn_kd = rospy.get_param("~turn_kd", 0.2)
        turn_ff = rospy.get_param("~turn_feed_forward", 0.0)
        turn_max_output = rospy.get_param("~turn_max_output", 0.5)

        max_linear_vel = rospy.get_param("~max_linear_velocity", 0.4)
        max_angular_vel = rospy.get_param("~max_angular_velocity", 0.4)

        self.wpt_def_nav_mode = ROUTEPT_NAV_MODE_PP
        nav_mode_str = rospy.get_param("~wpt_default_nav_mode", "PP")
        if nav_mode_str == "AB":
            self.wpt_def_nav_mode = ROUTEPT_NAV_MODE_AB

        self.wpt_tolerance = rospy.get_param("~wpt_default_tolerance", 0.5)
        self.wpt_def_drive_vel = rospy.get_param("~wpt_default_drive_velocity", 0.5)
        self.wpt_def_turn_vel = rospy.get_param("~wpt_default_turn_velocity", 0.3)
        self.wpt_def_pause = rospy.get_param("~wpt_default_wait_after_arrival", 0.0)
        self.wpt_def_implement = rospy.get_param("~wpt_default_implement_command", 0.0)

        target_ahead = rospy.get_param("~target_ahead", 1.0)
        turn_start_at_heading_err = rospy.get_param("~turn_start_at_heading_err", 20.0)
        turn_stop_at_heading_err = rospy.get_param("~turn_stop_at_heading_err", 2.0)
        ramp_drive_vel_at_dist = rospy.get_param("~ramp_drive_velocity_at_distance", 1.0)
        ramp_min_drive_vel = rospy.get_param("~ramp_min_drive_velocity", 0.1)
        ramp_turn_vel_at_angle = rospy.get_param("~ramp_turn_velocity_at_angle", 25.0)
        ramp_min_turn_vel = rospy.get_param("~ramp_min_turn_velocity", 0.05)
        stop_nav_at_dist = rospy.get_param("~stop_navigating_at_distance", 0.1)

        self.wptnav = waypoint_navigation(
            self.update_rate,
            self.w_dist,
            drive_kp,
            drive_ki,
            drive_kd,
            drive_ff,
            drive_max_output,
            turn_kp,
            turn_ki,
            turn_kd,
            turn_ff,
            turn_max_output,
            max_linear_vel,
            max_angular_vel,
            self.wpt_tolerance,
            target_ahead,
            turn_start_at_heading_err,
            turn_stop_at_heading_err,
            ramp_drive_vel_at_dist,
            ramp_min_drive_vel,
            ramp_turn_vel_at_angle,
            ramp_min_turn_vel,
            stop_nav_at_dist,
            self.debug,
        )

        self.wptlist = waypoint_list(
            self.wpt_def_nav_mode,
            self.wpt_def_drive_vel,
            self.wpt_def_turn_vel,
            self.wpt_def_pause,
            self.wpt_def_implement,
        )
        self.load_wpt_list()  # load locally saved waypoint list if it exist

        # setup subscription topic callbacks
        rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
        rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
        rospy.Subscriber(HMI_sub_topic, StringArrayStamped, self.on_hmi_message)
        rospy.Subscriber(ROUTEPT_topic, RoutePt, self.on_routept_message)

        # call updater function
        self.r = rospy.Rate(self.update_rate)
        self.updater()
	def __init__(self):
		# defines
		self.update_rate = 20 # set update frequency [Hz]
		self.IMPLEMENT_INVALID = -10000000.0
		self.STATE_IDLE = 0
		self.STATE_NAVIGATE = 1
		self.STATE_WAIT = 2
		self.state = self.STATE_IDLE
		self.state_prev = self.state
		self.automode_warn = False
		self.wait_after_arrival = 0.0
		self.wait_timeout = 0.0
		self.status = 0
		self.wpt = False
		self.prev_wpt = False
		self.linear_vel = 0.0
		self.angular_vel = 0.0
		self.pos = False
		self.bearing = False

		rospy.loginfo(rospy.get_name() + ": Start")
		self.quaternion = np.empty((4, ), dtype=np.float64)
		self.wii_a = False
		self.wii_a_changed = False
		self.wii_plus = False
		self.wii_plus_changed = False
		self.wii_minus = False
		self.wii_minus_changed = False
		self.wii_up = False
		self.wii_up_changed = False
		self.wii_down = False
		self.wii_down_changed = False
		self.wii_left = False
		self.wii_left_changed = False
		self.wii_right = False
		self.wii_right_changed = False
		self.wii_home = False
		self.wii_home_changed = False

		# get parameters
		self.debug = rospy.get_param("~print_debug_information", 'true') 
 		if self.debug:
			rospy.loginfo(rospy.get_name() + ": Debug enabled")
		self.status_publish_interval = rospy.get_param("~status_publish_interval", 0) 

		# get topic names
		self.automode_topic = rospy.get_param("~automode_sub",'/fmDecision/automode')
		self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
		self.joy_topic = rospy.get_param("~joy_sub",'/fmLib/joy')
		self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",'/fmCommand/cmd_vel')
		self.implement_topic = rospy.get_param("~implement_pub",'/fmCommand/implement')
		self.wptnav_status_topic = rospy.get_param("~status_pub",'/fmData/wptnav_status')

		# setup publish topics
		self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
		self.twist = TwistStamped()
		self.implement_pub = rospy.Publisher(self.implement_topic, FloatStamped)
		self.implement = FloatStamped()
		self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status)
		self.wptnav_status = waypoint_navigation_status()
		self.status_publish_count = 0

		# configure waypoint navigation
		drive_kp = rospy.get_param("~drive_kp", 1.0)
		drive_ki = rospy.get_param("~drive_ki", 0.0)
		drive_kd = rospy.get_param("~drive_kd", 0.0)
		drive_max_output = rospy.get_param("~drive_max_output", 0.3)
		turn_kp = rospy.get_param("~turn_kp", 1.0)
		turn_ki = rospy.get_param("~turn_ki", 0.0)
		turn_kd = rospy.get_param("~turn_kd", 0.0)
		turn_max_output = rospy.get_param("~turn_max_output", 0.5)

		max_linear_vel = rospy.get_param("~max_linear_velocity", 0.4)
		max_angular_vel = rospy.get_param("~max_angular_velocity", 0.4)

		self.wpt_def_tolerance = rospy.get_param("~wpt_default_tolerance", 0.5)
		self.wpt_def_drive_vel = rospy.get_param("~wpt_default_drive_velocity", 0.5)
		self.wpt_def_turn_vel = rospy.get_param("~wpt_default_turn_velocity", 0.3)
		self.wpt_def_wait_after_arrival = rospy.get_param("~wpt_default_wait_after_arrival", 0.0)
		self.wpt_def_implement = rospy.get_param("~wpt_default_implement_command", 0.0)

		target_distance = rospy.get_param("~target_distance", 1.0)
		turn_start_at_heading_err = rospy.get_param("~turn_start_at_heading_err", 20.0)
		turn_stop_at_heading_err = rospy.get_param("~turn_stop_at_heading_err", 2.0)
		ramp_drive_vel_at_dist = rospy.get_param("~ramp_drive_velocity_at_distance", 1.0)
		ramp_min_drive_vel = rospy.get_param("~ramp_min_drive_velocity", 0.1)
		ramp_turn_vel_at_angle = rospy.get_param("~ramp_turn_velocity_at_angle", 25.0)
		ramp_min_turn_vel = rospy.get_param("~ramp_min_turn_velocity", 0.05)

		self.wptnav = waypoint_navigation(self.update_rate, drive_kp, drive_ki, drive_kd, drive_max_output, turn_kp, turn_ki, turn_kd, turn_max_output, max_linear_vel, max_angular_vel, self.wpt_def_tolerance, self.wpt_def_drive_vel, self.wpt_def_turn_vel, target_distance, turn_start_at_heading_err, turn_stop_at_heading_err, ramp_drive_vel_at_dist, ramp_min_drive_vel, ramp_turn_vel_at_angle, ramp_min_turn_vel, self.debug)

		self.wptlist = waypoint_list()
		self.wptlist_loaded = False

		# setup subscription topic callbacks
		rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
		rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
		rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)

		# call updater function
		self.r = rospy.Rate(self.update_rate)
		self.updater()
예제 #5
0
    def __init__(self):
        # defines
        self.update_rate = 20  # set update frequency [Hz]
        self.IMPLEMENT_INVALID = -10000000.0
        self.STATE_IDLE = 0
        self.STATE_NAVIGATE = 1
        self.STATE_WAIT = 2
        self.state = self.STATE_IDLE
        self.state_prev = self.state
        self.automode_warn = False
        self.wait_after_arrival = 0.0
        self.wait_timeout = 0.0
        self.status = 0
        self.wpt = False
        self.prev_wpt = False
        self.linear_vel = 0.0
        self.angular_vel = 0.0
        self.pos = False
        self.bearing = False

        rospy.loginfo(rospy.get_name() + ": Start")
        self.quaternion = np.empty((4, ), dtype=np.float64)
        self.wii_a = False
        self.wii_a_changed = False
        self.wii_plus = False
        self.wii_plus_changed = False
        self.wii_minus = False
        self.wii_minus_changed = False
        self.wii_up = False
        self.wii_up_changed = False
        self.wii_down = False
        self.wii_down_changed = False
        self.wii_left = False
        self.wii_left_changed = False
        self.wii_right = False
        self.wii_right_changed = False
        self.wii_home = False
        self.wii_home_changed = False

        # get parameters
        self.debug = rospy.get_param("~print_debug_information", 'true')
        if self.debug:
            rospy.loginfo(rospy.get_name() + ": Debug enabled")
        self.status_publish_interval = rospy.get_param(
            "~status_publish_interval", 0)
        self.pid_publish_interval = rospy.get_param("~pid_publish_interval", 0)

        # get topic names
        self.automode_topic = rospy.get_param("~automode_sub",
                                              '/fmDecision/automode')
        self.pose_topic = rospy.get_param("~pose_sub", '/fmKnowledge/pose')
        self.joy_topic = rospy.get_param("~joy_sub", '/fmLib/joy')
        self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",
                                            '/fmCommand/cmd_vel')
        self.implement_topic = rospy.get_param("~implement_pub",
                                               '/fmCommand/implement')
        self.wptnav_status_topic = rospy.get_param(
            "~status_pub", '/fmInformation/wptnav_status')
        self.pid_topic = rospy.get_param("~pid_pub",
                                         '/fmInformation/wptnav_pid')

        # setup publish topics
        self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
        self.twist = TwistStamped()
        self.implement_pub = rospy.Publisher(self.implement_topic,
                                             FloatStamped)
        self.implement = FloatStamped()
        self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic,
                                                 waypoint_navigation_status)
        self.wptnav_status = waypoint_navigation_status()
        self.status_publish_count = 0
        self.pid_pub = rospy.Publisher(self.pid_topic, FloatArrayStamped)
        self.pid = FloatArrayStamped()
        self.pid_publish_count = 0

        # configure waypoint navigation
        self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2)  # [m]
        drive_kp = rospy.get_param("~drive_kp", 1.0)
        drive_ki = rospy.get_param("~drive_ki", 0.0)
        drive_kd = rospy.get_param("~drive_kd", 0.0)
        drive_ff = rospy.get_param("~drive_feed_forward", 0.0)
        drive_max_output = rospy.get_param("~drive_max_output", 0.3)
        turn_kp = rospy.get_param("~turn_kp", 1.0)
        turn_ki = rospy.get_param("~turn_ki", 0.0)
        turn_kd = rospy.get_param("~turn_kd", 0.2)
        turn_ff = rospy.get_param("~turn_feed_forward", 0.0)
        turn_max_output = rospy.get_param("~turn_max_output", 0.5)

        max_linear_vel = rospy.get_param("~max_linear_velocity", 0.4)
        max_angular_vel = rospy.get_param("~max_angular_velocity", 0.4)

        self.wpt_def_tolerance = rospy.get_param("~wpt_default_tolerance", 0.5)
        self.wpt_def_drive_vel = rospy.get_param("~wpt_default_drive_velocity",
                                                 0.5)
        self.wpt_def_turn_vel = rospy.get_param("~wpt_default_turn_velocity",
                                                0.3)
        self.wpt_def_wait_after_arrival = rospy.get_param(
            "~wpt_default_wait_after_arrival", 0.0)
        self.wpt_def_implement = rospy.get_param(
            "~wpt_default_implement_command", 0.0)

        turn_start_at_heading_err = rospy.get_param(
            "~turn_start_at_heading_err", 20.0)
        turn_stop_at_heading_err = rospy.get_param("~turn_stop_at_heading_err",
                                                   2.0)
        ramp_drive_vel_at_dist = rospy.get_param(
            "~ramp_drive_velocity_at_distance", 1.0)
        ramp_min_drive_vel = rospy.get_param("~ramp_min_drive_velocity", 0.1)
        ramp_turn_vel_at_angle = rospy.get_param(
            "~ramp_turn_velocity_at_angle", 25.0)
        ramp_min_turn_vel = rospy.get_param("~ramp_min_turn_velocity", 0.05)
        stop_nav_at_dist = rospy.get_param("~stop_navigating_at_distance", 0.1)

        self.wptnav = waypoint_navigation(
            self.update_rate, self.w_dist, drive_kp, drive_ki, drive_kd,
            drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd, turn_ff,
            turn_max_output, max_linear_vel, max_angular_vel,
            self.wpt_def_tolerance, self.wpt_def_drive_vel,
            self.wpt_def_turn_vel, turn_start_at_heading_err,
            turn_stop_at_heading_err, ramp_drive_vel_at_dist,
            ramp_min_drive_vel, ramp_turn_vel_at_angle, ramp_min_turn_vel,
            stop_nav_at_dist, self.debug)

        self.wptlist = waypoint_list()
        self.wptlist_loaded = False

        # setup subscription topic callbacks
        rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
        rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
        rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)

        # call updater function
        self.r = rospy.Rate(self.update_rate)
        self.updater()
	def __init__(self):
		# defines
		self.update_rate = 20 # set update frequency [Hz]
		self.IMPLEMENT_INVALID = -10000000.0
		self.STATE_IDLE = 0
		self.STATE_NAVIGATE = 1
		self.STATE_WAIT = 2
		self.state = self.STATE_IDLE
		self.state_prev = self.state
		self.BHV_RC = 'RC' # remote control (platform behaviour)
		self.BHV_NAV = 'NAV' # waypoint navigation (platform behaviour)
		self.platform_behaviour_warn = False
		self.wait_after_arrival = 0.0
		self.wait_timeout = 0.0
		self.status = 0
		self.wpt = False
		self.prev_wpt = False
		self.linear_vel = 0.0
		self.angular_vel = 0.0
		self.pos = False
		self.bearing = False

		rospy.loginfo(rospy.get_name() + ": Start")
		self.quaternion = np.empty((4, ), dtype=np.float64)
		self.hmi_digital_joy_a_up = False
		self.hmi_digital_joy_a_up_changed = False
		self.hmi_digital_joy_a_down = False
		self.hmi_digital_joy_a_down_changed = False

		# get parameters
		self.debug = rospy.get_param("~print_debug_information", 'true') 
 		if self.debug:
			rospy.loginfo(rospy.get_name() + ": Debug enabled")
		self.status_publish_interval = rospy.get_param("~status_publish_interval", 0) 
		self.pid_publish_interval = rospy.get_param("~pid_publish_interval", 0) 

		# get topic names
		self.platform_behaviour_topic = rospy.get_param("~platform_behaviour_sub",'/fmDecision/platform_behaviour')
		self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
		topic_rc = rospy.get_param("~remote_control_sub",'/fmHMI/remote_control')
		self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",'/fmCommand/cmd_vel')
		self.implement_topic = rospy.get_param("~implement_pub",'/fmCommand/implement')
		self.wptnav_status_topic = rospy.get_param("~status_pub",'/fmInformation/wptnav_status')
		self.pid_topic = rospy.get_param("~pid_pub",'/fmInformation/wptnav_pid')

		# setup publish topics
		self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped, queue_size=1)
		self.twist = TwistStamped()
		self.implement_pub = rospy.Publisher(self.implement_topic, FloatStamped, queue_size=1)
		self.implement = FloatStamped()
		self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status, queue_size=5)
		self.wptnav_status = waypoint_navigation_status()
		self.status_publish_count = 0
		self.pid_pub = rospy.Publisher(self.pid_topic, FloatArrayStamped, queue_size=5)
		self.pid = FloatArrayStamped()
		self.pid_publish_count = 0

		# configure waypoint navigation
		self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2) # [m]
		drive_kp = rospy.get_param("~drive_kp", 1.0)
		drive_ki = rospy.get_param("~drive_ki", 0.0)
		drive_kd = rospy.get_param("~drive_kd", 0.0)
		drive_ff = rospy.get_param("~drive_feed_forward", 0.0)
		drive_max_output = rospy.get_param("~drive_max_output", 0.3)
		turn_kp = rospy.get_param("~turn_kp", 1.0)
		turn_ki = rospy.get_param("~turn_ki", 0.0)
		turn_kd = rospy.get_param("~turn_kd", 0.2)
		turn_ff = rospy.get_param("~turn_feed_forward", 0.0)
		turn_max_output = rospy.get_param("~turn_max_output", 0.5)

		max_linear_vel = rospy.get_param("~max_linear_velocity", 0.4)
		max_angular_vel = rospy.get_param("~max_angular_velocity", 0.4)

		self.wpt_def_tolerance = rospy.get_param("~wpt_default_tolerance", 0.5)
		self.wpt_def_drive_vel = rospy.get_param("~wpt_default_drive_velocity", 0.5)
		self.wpt_def_turn_vel = rospy.get_param("~wpt_default_turn_velocity", 0.3)
		self.wpt_def_wait_after_arrival = rospy.get_param("~wpt_default_wait_after_arrival", 0.0)
		self.wpt_def_implement = rospy.get_param("~wpt_default_implement_command", 0.0)

		target_ahead = rospy.get_param("~target_ahead", 1.0)
		turn_start_at_heading_err = rospy.get_param("~turn_start_at_heading_err", 20.0)
		turn_stop_at_heading_err = rospy.get_param("~turn_stop_at_heading_err", 2.0)
		ramp_drive_vel_at_dist = rospy.get_param("~ramp_drive_velocity_at_distance", 1.0)
		ramp_min_drive_vel = rospy.get_param("~ramp_min_drive_velocity", 0.1)
		ramp_turn_vel_at_angle = rospy.get_param("~ramp_turn_velocity_at_angle", 25.0)
		ramp_min_turn_vel = rospy.get_param("~ramp_min_turn_velocity", 0.05)
		stop_nav_at_dist = rospy.get_param("~stop_navigating_at_distance", 0.1)

		self.wptnav = waypoint_navigation(self.update_rate, self.w_dist, drive_kp, drive_ki, drive_kd, drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd, turn_ff, turn_max_output, max_linear_vel, max_angular_vel, self.wpt_def_tolerance, self.wpt_def_drive_vel, self.wpt_def_turn_vel, target_ahead, turn_start_at_heading_err, turn_stop_at_heading_err, ramp_drive_vel_at_dist, ramp_min_drive_vel, ramp_turn_vel_at_angle, ramp_min_turn_vel, stop_nav_at_dist, self.debug)

		self.wptlist = waypoint_list()
		self.wptlist_loaded = False

		# setup subscription topic callbacks
		rospy.Subscriber(self.platform_behaviour_topic, StringStamped, self.on_platform_behaviour_message)
		rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
		rospy.Subscriber(topic_rc, RemoteControl, self.on_rc_message)

		# call updater function
		self.r = rospy.Rate(self.update_rate)
		self.updater()
	def __init__(self):
		# defines
		self.update_rate = 20 # set update frequency [Hz]
		self.automode = False
		self.automode_prev = False
		self.status = 0
		self.wpt = False
		self.prev_wpt = False
		self.linear_speed = 0.0
		self.angular_speed = 0.0
		self.pos = False
		self.bearing = False

		rospy.loginfo(rospy.get_name() + ": Start")
		self.quaternion = np.empty((4, ), dtype=np.float64)
		self.wii_a = False
		self.wii_a_changed = False
		self.wii_plus = False
		self.wii_plus_changed = False
		self.wii_minus = False
		self.wii_minus_changed = False
		self.wii_up = False
		self.wii_up_changed = False
		self.wii_down = False
		self.wii_down_changed = False
		self.wii_left = False
		self.wii_left_changed = False
		self.wii_right = False
		self.wii_right_changed = False
		self.wii_home = False
		self.wii_home_changed = False

		# get parameters
		self.debug = rospy.get_param("~print_debug_information", 'true') 
 		if self.debug:
			rospy.loginfo(rospy.get_name() + ": Debug enabled")
		self.status_publish_interval = rospy.get_param("~status_publish_interval", 0) 

		# get topic names
		self.automode_topic = rospy.get_param("~automode_sub",'/fmDecisionMakers/automode')
		self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
		self.joy_topic = rospy.get_param("~joy_sub",'/fmLib/joy')
		self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",'/fmCommand/cmd_vel')
		self.wptnav_status_topic = rospy.get_param("~status_pub",'/fmData/wptnav_status')

		# setup publish topics
		self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
		self.twist = TwistStamped()
		self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status)
		self.wptnav_status = waypoint_navigation_status()
		self.status_publish_count = 0

		# configure waypoint navigation
		drive_kp = rospy.get_param("~drive_kp", 1.0)
		drive_ki = rospy.get_param("~drive_ki", 0.0)
		drive_kd = rospy.get_param("~drive_kd", 0.0)
		drive_integral_max = rospy.get_param("~drive_integral_max", 1.0)
		turn_kp = rospy.get_param("~turn_kp", 1.0)
		turn_ki = rospy.get_param("~turn_ki", 0.0)
		turn_kd = rospy.get_param("~turn_kd", 0.0)
		turn_integral_max = rospy.get_param("~turn_integral_max", 1.0)

		max_linear_velocity = rospy.get_param("~max_linear_velocity", 0.4)
		max_angular_velocity = rospy.get_param("~max_angular_velocity", 0.4)

		self.wpt_tolerance = rospy.get_param("~wpt_tolerance", 0.5)
		wpt_target_distance = rospy.get_param("~wpt_target_distance", 1.0)
		wpt_turn_start_at_heading_err = rospy.get_param("~wpt_turn_start_at_heading_err", 20.0)
		wpt_turn_stop_at_heading_err = rospy.get_param("~wpt_turn_stop_at_heading_err", 1.0)
		self.wpt_linear_velocity = rospy.get_param("~wpt_linear_velocity", 0.5)
		wpt_ramp_down_velocity_at_distance = rospy.get_param("~wpt_ramp_down_velocity_at_distance", 1.0)
		wpt_ramp_down_minimum_velocity = rospy.get_param("~wpt_ramp_down_minimum_velocity", 0.3)

		self.wptnav = waypoint_navigation(self.update_rate, drive_kp, drive_ki, drive_kd, drive_integral_max, turn_kp, turn_ki, turn_kd, turn_integral_max, max_linear_velocity, max_angular_velocity, self.wpt_tolerance, wpt_target_distance, wpt_turn_start_at_heading_err, wpt_turn_stop_at_heading_err, self.wpt_linear_velocity, wpt_ramp_down_velocity_at_distance, wpt_ramp_down_minimum_velocity, self.debug)

		self.wptlist = waypoint_list()
		self.wptlist_loaded = False

		# setup subscription topic callbacks
		rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
		rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
		rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)

		# call updater function
		self.r = rospy.Rate(self.update_rate)
		self.updater()
    def __init__(self):
        # defines
        self.update_rate = 20  # set update frequency [Hz]
        self.automode = False
        self.automode_prev = False
        self.status = 0
        self.wpt = False
        self.prev_wpt = False
        self.linear_speed = 0.0
        self.angular_speed = 0.0
        self.pos = False
        self.bearing = False
        self.implement_state = S_IMP_IDLE

        rospy.loginfo(rospy.get_name() + ": Start")
        self.quaternion = np.empty((4, ), dtype=np.float64)
        self.wii_a = False
        self.wii_a_changed = False
        self.wii_home = False
        self.wii_home_changed = False
        self.wii_up = False
        self.wii_up_changed = False
        self.wii_down = False
        self.wii_down_changed = False
        self.wii_left = False
        self.wii_left_changed = False
        self.wii_right = False
        self.wii_right_changed = False

        # get parameters
        self.debug = rospy.get_param("~print_debug_information", 'true')
        if self.debug:
            rospy.loginfo(rospy.get_name() + ": Debug enabled")
        self.status_publish_interval = rospy.get_param(
            "~status_publish_interval", 0)

        # get topic names
        self.automode_topic = rospy.get_param("~automode_sub",
                                              '/fmDecisionMakers/automode')
        self.pose_topic = rospy.get_param("~pose_sub", '/fmKnowledge/pose')
        self.joy_topic = rospy.get_param("~joy_sub", '/fmLib/joy')
        self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",
                                            '/fmCommand/cmd_vel')
        self.wptnav_status_topic = rospy.get_param("~status_pub",
                                                   '/fmData/wptnav_status')

        # setup subscription topic callbacks
        rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
        rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
        rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)

        # setup publish topics
        self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
        self.twist = TwistStamped()
        self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic,
                                                 waypoint_navigation_status)
        self.wptnav_status = waypoint_navigation_status()
        self.status_publish_count = 0

        # configure waypoint navigation
        drive_kp = rospy.get_param("~drive_kp", 1.0)
        drive_ki = rospy.get_param("~drive_ki", 0.0)
        drive_kd = rospy.get_param("~drive_kd", 0.0)
        drive_integral_max = rospy.get_param("~drive_integral_max", 1.0)
        turn_kp = rospy.get_param("~turn_kp", 1.0)
        turn_ki = rospy.get_param("~turn_ki", 0.0)
        turn_kd = rospy.get_param("~turn_kd", 0.0)
        turn_integral_max = rospy.get_param("~turn_integral_max", 1.0)

        max_linear_velocity = rospy.get_param("~max_linear_velocity", 0.4)
        max_angular_velocity = rospy.get_param("~max_angular_velocity", 0.4)

        self.wpt_tolerance = rospy.get_param("~wpt_tolerance", 0.5)
        wpt_target_distance = rospy.get_param("~wpt_target_distance", 1.0)
        wpt_turn_start_at_heading_err = rospy.get_param(
            "~wpt_turn_start_at_heading_err", 20.0)
        wpt_turn_stop_at_heading_err = rospy.get_param(
            "~wpt_turn_stop_at_heading_err", 1.0)
        self.wpt_linear_velocity = rospy.get_param("~wpt_linear_velocity", 0.5)
        wpt_ramp_down_velocity_at_distance = rospy.get_param(
            "~wpt_ramp_down_velocity_at_distance", 1.0)
        wpt_ramp_down_minimum_velocity = rospy.get_param(
            "~wpt_ramp_down_minimum_velocity", 0.3)

        self.wptnav = waypoint_navigation(
            self.update_rate, drive_kp, drive_ki, drive_kd, drive_integral_max,
            turn_kp, turn_ki, turn_kd, turn_integral_max, max_linear_velocity,
            max_angular_velocity, self.wpt_tolerance, wpt_target_distance,
            wpt_turn_start_at_heading_err, wpt_turn_stop_at_heading_err,
            self.wpt_linear_velocity, wpt_ramp_down_velocity_at_distance,
            wpt_ramp_down_minimum_velocity, self.debug)

        # configure casmo
        casmo_width = rospy.get_param("~casmo_width", 0.8)
        casmo_default_length = rospy.get_param("~casmo_default_length", 20)
        self.casmo = area_coverage_casmo()
        self.casmo.param_set_width(casmo_width)
        self.casmo.param_set_default_length(casmo_default_length)

        # call updater function
        self.r = rospy.Rate(self.update_rate)
        self.updater()
예제 #9
0
    def __init__(self):
        # defines
        self.update_rate = 20  # set update frequency [Hz]
        self.automode = False
        self.automode_prev = False
        self.status = 0
        self.wpt = False
        self.prev_wpt = False
        self.linear_speed = 0.0
        self.angular_speed = 0.0

        rospy.loginfo(rospy.get_name() + ": Start")
        self.quaternion = np.empty((4, ), dtype=np.float64)
        self.wii_a = False
        self.wii_a_changed = False
        self.wii_home = False
        self.wii_home_changed = False
        self.wii_up = False
        self.wii_up_changed = False
        self.wii_down = False
        self.wii_down_changed = False

        # get parameters
        self.debug = rospy.get_param("~print_debug_information", 'true')
        if self.debug:
            rospy.loginfo(rospy.get_name() + ": Debug enabled")
        self.status_publish_interval = rospy.get_param(
            "~status_publish_interval", 0)

        # get topic names
        self.automode_topic = rospy.get_param("~automode_sub",
                                              '/fmDecisionMakers/automode')
        self.pose_topic = rospy.get_param("~pose_sub", '/fmKnowledge/pose')
        self.joy_topic = rospy.get_param("~joy_sub", '/joy')
        self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",
                                            '/fmCommand/cmd_vel')
        self.wptnav_status_topic = rospy.get_param("~status_pub",
                                                   '/fmData/wptnav_status')

        # setup subscription topic callbacks
        rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
        rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
        rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)

        # setup publish topics
        self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
        self.twist = TwistStamped()
        self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic,
                                                 waypoint_navigation_status)
        self.wptnav_status = waypoint_navigation_status()
        self.status_publish_count = 0

        # configure waypoint navigation
        self.wptlist = waypoint_list()
        self.wptnav = waypoint_navigation(self.update_rate, self.debug)
        self.wptlist_loaded = False

        # Configure wriggle
        turn_angle_left = rospy.get_param("~turn_angle_left", pi / 6)
        turn_angle_right = rospy.get_param("~turn_angle_right", pi / 6 * -1)
        turn_max_speed = rospy.get_param("~max_angular_velocity", 0.6)
        speed_gain = rospy.get_param("~turn_angular_speed_gain", 100)
        sensor_penalty_distance = rospy.get_param("~sensor_penalty_distance",
                                                  5)
        self.wriggle = Wriggle(turn_angle_left, turn_angle_right,
                               turn_max_speed, speed_gain,
                               sensor_penalty_distance)

        # Configure wads sensor
        self.wads_topic = rospy.get_param("~wads_sub", '/fmInformation/wads')
        self.wads_threshold = rospy.get_param("~wads_threshold", 3)
        rospy.Subscriber(self.wads_topic, Float64, self.on_wads_sensor_msg)
        self.wads_value = 0.0

        # call updater function
        self.r = rospy.Rate(self.update_rate)
        self.updater()
예제 #10
0
	def __init__(self):
		# defines
		self.update_rate = 20 # set update frequency [Hz]
		self.IMPLEMENT_INVALID = -10000000.0
		self.STATE_IDLE = 0
		self.STATE_NAVIGATE = 1
		self.STATE_WAIT = 2
		self.state = self.STATE_IDLE
		self.state_prev = self.state
		self.automode_warn = False
		self.no_route_plan_warn = False
		self.wait_after_arrival = 0.0
		self.wait_timeout = 0.0
		self.status = 0
		self.wpt = False
		self.prev_wpt = False
		self.linear_vel = 0.0
		self.angular_vel = 0.0
		self.pos = False
		self.bearing = False

		# HMI defines
		self.HMI_ID_DEADMAN = 0
		self.HMI_ID_MODE = 1
		self.HMI_ID_GOTO_WAYPOINT = 2
		self.HMI_MODE_MANUAL = 0
		self.HMI_MODE_AUTO = 1

		# route point defines
		self.ROUTEPT_CMD_DELETE = 0
		self.ROUTEPT_CMD_DELETE_THEN_APPEND = 1
		self.ROUTEPT_CMD_APPEND = 2
		self.ROUTEPT_MODE_PP = 0
		self.ROUTEPT_MODE_MCTE = 1
		self.ROUTEPT_INVALID_DATA = -1000000

		rospy.loginfo(rospy.get_name() + ": Start")
		self.quaternion = np.empty((4, ), dtype=np.float64)

		# get parameters
		self.debug = rospy.get_param("~print_debug_information", 'true') 
 		if self.debug:
			rospy.loginfo(rospy.get_name() + ": Debug enabled")
		self.status_publish_interval = rospy.get_param("~status_publish_interval", 0) 
		self.pid_publish_interval = rospy.get_param("~pid_publish_interval", 0) 

		# get topic names
		self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
		self.automode_topic = rospy.get_param("~automode_sub",'/fmDecision/automode')
		self.hmi_sub_topic = rospy.get_param("~hmi_sub",'/fmDecision/hmi')
		self.routept_topic = rospy.get_param("~routept_sub",'/fmPlan/route_point')
		self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",'/fmCommand/cmd_vel')
		self.implement_topic = rospy.get_param("~implement_pub",'/fmCommand/implement')
		self.wptnav_status_topic = rospy.get_param("~status_pub",'/fmInformation/wptnav_status')
		self.pid_topic = rospy.get_param("~pid_pub",'/fmInformation/wptnav_pid')

		# setup publish topics
		self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
		self.twist = TwistStamped()
		self.implement_pub = rospy.Publisher(self.implement_topic, FloatStamped)
		self.implement = FloatStamped()
		self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status)
		self.wptnav_status = waypoint_navigation_status()
		self.status_publish_count = 0
		self.pid_pub = rospy.Publisher(self.pid_topic, FloatArrayStamped)
		self.pid = FloatArrayStamped()
		self.pid_publish_count = 0

		# configure waypoint navigation
		self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2) # [m]
		drive_kp = rospy.get_param("~drive_kp", 1.0)
		drive_ki = rospy.get_param("~drive_ki", 0.0)
		drive_kd = rospy.get_param("~drive_kd", 0.0)
		drive_ff = rospy.get_param("~drive_feed_forward", 0.0)
		drive_max_output = rospy.get_param("~drive_max_output", 0.3)
		turn_kp = rospy.get_param("~turn_kp", 1.0)
		turn_ki = rospy.get_param("~turn_ki", 0.0)
		turn_kd = rospy.get_param("~turn_kd", 0.2)
		turn_ff = rospy.get_param("~turn_feed_forward", 0.0)
		turn_max_output = rospy.get_param("~turn_max_output", 0.5)

		max_linear_vel = rospy.get_param("~max_linear_velocity", 0.4)
		max_angular_vel = rospy.get_param("~max_angular_velocity", 0.4)

		self.wpt_def_tolerance = rospy.get_param("~wpt_default_tolerance", 0.5)
		self.wpt_def_drive_vel = rospy.get_param("~wpt_default_drive_velocity", 0.5)
		self.wpt_def_turn_vel = rospy.get_param("~wpt_default_turn_velocity", 0.3)
		self.wpt_def_wait_after_arrival = rospy.get_param("~wpt_default_wait_after_arrival", 0.0)
		self.wpt_def_implement = rospy.get_param("~wpt_default_implement_command", 0.0)

		target_ahead = rospy.get_param("~target_ahead", 1.0)
		turn_start_at_heading_err = rospy.get_param("~turn_start_at_heading_err", 20.0)
		turn_stop_at_heading_err = rospy.get_param("~turn_stop_at_heading_err", 2.0)
		ramp_drive_vel_at_dist = rospy.get_param("~ramp_drive_velocity_at_distance", 1.0)
		ramp_min_drive_vel = rospy.get_param("~ramp_min_drive_velocity", 0.1)
		ramp_turn_vel_at_angle = rospy.get_param("~ramp_turn_velocity_at_angle", 25.0)
		ramp_min_turn_vel = rospy.get_param("~ramp_min_turn_velocity", 0.05)
		stop_nav_at_dist = rospy.get_param("~stop_navigating_at_distance", 0.1)

		self.wptnav = waypoint_navigation(self.update_rate, self.w_dist, drive_kp, drive_ki, drive_kd, drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd, turn_ff, turn_max_output, max_linear_vel, max_angular_vel, self.wpt_def_tolerance, self.wpt_def_drive_vel, self.wpt_def_turn_vel, target_ahead, turn_start_at_heading_err, turn_stop_at_heading_err, ramp_drive_vel_at_dist, ramp_min_drive_vel, ramp_turn_vel_at_angle, ramp_min_turn_vel, stop_nav_at_dist, self.debug)

		self.wptlist = waypoint_list()
		self. load_wpt_list() # load locally saved waypoint list if it exist

		# setup subscription topic callbacks
		rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
		rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
		rospy.Subscriber(self.hmi_sub_topic, StringArrayStamped, self.on_hmi_message)
		rospy.Subscriber(self.routept_topic, RoutePt, self.on_routept_message)

		# call updater function
		self.r = rospy.Rate(self.update_rate)
		self.updater()
예제 #11
0
	def __init__(self):
		# defines
		self.update_rate = 20 # set update frequency [Hz]
		self.IMPLEMENT_INVALID = -10000000.0
		self.STATE_IDLE = 0
		self.STATE_NAVIGATE = 1
		self.STATE_WAIT = 2
		self.state = self.STATE_IDLE
		self.state_prev = self.state
		self.automode_warn = False
		self.wait_after_arrival = 0.0 # wait after the robot reaches a wpt
		self.wait_timeout = 0.0
		self.status = 0
		self.wpt = False
		self.prev_wpt = False
		self.linear_vel = 0.0
		self.angular_vel = 0.0
		self.pos = False
		self.bearing = False

		rospy.loginfo(rospy.get_name() + ": Start")
		self.quaternion = np.empty((4, ), dtype=np.float64)
		self.wii_a = False
		self.wii_a_changed = False
		self.wii_plus = False
		self.wii_plus_changed = False
		self.wii_minus = False
		self.wii_minus_changed = False
		self.wii_up = False
		self.wii_up_changed = False
		self.wii_down = False
		self.wii_down_changed = False
		self.wii_left = False
		self.wii_left_changed = False
		self.wii_right = False
		self.wii_right_changed = False
		self.wii_home = False
		self.wii_home_changed = False

		# get parameters
		self.debug = rospy.get_param("~print_debug_information", 'true') 
#  		if self.debug:
#              rospy.loginfo(rospy.get_name() + ": Debug enabled")
		self.status_publish_interval = rospy.get_param("~status_publish_interval", 0) 
		self.pid_publish_interval = rospy.get_param("~pid_publish_interval", 0) 

		# get topic names
		self.automode_topic = rospy.get_param("~automode_sub",'/fmDecision/automode')
		self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
		self.joy_topic = rospy.get_param("~joy_sub",'/fmLib/joy')
		self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",'/fmCommand/cmd_vel')
		self.implement_topic = rospy.get_param("~implement_pub",'/fmCommand/implement')
		self.wptnav_status_topic = rospy.get_param("~status_pub",'/fmInformation/wptnav_status')
		self.pid_topic = rospy.get_param("~pid_pub",'/fmInformation/wptnav_pid')

		# setup publish topics
		self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
		self.twist = TwistStamped()
		self.implement_pub = rospy.Publisher(self.implement_topic, FloatStamped)
		self.implement = FloatStamped()
		self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status)
		self.wptnav_status = waypoint_navigation_status()
		self.status_publish_count = 0
		self.pid_pub = rospy.Publisher(self.pid_topic, FloatArrayStamped)
		self.pid = FloatArrayStamped()
		self.pid_publish_count = 0

		# configure waypoint navigation
		self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2) # [m] distance between the two wheels
		drive_kp = rospy.get_param("~drive_kp", 1.0)# driving constant for proportion term in pid controller
		drive_ki = rospy.get_param("~drive_ki", 0.0)# driving constant for integral term in pid controller
		drive_kd = rospy.get_param("~drive_kd", 0.0)# driving constant for differential term in pid controller
		drive_ff = rospy.get_param("~drive_feed_forward", 0.0)# feed forward velocity. It determines the velocity of the robot in response to some stimuli.
                                                              # like for example when a door is opened a feedforward control would heat the room before
                                                              # it gets too cold
		drive_max_output = rospy.get_param("~drive_max_output", 0.3)
		turn_kp = rospy.get_param("~turn_kp", 1.0)# turning const for proportion term in pid controller
		turn_ki = rospy.get_param("~turn_ki", 0.0)# turning const for integral term in pid controller
		turn_kd = rospy.get_param("~turn_kd", 0.2)# turning const for differential term in pid controller
		turn_ff = rospy.get_param("~turn_feed_forward", 0.0)# feed forward term for turning
		turn_max_output = rospy.get_param("~turn_max_output", 0.5)

		max_linear_vel = rospy.get_param("~max_linear_velocity", 0.4)
		max_angular_vel = rospy.get_param("~max_angular_velocity", 0.4)

		self.wpt_def_tolerance = rospy.get_param("~wpt_default_tolerance", 0.5)#default tolerance when the robot reaches the waypoint
		self.wpt_def_drive_vel = rospy.get_param("~wpt_default_drive_velocity", 0.5)# default driive velocity when the robot reaches the wpt
		self.wpt_def_turn_vel = rospy.get_param("~wpt_default_turn_velocity", 0.3)# default turn velocity when the robot reaches the wpt
		self.wpt_def_wait_after_arrival = rospy.get_param("~wpt_default_wait_after_arrival", 0.0)# default wait time after robot reaches the wpt
		self.wpt_def_implement = rospy.get_param("~wpt_default_implement_command", 0.0) # default value for the implement stored in the implement param which is a floatstamped message

		turn_start_at_heading_err = rospy.get_param("~turn_start_at_heading_err", 20.0)# Not Clear
		turn_stop_at_heading_err = rospy.get_param("~turn_stop_at_heading_err", 2.0)# Not Clear
		ramp_drive_vel_at_dist = rospy.get_param("~ramp_drive_velocity_at_distance", 1.0)# velocity of the robot when it reaches a ramp
		ramp_min_drive_vel = rospy.get_param("~ramp_min_drive_velocity", 0.1)# min vel of the robot when it reaches the ramp
		ramp_turn_vel_at_angle = rospy.get_param("~ramp_turn_velocity_at_angle", 25.0)# turn velocity of the robot in a ramp
		ramp_min_turn_vel = rospy.get_param("~ramp_min_turn_velocity", 0.05)# min turn vel of the robot in a ramp
		stop_nav_at_dist = rospy.get_param("~stop_navigating_at_distance", 0.1)# the distance at which the robot should stop navigating. Not clear

		self.wptnav = waypoint_navigation(self.update_rate, self.w_dist, drive_kp, drive_ki, drive_kd, drive_ff, drive_max_output, turn_kp, 
                                          turn_ki, turn_kd, turn_ff, turn_max_output, max_linear_vel, max_angular_vel, self.wpt_def_tolerance, 
                                          self.wpt_def_drive_vel, self.wpt_def_turn_vel, turn_start_at_heading_err, turn_stop_at_heading_err, 
                                          ramp_drive_vel_at_dist, ramp_min_drive_vel, ramp_turn_vel_at_angle, ramp_min_turn_vel,
                                          stop_nav_at_dist, self.debug)# waypoint navigation is called which is another class and these params
                                                                       # are passed to it for performing the actual navigation of the robot 
                                                                       # Initializes the values for wpt navigation and for pid control

		self.wptlist = waypoint_list()# list of waypoints are loaded
		self.wptlist_loaded = False# status whether the waypoints have been loaded or not. Initially set to false

		# setup subscription topic callbacks
		rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
		rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
		rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)

		# call updater function
		self.r = rospy.Rate(self.update_rate)# update rate 20Hz
		self.updater()
예제 #12
0
	def __init__(self):
		# defines
		self.update_rate = 20 # set update frequency [Hz]
		self.update_count = 0
		self.IMPLEMENT_INVALID = -10000000.0
		self.STATE_IDLE = 0
		self.STATE_NAVIGATE = 1
		self.STATE_WAIT = 2
		self.state = self.STATE_IDLE
		self.state_prev = self.state
		self.automode_warn = False
		self.wait_after_arrival = 0.0
		self.wait_timeout = 0.0
		self.status = 0
		self.wpt = False
		self.prev_wpt = False
		self.linear_vel = 0.0
		self.angular_vel = 0.0
		self.pos = False
		self.bearing = False

		rospy.loginfo(rospy.get_name() + ": Start")
		self.quaternion = np.empty((4, ), dtype=np.float64)
		self.wii_a = False
		self.wii_a_changed = False
		self.wii_plus = False
		self.wii_plus_changed = False
		self.wii_minus = False
		self.wii_minus_changed = False
		self.wii_up = False
		self.wii_up_changed = False
		self.wii_down = False
		self.wii_down_changed = False
		self.wii_left = False
		self.wii_left_changed = False
		self.wii_right = False
		self.wii_right_changed = False
		self.wii_home = False
		self.wii_home_changed = False

		# get parameters
		self.debug = rospy.get_param("~print_debug_information", 'true') 
 		if self.debug:
			rospy.loginfo(rospy.get_name() + ": Debug enabled")
		self.status_publish_interval = rospy.get_param("~status_publish_interval", 0) 
		self.pid_publish_interval = rospy.get_param("~pid_publish_interval", 0) 

		# get topic names
		self.automode_topic = rospy.get_param("~automode_sub",'/fmDecision/automode')
		self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
		self.joy_topic = rospy.get_param("~joy_sub",'/fmLib/joy')
		self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",'/fmCommand/cmd_vel')
		self.implement_topic = rospy.get_param("~implement_pub",'/fmCommand/implement')
		self.wptnav_status_topic = rospy.get_param("~status_pub",'/fmInformation/wptnav_status')
		self.pid_topic = rospy.get_param("~pid_pub",'/fmInformation/wptnav_pid')

		# setup publish topics
		self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
		self.twist = TwistStamped()
		self.implement_pub = rospy.Publisher(self.implement_topic, FloatStamped)
		self.implement = FloatStamped()
		self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status)
		self.wptnav_status = waypoint_navigation_status()
		self.status_publish_count = 0
		self.pid_pub = rospy.Publisher(self.pid_topic, FloatArrayStamped)
		self.pid = FloatArrayStamped()
		self.pid_publish_count = 0

		# configure waypoint navigation
		self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2) # [m]
		drive_kp = rospy.get_param("~drive_kp", 1.0)
		drive_ki = rospy.get_param("~drive_ki", 0.0)
		drive_kd = rospy.get_param("~drive_kd", 0.0)
		drive_ff = rospy.get_param("~drive_feed_forward", 0.0)
		drive_max_output = rospy.get_param("~drive_max_output", 0.3)
		turn_kp = rospy.get_param("~turn_kp", 1.0)
		turn_ki = rospy.get_param("~turn_ki", 0.0)
		turn_kd = rospy.get_param("~turn_kd", 0.2)
		turn_ff = rospy.get_param("~turn_feed_forward", 0.0)
		turn_max_output = rospy.get_param("~turn_max_output", 0.5)

		max_linear_vel = rospy.get_param("~max_linear_velocity", 0.4)
		max_angular_vel = rospy.get_param("~max_angular_velocity", 0.4)

		self.wpt_def_tolerance = rospy.get_param("~wpt_default_tolerance", 0.5)
		self.wpt_def_drive_vel = rospy.get_param("~wpt_default_drive_velocity", 0.5)
		self.wpt_def_turn_vel = rospy.get_param("~wpt_default_turn_velocity", 0.3)
		self.wpt_def_wait_after_arrival = rospy.get_param("~wpt_default_wait_after_arrival", 0.0)
		self.wpt_def_implement = rospy.get_param("~wpt_default_implement_command", 0.0)

		turn_start_at_heading_err = rospy.get_param("~turn_start_at_heading_err", 20.0)
		turn_stop_at_heading_err = rospy.get_param("~turn_stop_at_heading_err", 2.0)
		ramp_drive_vel_at_dist = rospy.get_param("~ramp_drive_velocity_at_distance", 1.0)
		ramp_min_drive_vel = rospy.get_param("~ramp_min_drive_velocity", 0.1)
		ramp_turn_vel_at_angle = rospy.get_param("~ramp_turn_velocity_at_angle", 25.0)
		ramp_min_turn_vel = rospy.get_param("~ramp_min_turn_velocity", 0.05)
		stop_nav_at_dist = rospy.get_param("~stop_navigating_at_distance", 0.1)

		self.wptnav = waypoint_navigation(self.update_rate, self.w_dist, drive_kp, drive_ki, drive_kd, drive_ff, drive_max_output, turn_kp, turn_ki, turn_kd, turn_ff, turn_max_output, max_linear_vel, max_angular_vel, self.wpt_def_tolerance, self.wpt_def_drive_vel, self.wpt_def_turn_vel, turn_start_at_heading_err, turn_stop_at_heading_err, ramp_drive_vel_at_dist, ramp_min_drive_vel, ramp_turn_vel_at_angle, ramp_min_turn_vel, stop_nav_at_dist, self.debug)

		# configure casmo
		casmo_width = rospy.get_param("~casmo_width", 0.8)
		casmo_default_length = rospy.get_param("~casmo_default_length", 100)
		self.casmo = area_coverage_casmo()
		self.casmo.param_set_width(casmo_width)
		self.casmo.param_set_default_length(casmo_default_length)

		# Configure wriggle
		turn_angle_left = rospy.get_param("~wads_turn_angle",pi/6)
		turn_angle_right = -turn_angle_left
		speed_gain = rospy.get_param("~wads_turn_angular_speed_gain", 5)
		sensor_penalty_distance = rospy.get_param("~wads_penalty_distance", 5)
		self.wriggle = Wriggle(turn_angle_left, turn_angle_right, self.wpt_def_turn_vel, speed_gain, sensor_penalty_distance)

		# Configure wads sensor
		self.wads_topic = rospy.get_param("~wads_sub",'/fmData/nmea_from_wads')
		self.wads_threshold = rospy.get_param("~wads_threshold", 300)
		rospy.Subscriber(self.wads_topic, nmea, self.on_wads_sensor_msg)

		self.wads_value = [0.0]*10 # buffer length 10
		self.wads_value_updated = False
		self.wads_error = True

		#self.wptlist = waypoint_list()
		#self.wptlist_loaded = False

		# setup subscription topic callbacks
		rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
		rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
		rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)

		# call updater function
		self.r = rospy.Rate(self.update_rate)
		self.updater()
	def __init__(self):
		# defines
		self.update_rate = 20 # set update frequency [Hz]
		self.automode = False
		self.automode_prev = False
		self.status = 0
		self.wpt = False
		self.prev_wpt = False
		self.linear_speed = 0.0
		self.angular_speed = 0.0

		self.pos = False
		self.bearing = False

		rospy.loginfo(rospy.get_name() + ": Start")
		self.quaternion = np.empty((4, ), dtype=np.float64)
		self.wii_a = False
		self.wii_a_changed = False
		self.wii_home = False
		self.wii_home_changed = False
		self.wii_up = False
		self.wii_up_changed = False
		self.wii_down = False
		self.wii_down_changed = False
		self.wii_left = False
		self.wii_left_changed = False
		self.wii_right = False
		self.wii_right_changed = False


		# get parameters
		self.debug = rospy.get_param("~print_debug_information", 'true') 
 		if self.debug:
			rospy.loginfo(rospy.get_name() + ": Debug enabled")
		self.status_publish_interval = rospy.get_param("~status_publish_interval", 0) 

		# get topic names
		self.automode_topic = rospy.get_param("~automode_sub",'/fmDecisionMakers/automode')
		self.pose_topic = rospy.get_param("~pose_sub",'/fmKnowledge/pose')
		self.joy_topic = rospy.get_param("~joy_sub",'/fmHMI/joy')
		self.cmdvel_topic = rospy.get_param("~cmd_vel_pub",'/fmCommand/cmd_vel')
		self.wptnav_status_topic = rospy.get_param("~status_pub",'/fmData/wptnav_status')

		# setup subscription topic callbacks
		rospy.Subscriber(self.automode_topic, Bool, self.on_automode_message)
		rospy.Subscriber(self.pose_topic, Odometry, self.on_pose_message)
		rospy.Subscriber(self.joy_topic, Joy, self.on_joy_message)

		# setup publish topics
		self.cmd_vel_pub = rospy.Publisher(self.cmdvel_topic, TwistStamped)
		self.twist = TwistStamped()
		self.wptnav_status_pub = rospy.Publisher(self.wptnav_status_topic, waypoint_navigation_status)
		self.wptnav_status = waypoint_navigation_status()
		self.status_publish_count = 0

		# configure waypoint navigation and casmo
		casmo_width = rospy.get_param("~casmo_width", 0.8)
		casmo_default_length = rospy.get_param("~casmo_default_length", 100)
		self.max_linear_velocity = rospy.get_param("~max_linear_velocity", 0.5)
		self.wpt_tolerance = rospy.get_param("~wpt_tolerance", 0.5)
		self.wptnav = waypoint_navigation(self.update_rate, self.debug)
		self.casmo = area_coverage_casmo()
		self.casmo.param_set_width(casmo_width)
		self.casmo.param_set_default_length(casmo_default_length)

		# Configure wriggle
		turn_angle_left = rospy.get_param("~turn_angle_left",pi/6)
		turn_angle_right = rospy.get_param("~turn_angle_right",pi/6 * -1)
		turn_max_speed = rospy.get_param("~max_angular_velocity", 0.3)
		speed_gain = rospy.get_param("~turn_angular_speed_gain", 5)
		sensor_penalty_distance = rospy.get_param("~sensor_penalty_distance", 5)
		self.wriggle = Wriggle(turn_angle_left, turn_angle_right, turn_max_speed, speed_gain, sensor_penalty_distance)

		# Configure wads sensor
		self.wads_topic = rospy.get_param("~wads_sub",'/fmInformation/wads')
		self.wads_threshold = rospy.get_param("~wads_threshold", 3)
		rospy.Subscriber(self.wads_topic, Float64, self.on_wads_sensor_msg)
		self.wads_value = 0.0

		# call updater function
		self.r = rospy.Rate(self.update_rate)
		self.updater()