Ejemplo n.º 1
0
    def __init__(self):
        rospy.init_node('turtlebot_state_machine', anonymous=True)
        self.mode = Mode.EXPLORE

        # current nav cmd
        self.cmd_nav = Pose2D()

	# map
	self.map = OccupancyGrid()
	self.map_meta_data = MapMetaData()

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)
	
	# publishers
        self.map_pub = rospy.Publisher('/map', OccupancyGrid, queue_size=10)
	self.map_meta_data_pub = rospy.Publisher('/map_meta_data', MapMetaData, queue_size=10)
	self.cmd_nav_pub = rospy.Publisher('/cmd_nav',Pose2D, queue_size=10)
	
	# subscribers
        #rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        #rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        #rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)

        print "finished init"
Ejemplo n.º 2
0
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0, 0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution = 0.1
        self.plan_horizon = 15

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0., 0.]

        # Robot limits
        self.v_max = 0.2  # maximum velocity
        self.om_max = 0.4  # maximum angular velocity

        self.v_des = 0.12  # desired cruising velocity
        self.theta_start_thresh = 0.05  # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2  # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05

        # trajectory smoothing
        self.spline_alpha = 0.15
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
                                                 self.kdy, self.v_max,
                                                 self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max,
                                              self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path',
                                                    Path,
                                                    queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
                                                     Path,
                                                     queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher(
            '/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)

        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)

        # Section 6

        print "finished init"
Ejemplo n.º 3
0
class Navigator:
    """
    This node handles point to point turtlebot motion, avoiding obstacles.
    It is the sole node that should publish to cmd_vel
    """
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0, 0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution = 0.1
        self.plan_horizon = 15

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0., 0.]

        # Robot limits
        self.v_max = 0.2  # maximum velocity
        self.om_max = 0.4  # maximum angular velocity

        self.v_des = 0.12  # desired cruising velocity
        self.theta_start_thresh = 0.05  # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2  # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05

        # trajectory smoothing
        self.spline_alpha = 0.15
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
                                                 self.kdy, self.v_max,
                                                 self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max,
                                              self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path',
                                                    Path,
                                                    queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
                                                     Path,
                                                     queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher(
            '/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)

        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)

        # Section 6

        print "finished init"

    def dyn_cfg_callback(self, config, level):
        rospy.loginfo(
            "Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}".format(**config))
        self.pose_controller.k1 = config["k1"]
        self.pose_controller.k2 = config["k2"]
        self.pose_controller.k3 = config["k3"]
        return config

    def cmd_nav_callback(self, data):
        """
        loads in goal if different from current goal, and replans
        """
        if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
            self.x_g = data.x
            self.y_g = data.y
            self.theta_g = data.theta
            self.replan()

    def map_md_callback(self, msg):
        """
        receives maps meta data and stores it
        """
        self.map_width = msg.width
        self.map_height = msg.height
        self.map_resolution = msg.resolution
        self.map_origin = (msg.origin.position.x, msg.origin.position.y)

    def map_callback(self, msg):
        """
        receives new map info and updates the map
        """
        self.map_probs = msg.data
        # if we've received the map metadata and have a way to update it:
        if self.map_width > 0 and self.map_height > 0 and len(
                self.map_probs) > 0:
            self.occupancy = StochOccupancyGrid2D(
                self.map_resolution, self.map_width, self.map_height,
                self.map_origin[0], self.map_origin[1], 8, self.map_probs)
            if self.x_g is not None:
                # if we have a goal to plan to, replan
                rospy.loginfo("replanning because of new map")
                self.replan()  # new map, need to replan

    def shutdown_callback(self):
        """
        publishes zero velocities upon rospy shutdown
        """
        cmd_vel = Twist()
        cmd_vel.linear.x = 0.0
        cmd_vel.angular.z = 0.0
        self.nav_vel_pub.publish(cmd_vel)

    def near_goal(self):
        """
        returns whether the robot is close enough in position to the goal to
        start using the pose controller
        """
        return linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g
                                     ])) < self.near_thresh

    def at_goal(self):
        """
        returns whether the robot has reached the goal position with enough
        accuracy to return to idle state
        """
        return (linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g])) <
                self.near_thresh and abs(wrapToPi(self.theta - self.theta_g)) <
                self.at_thresh_theta)

    def aligned(self):
        """
        returns whether robot is aligned with starting direction of path
        (enough to switch to tracking controller)
        """
        return (abs(wrapToPi(self.theta - self.th_init)) <
                self.theta_start_thresh)

    def close_to_plan_start(self):
        return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh
                and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)

    def snap_to_grid(self, x):
        return (self.plan_resolution * round(x[0] / self.plan_resolution),
                self.plan_resolution * round(x[1] / self.plan_resolution))

    def switch_mode(self, new_mode):
        rospy.loginfo("Switching from %s -> %s", self.mode, new_mode)
        self.mode = new_mode

    def publish_planned_path(self, path, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for state in path:
            pose_st = PoseStamped()
            pose_st.pose.position.x = state[0]
            pose_st.pose.position.y = state[1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_smoothed_path(self, traj, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for i in range(traj.shape[0]):
            pose_st = PoseStamped()
            pose_st.pose.position.x = traj[i, 0]
            pose_st.pose.position.y = traj[i, 1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_control(self):
        """
        Runs appropriate controller depending on the mode. Assumes all controllers
        are all properly set up / with the correct goals loaded
        """
        t = self.get_current_plan_time()

        if self.mode == Mode.PARK:
            V, om = self.pose_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.TRACK:
            V, om = self.traj_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.ALIGN:
            V, om = self.heading_controller.compute_control(
                self.x, self.y, self.theta, t)
        else:
            V = 0.
            om = 0.

        cmd_vel = Twist()
        cmd_vel.linear.x = V
        cmd_vel.angular.z = om
        self.nav_vel_pub.publish(cmd_vel)

    def get_current_plan_time(self):
        t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
        return max(0.0, t)  # clip negative time to 0

    def replan(self):
        """
        loads goal into pose controller
        runs planner based on current pose
        if plan long enough to track:
            smooths resulting traj, loads it into traj_controller
            sets self.current_plan_start_time
            sets mode to ALIGN
        else:
            sets mode to PARK
        """
        # Make sure we have a map
        if not self.occupancy:
            rospy.loginfo(
                "Navigator: replanning canceled, waiting for occupancy map.")
            self.switch_mode(Mode.IDLE)
            return

        # Attempt to plan a path
        state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
        state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
        x_init = self.snap_to_grid((self.x, self.y))
        self.plan_start = x_init
        x_goal = self.snap_to_grid((self.x_g, self.y_g))
        problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
                        self.plan_resolution)

        rospy.loginfo("Navigator: computing navigation plan")
        success = problem.solve()
        if not success:
            rospy.loginfo("Planning failed")
            return
        rospy.loginfo("Planning Succeeded")

        planned_path = problem.path

        # Check whether path is too short
        if len(planned_path) < 4:
            rospy.loginfo("Path too short to track")
            self.switch_mode(Mode.PARK)
            return

        # Smooth and generate a trajectory
        traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
                                                self.spline_alpha,
                                                self.traj_dt)

        # If currently tracking a trajectory, check whether new trajectory will take more time to follow
        if self.mode == Mode.TRACK:
            t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
            )

            # Estimate duration of new trajectory
            th_init_new = traj_new[0, 2]
            th_err = wrapToPi(th_init_new - self.theta)
            t_init_align = abs(th_err / self.om_max)
            t_remaining_new = t_init_align + t_new[-1]

            if t_remaining_new > t_remaining_curr:
                rospy.loginfo(
                    "New plan rejected (longer duration than current plan)")
                self.publish_smoothed_path(traj_new,
                                           self.nav_smoothed_path_rej_pub)
                return

        # Otherwise follow the new plan
        self.publish_planned_path(planned_path, self.nav_planned_path_pub)
        self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)

        self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
        self.traj_controller.load_traj(t_new, traj_new)

        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = t_new[-1]

        self.th_init = traj_new[0, 2]
        self.heading_controller.load_goal(self.th_init)

        if not self.aligned():
            rospy.loginfo("Not aligned with start direction")
            self.switch_mode(Mode.ALIGN)
            return

        rospy.loginfo("Ready to track")
        self.switch_mode(Mode.TRACK)

    def run(self):
        rate = rospy.Rate(10)  # 10 Hz
        while not rospy.is_shutdown():
            # try to get state information to update self.x, self.y, self.theta
            try:
                (translation, rotation) = self.trans_listener.lookupTransform(
                    '/map', '/base_footprint', rospy.Time(0))
                self.x = translation[0]
                self.y = translation[1]
                euler = tf.transformations.euler_from_quaternion(rotation)
                self.theta = euler[2]
            except (tf.LookupException, tf.ConnectivityException,
                    tf.ExtrapolationException) as e:
                self.current_plan = []
                rospy.loginfo("Navigator: waiting for state info")
                self.switch_mode(Mode.IDLE)
                print e
                pass

            # STATE MACHINE LOGIC
            # some transitions handled by callbacks
            if self.mode == Mode.IDLE:
                pass
            elif self.mode == Mode.ALIGN:
                if self.aligned():
                    self.current_plan_start_time = rospy.get_rostime()
                    self.switch_mode(Mode.TRACK)
            elif self.mode == Mode.TRACK:
                if self.near_goal():
                    self.switch_mode(Mode.PARK)
                elif not self.close_to_plan_start():
                    rospy.loginfo("replanning because far from start")
                    self.replan()
                elif (rospy.get_rostime() - self.current_plan_start_time
                      ).to_sec() > self.current_plan_duration:
                    rospy.loginfo("replanning because out of time")
                    self.replan(
                    )  # we aren't near the goal but we thought we should have been, so replan
            elif self.mode == Mode.PARK:
                if self.at_goal():
                    # forget about goal:
                    self.x_g = None
                    self.y_g = None
                    self.theta_g = None
                    self.switch_mode(Mode.IDLE)

            self.publish_control()

            self.pose_controller.publish()  # For section 6

            rate.sleep()
Ejemplo n.º 4
0
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE
        self.mode_at_stop = None
        self.x_saved = None
        self.y_saved = None
        self.theta_saved = None

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # history tracking of controls
        self.history_cnt = 0
        self.V_history = np.zeros(CMD_HISTORY_SIZE)
        self.om_history = np.zeros(CMD_HISTORY_SIZE)
        self.backing_cnt = 0

        #laser scans for collision
        self.laser_ranges = []
        self.laser_angle_increment = 0.01  # this gets updated
        self.chunky_radius = 0.1  #TODO: Tune this!

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        self.iters = 0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0, 0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution = 0.1
        self.plan_horizon = 15

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0., 0.]

        # Robot limits
        self.v_max = rospy.get_param("~v_max", 0.2)  #0.2  # maximum velocity
        self.om_max = rospy.get_param("~om_max",
                                      0.4)  #0.4   # maximum angular velocity

        self.v_des = 0.12  # desired cruising velocity
        self.theta_start_thresh = 0.05  # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2  # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = (2.0 * np.pi) / 20.0  #0.05

        # trajectory smoothing
        self.spline_alpha = 0.01
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
                                                 self.kdy, self.v_max,
                                                 self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max,
                                              self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        # Publishers
        self.nav_planned_path_pub = rospy.Publisher('/planned_path',
                                                    Path,
                                                    queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
                                                     Path,
                                                     queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher(
            '/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
        #communication with squirtle_fsm to inform goal status
        self.publish_squirtle = rospy.Publisher('/post/squirtle_fsm',
                                                String,
                                                queue_size=10)

        # Subscriber Constructors
        rospy.Subscriber('/post/nav_fsm', Bool,
                         self.post_callback)  #service queue
        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
        rospy.Subscriber('/scan', LaserScan, self.laser_callback)
        rospy.Subscriber('debug/nav_fsm', String, self.debug_callback)

        print "finished init"
Ejemplo n.º 5
0
class Navigator:
    """
    This node handles point to point turtlebot motion, avoiding obstacles.
    It is the sole node that should publish to cmd_vel
    """
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE
        self.mode_at_stop = None
        self.x_saved = None
        self.y_saved = None
        self.theta_saved = None

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # history tracking of controls
        self.history_cnt = 0
        self.V_history = np.zeros(CMD_HISTORY_SIZE)
        self.om_history = np.zeros(CMD_HISTORY_SIZE)
        self.backing_cnt = 0

        #laser scans for collision
        self.laser_ranges = []
        self.laser_angle_increment = 0.01  # this gets updated
        self.chunky_radius = 0.1  #TODO: Tune this!

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        self.iters = 0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0, 0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution = 0.1
        self.plan_horizon = 15

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0., 0.]

        # Robot limits
        self.v_max = rospy.get_param("~v_max", 0.2)  #0.2  # maximum velocity
        self.om_max = rospy.get_param("~om_max",
                                      0.4)  #0.4   # maximum angular velocity

        self.v_des = 0.12  # desired cruising velocity
        self.theta_start_thresh = 0.05  # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2  # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = (2.0 * np.pi) / 20.0  #0.05

        # trajectory smoothing
        self.spline_alpha = 0.01
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
                                                 self.kdy, self.v_max,
                                                 self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max,
                                              self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        # Publishers
        self.nav_planned_path_pub = rospy.Publisher('/planned_path',
                                                    Path,
                                                    queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
                                                     Path,
                                                     queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher(
            '/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
        #communication with squirtle_fsm to inform goal status
        self.publish_squirtle = rospy.Publisher('/post/squirtle_fsm',
                                                String,
                                                queue_size=10)

        # Subscriber Constructors
        rospy.Subscriber('/post/nav_fsm', Bool,
                         self.post_callback)  #service queue
        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
        rospy.Subscriber('/scan', LaserScan, self.laser_callback)
        rospy.Subscriber('debug/nav_fsm', String, self.debug_callback)

        print "finished init"

    #------------------------------------------------------------------
    # Subscriber Callbacks
    #------------------------------------------------------------------
    #for in terminal debug
    def debug_callback(self, msg):
        if msg.data == "query_goal":
            print('[NAV DEBUG]: The goal is: %f, %f, %f' %
                  (self.x_g, self.y_g, self.theta_g))
        elif msg.data == "query_state":
            print("[NAV DEBUG]: The current state is: %s" % str(self.mode))
        elif msg.data == "why_u_do_dis?":
            print("[NAV DEBUG]: The current position delta is: %f, %f, %f" %
                  (self.x_g - self.x, self.y_g - self.y,
                   self.theta_g - self.theta))
        else:
            print("[NAV DEBUG]: Invalid debug message")

    #for the interface topic between nav and supervisor
    def post_callback(self, data):
        rospy.loginfo("Received from interface topic")

        # received true = stop
        if data.data is True:

            # store current goal
            self.x_saved = self.x_g
            self.y_saved = self.y_g
            self.theta_saved = self.theta_g

            # set goal to nothing
            self.x_g = None
            self.y_g = None
            self.theta_g = None

            #put machine back into idle
            self.switch_mode(Mode.IDLE)

        return

    # for getting the laser data
    def laser_callback(self, msg):
        """ callback for thr laser rangefinder """

        self.laser_ranges = msg.ranges
        self.laser_angle_increment = msg.angle_increment

    def dyn_cfg_callback(self, config, level):
        rospy.loginfo(
            "Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}, spline_alpha:{spline_alpha}"
            .format(**config))
        self.pose_controller.k1 = config["k1"]
        self.pose_controller.k2 = config["k2"]
        self.pose_controller.k3 = config["k3"]
        self.spline_alpha = config["spline_alpha"]

        self.traj_controller.kpx = config["kpx"]
        self.traj_controller.kpy = config["kpy"]
        self.traj_controller.kdx = config["kdx"]
        self.traj_controller.kdy = config["kdy"]
        self.traj_controller.V_max = config["V_max"]
        self.traj_controller.om_max = config["om_max"]

        self.chunky_radius = config["chunky_radius"]
        return config

    def cmd_nav_callback(self, data):
        """
        loads in goal if different from current goal, and replans
        """
        if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
            self.x_g = data.x
            self.y_g = data.y
            self.theta_g = data.theta
            self.replan()

            #tell squirtle we are no longer at the goal
            #msg = String()
            #msg.data = "not_at_goal"
            #self.publish_squirtle.publish(msg)

    def map_md_callback(self, msg):
        """
        receives maps meta data and stores it
        """
        self.map_width = msg.width
        self.map_height = msg.height
        self.map_resolution = msg.resolution
        self.map_origin = (msg.origin.position.x, msg.origin.position.y)

    def map_callback(self, msg):
        """
        receives new map info and updates the map
        """
        self.map_probs = msg.data
        # if we've received the map metadata and have a way to update it:
        if self.map_width > 0 and self.map_height > 0 and len(
                self.map_probs) > 0:
            self.occupancy = StochOccupancyGrid2D(
                self.map_resolution, self.map_width, self.map_height,
                self.map_origin[0], self.map_origin[1], 8, self.map_probs)
            if self.x_g is not None:
                # if we have a goal to plan to, replan
                rospy.loginfo("replanning because of new map")
                self.replan()  # new map, need to replan

    def shutdown_callback(self):
        """
        publishes zero velocities upon rospy shutdown
        """
        cmd_vel = Twist()
        cmd_vel.linear.x = 0.0
        cmd_vel.angular.z = 0.0
        self.nav_vel_pub.publish(cmd_vel)

    def near_goal(self):
        """
        returns whether the robot is close enough in position to the goal to
        start using the pose controller
        """
        return linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g
                                     ])) < self.near_thresh

    def at_goal(self):
        """
        returns whether the robot has reached the goal position with enough
        accuracy to return to idle state
        """
        # if the goal is none, it's already there you dumb Winnebago
        if self.x_g is None:
            return True
        return (linalg.norm(np.array(
            [self.x - self.x_g, self.y - self.y_g])) < self.at_thresh and abs(
                wrapToPi(self.theta - self.theta_g)) < self.at_thresh_theta)

    def aligned(self):
        """
        returns whether robot is aligned with starting direction of path
        (enough to switch to tracking controller)
        """
        return (abs(wrapToPi(self.theta - self.th_init)) <
                self.theta_start_thresh)

    def close_to_plan_start(self):
        return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh
                and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)

    def snap_to_grid(self, x):
        return (self.plan_resolution * round(x[0] / self.plan_resolution),
                self.plan_resolution * round(x[1] / self.plan_resolution))

    def switch_mode(self, new_mode):
        rospy.loginfo("Switching from %s -> %s", self.mode, new_mode)
        self.mode = new_mode

    #------------------------------------------------------------------
    # Publishers
    #------------------------------------------------------------------
    def publish_planned_path(self, path, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for state in path:
            pose_st = PoseStamped()
            pose_st.pose.position.x = state[0]
            pose_st.pose.position.y = state[1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_smoothed_path(self, traj, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for i in range(traj.shape[0]):
            pose_st = PoseStamped()
            pose_st.pose.position.x = traj[i, 0]
            pose_st.pose.position.y = traj[i, 1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_control(self):
        """
        Runs appropriate controller depending on the mode. Assumes all controllers
        are all properly set up / with the correct goals loaded
        """

        #-------------------------------------
        #  helper functions for LIFO queue
        #-------------------------------------

        def enQ_buffer(V_in, om_in):

            #rospy.loginfo("Enqueuing controls")
            # roll right 1 [n]->[n+1]
            self.V_history = np.roll(self.V_history, 1, axis=None)
            self.om_history = np.roll(self.om_history, 1, axis=None)
            # load in the value
            self.V_history[0] = V_in
            self.om_history[0] = om_in

        def deQ_buffer():
            #rospy.loginfo("Dequeuing controls")
            # get first value out
            V_out = -1.0 * self.V_history[0]
            om_out = -1.0 * self.om_history[0]
            # roll left 1 [n]<-[n+1]
            self.V_history = np.roll(self.V_history, -1, axis=None)
            self.om_history = np.roll(self.om_history, -1, axis=None)
            return V_out, om_out

        t = self.get_current_plan_time()

        if self.mode == Mode.PARK:
            V, om = self.pose_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.TRACK:
            V, om = self.traj_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.ALIGN:
            V, om = self.heading_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.BACKING_UP:
            #extract elements from the queue
            V, om = deQ_buffer()
        else:
            V = 0.
            om = 0.

        #enqueue for history tracking
        if self.mode is not Mode.BACKING_UP:
            enQ_buffer(V, om)

        cmd_vel = Twist()
        cmd_vel.linear.x = V
        cmd_vel.angular.z = om
        self.nav_vel_pub.publish(cmd_vel)

    def get_current_plan_time(self):
        t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
        return max(0.0, t)  # clip negative time to 0

    def replan(self):
        """
        loads goal into pose controller
        runs planner based on current pose
        if plan long enough to track:
            smooths resulting traj, loads it into traj_controller
            sets self.current_plan_start_time
            sets mode to ALIGN
        else:
            sets mode to PARK
        """
        # Make sure we have a map
        if not self.occupancy:
            rospy.loginfo(
                "Navigator: replanning canceled, waiting for occupancy map.")
            self.switch_mode(Mode.IDLE)
            return

        # Attempt to plan a path
        state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
        state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
        x_init = self.snap_to_grid((self.x, self.y))
        self.plan_start = x_init
        x_goal = self.snap_to_grid((self.x_g, self.y_g))
        problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
                        self.plan_resolution)

        rospy.loginfo("Navigator: computing navigation plan")

        success = problem.solve()
        if not success:
            rospy.loginfo("Planning failed")
            return
        rospy.loginfo("Planning Succeeded")
        '''
        self.iters = 0
        success = False
        while not success:
            success =  problem.solve()
            print("Ran Solve")
            if not success and not self.at_goal():
                if self.iters < 5 and self.theta_g is not None:
                    rospy.loginfo("Planning Iteration: %d", self.iters) 
                    rospy.loginfo("Planning failed, adjusting heading and retrying")
                    #increment heading
                    self.theta_g = wrapToPi(self.theta_g + np.pi/8.0)
                    rospy.loginfo("New heading: %f", self.theta_g)
                    #increment count
                    self.iters += 1
                else:
                    rospy.loginfo("Planning failed")
                    return
        
        rospy.loginfo("Planning Succeeded")
        '''

        planned_path = problem.path

        # Check whether path is too short
        if self.at_goal():
            rospy.loginfo("Path already at goal pose")
            self.switch_mode(Mode.IDLE)
            return
        elif len(planned_path) < 4:
            rospy.loginfo("Path too short to track")
            self.switch_mode(Mode.PARK)
            return

        # Smooth and generate a trajectory
        traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
                                                self.spline_alpha,
                                                self.traj_dt)

        # If currently tracking a trajectory, check whether new trajectory will take more time to follow
        if self.mode == Mode.TRACK:
            t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
            )

            # Estimate duration of new trajectory
            th_init_new = traj_new[0, 2]
            th_err = wrapToPi(th_init_new - self.theta)
            t_init_align = abs(th_err / self.om_max)
            t_remaining_new = t_init_align + t_new[-1]

            if t_remaining_new > t_remaining_curr:
                rospy.loginfo(
                    "New plan rejected (longer duration than current plan)")
                self.publish_smoothed_path(traj_new,
                                           self.nav_smoothed_path_rej_pub)
                return

        # Otherwise follow the new plan
        self.publish_planned_path(planned_path, self.nav_planned_path_pub)
        self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)

        self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
        self.traj_controller.load_traj(t_new, traj_new)

        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = t_new[-1]

        self.th_init = traj_new[0, 2]
        self.heading_controller.load_goal(self.th_init)

        if not self.aligned():
            rospy.loginfo("Not aligned with start direction")
            self.switch_mode(Mode.ALIGN)
            return

        rospy.loginfo("Ready to track")
        self.switch_mode(Mode.TRACK)

    def run(self):
        rate = rospy.Rate(10)  # 10 Hz
        while not rospy.is_shutdown():
            # try to get state information to update self.x, self.y, self.theta
            try:
                (translation, rotation) = self.trans_listener.lookupTransform(
                    '/map', '/base_footprint', rospy.Time(0))
                self.x = translation[0]
                self.y = translation[1]
                euler = tf.transformations.euler_from_quaternion(rotation)
                self.theta = euler[2]
            except (tf.LookupException, tf.ConnectivityException,
                    tf.ExtrapolationException) as e:
                self.current_plan = []
                rospy.loginfo("Navigator: waiting for state info")
                self.switch_mode(Mode.IDLE)
                print e
                pass
            """  
            def if_about_to_hit_wall():
                
                # check if inflated turtlebot circumference will hit the wall
                #th_offset = np.pi/8.0
                #arr = np.arange(wrapToPi(self.theta-th_offset), wrapToPi(self.theta+th_offset), 0.02)
                arr = np.arange(0, 2*np.pi, 0.02)
                for i in range(len(arr)):
                    if not self.occupancy.is_free(np.array([self.x + np.cos(arr[i])*self.chunky_radius,self.y+np.sin(arr[i])*self.chunky_radius])):
                        return True
                return False  
                
                #return not self.occupancy.is_free(np.array([self.x ,self.y]))
            """
            def if_about_to_hit_wall(laserRanges):
                #initialize return value to false
                returnFlag = False
                #remove the zeros
                #validRanges = [i for i, dist in enumerate(laserRanges) if dist != 0]
                #check the minimum scan distance
                #rospy.loginfo(laserRanges)
                minScanDist = min(laserRanges)
                #rospy.loginfo("Minimum Scan Distance: %f", minScanDist)

                #see if less that our boy's fat body
                if minScanDist < self.chunky_radius:
                    #set flag to true
                    returnFlag = True
                #return the flag
                return returnFlag

            # STATE MACHINE LOGIC
            # some transitions handled by callbacks
            if self.mode == Mode.IDLE:
                pass
            elif self.mode == Mode.ALIGN:
                if self.aligned():
                    self.current_plan_start_time = rospy.get_rostime()
                    self.switch_mode(Mode.TRACK)
            elif self.mode == Mode.TRACK:
                if if_about_to_hit_wall(self.laser_ranges):
                    rospy.loginfo("About to hit a wall")
                    self.switch_mode(Mode.BACKING_UP)
                elif self.near_goal():
                    self.switch_mode(Mode.PARK)
                elif not self.close_to_plan_start():
                    rospy.loginfo("replanning because far from start")
                    self.replan()
                elif (rospy.get_rostime() - self.current_plan_start_time
                      ).to_sec() > self.current_plan_duration:
                    rospy.loginfo("replanning because out of time")
                    self.replan(
                    )  # we aren't near the goal but we thought we should have been, so replan
            elif self.mode == Mode.PARK:
                #if if_about_to_hit_wall():
                #   self.switch_mode(Mode.BACKING_UP)
                if self.at_goal():
                    # forget about goal:
                    self.x_g = None
                    self.y_g = None
                    self.theta_g = None
                    self.switch_mode(Mode.IDLE)

                    #tell squirtle we are at the goal
                    msg = String()
                    msg.data = "at_goal"
                    self.publish_squirtle.publish(msg)

            elif self.mode == Mode.BACKING_UP:
                # see if we have backed enough counts
                if (self.backing_cnt > CMD_HISTORY_SIZE
                    ):  # or if_about_to_hit_wall(self.laser_ranges):
                    # NUKE EVERYTHING!!!!!!!!!
                    self.history_cnt = 0
                    self.V_history = np.zeros(CMD_HISTORY_SIZE)
                    self.om_history = np.zeros(CMD_HISTORY_SIZE)
                    self.backing_cnt = 0
                    self.replan()  #switches mode internally
                    #self.switch_mode(Mode.TRACK)
                else:
                    # increment count
                    self.backing_cnt += 1

            self.publish_control()
            rate.sleep()
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE
        self.xlist = []
        self.ylist = []
        self.tlist = []
        self.object_dic = {}
        with open('locations.json', 'r') as f:
            self.object_dic = json.load(f)
            for i in self.object_dic:
                print i
        cal_points(self.object_dic)
        print(point_mat)

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0, 0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution = 0.1
        self.plan_horizon = 15

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0., 0.]

        # Robot limits
        self.v_max = 0.2
        self.om_max = 0.5  #0.5

        self.v_des = 0.12  # desired cruising velocity
        self.theta_start_thresh = 0.05  # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2  # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05

        # trajectory smoothing
        self.spline_alpha = 0.15
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        #try
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.
        self.kp_th = 5.  #try

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
                                                 self.kdy, self.v_max,
                                                 self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max,
                                              self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path',
                                                    Path,
                                                    queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
                                                     Path,
                                                     queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher(
            '/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.vis_pub = rospy.Publisher('marker_topic', Marker, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)

        self.stop_min_dist = 1.0
        self.stop_time = 3.0
        self.crossing_time = 10.0
        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
        rospy.Subscriber('/detector/stop_sign', DetectedObject,
                         self.stop_sign_detected_callback)
        rospy.Subscriber('/detector/broccoli', DetectedObject,
                         self.broccoli_callback)
        rospy.Subscriber('/detector/cake', DetectedObject, self.cake_callback)
        rospy.Subscriber('/detector/bowl', DetectedObject, self.bowl_callback)
        rospy.Subscriber('/detector/banana', DetectedObject,
                         self.banana_callback)
        rospy.Subscriber('/detector/donut', DetectedObject,
                         self.donut_callback)
        rospy.Subscriber('/delivery_request', String, self.delivery_callback)
        self.initPos = False
        self.auto = False
        self.broccoli = 0
        self.cake = 1
        self.bowl = 2
        self.banana = 3
        self.donut = 4
        self.control = False

        print "finished init"
class Navigator:
    """
    This node handles point to point turtlebot motion, avoiding obstacles.
    It is the sole node that should publish to cmd_vel
    """
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE
        self.xlist = []
        self.ylist = []
        self.tlist = []
        self.object_dic = {}
        with open('locations.json', 'r') as f:
            self.object_dic = json.load(f)
            for i in self.object_dic:
                print i
        cal_points(self.object_dic)
        print(point_mat)

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0, 0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution = 0.1
        self.plan_horizon = 15

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0., 0.]

        # Robot limits
        self.v_max = 0.2
        self.om_max = 0.5  #0.5

        self.v_des = 0.12  # desired cruising velocity
        self.theta_start_thresh = 0.05  # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2  # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05

        # trajectory smoothing
        self.spline_alpha = 0.15
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        #try
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.
        self.kp_th = 5.  #try

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
                                                 self.kdy, self.v_max,
                                                 self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max,
                                              self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path',
                                                    Path,
                                                    queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
                                                     Path,
                                                     queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher(
            '/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.vis_pub = rospy.Publisher('marker_topic', Marker, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)

        self.stop_min_dist = 1.0
        self.stop_time = 3.0
        self.crossing_time = 10.0
        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
        rospy.Subscriber('/detector/stop_sign', DetectedObject,
                         self.stop_sign_detected_callback)
        rospy.Subscriber('/detector/broccoli', DetectedObject,
                         self.broccoli_callback)
        rospy.Subscriber('/detector/cake', DetectedObject, self.cake_callback)
        rospy.Subscriber('/detector/bowl', DetectedObject, self.bowl_callback)
        rospy.Subscriber('/detector/banana', DetectedObject,
                         self.banana_callback)
        rospy.Subscriber('/detector/donut', DetectedObject,
                         self.donut_callback)
        rospy.Subscriber('/delivery_request', String, self.delivery_callback)
        self.initPos = False
        self.auto = False
        self.broccoli = 0
        self.cake = 1
        self.bowl = 2
        self.banana = 3
        self.donut = 4
        self.control = False

        print "finished init"

    def startPlan(self):
        for i in range(5):
            if i == self.broccoli:
                print i
                print "broccoli"
                if 'broccoli' in self.object_dic:
                    x, y, t = self.object_dic['broccoli']
                    self.xlist.append(x)
                    self.ylist.append(y)
                    self.tlist.append(t)
            elif i == self.cake:
                print i
                print "cake"
                if 'cake' in self.object_dic:
                    x, y, t = self.object_dic['cake']
                    self.xlist.append(x)
                    self.ylist.append(y)
                    self.tlist.append(t)
            elif i == self.bowl:
                print i
                print "bowl"
                if 'bowl' in self.object_dic:
                    x, y, t = self.object_dic['bowl']
                    self.xlist.append(x)
                    self.ylist.append(y)
                    self.tlist.append(t)
            elif i == self.banana:
                print i
                print "banana"
                if 'banana' in self.object_dic:
                    x, y, t = self.object_dic['banana']
                    self.xlist.append(x)
                    self.ylist.append(y)
                    self.tlist.append(t)
            elif i == self.donut:
                print i
                print "donut"
                if 'donut' in self.object_dic:
                    x, y, t = self.object_dic['donut']
                    self.xlist.append(x)
                    self.ylist.append(y)
                    self.tlist.append(t)
        print 4
        print "Destination"
        x, y, t = self.object_dic['start']
        self.xlist.append(x)
        self.ylist.append(y)
        self.tlist.append(t)

    def show(self, idx, x, y, r, g, b, t):
        marker = Marker()
        marker.header.frame_id = "map"
        marker.header.stamp = rospy.Time()
        marker.id = idx
        marker.type = t  # sphere
        marker.pose.position.x = x
        marker.pose.position.y = y
        marker.pose.position.z = 0
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 1.0
        marker.scale.x = 0.2
        marker.scale.y = 0.2
        marker.scale.z = 0.2
        marker.color.a = 1.0
        marker.color.r = r
        marker.color.g = g
        marker.color.b = b
        self.vis_pub.publish(marker)

    def delivery_callback(self, msg):
        obj = msg.data
        destination_list = obj.split(',')
        for dest in destination_list:
            if dest in self.object_dic:
                x, y, t = self.object_dic[dest]
                if x in self.xlist or x == self.x:
                    print "Request already taken"
                else:
                    print "New request: " + dest
                    n = len(self.xlist)
                    self.xlist.append(x)
                    self.ylist.append(y)
                    self.tlist.append(t)
                    key = self.object_dic.keys()

                    # for k in key:
                    # point_mat[(k, dest)] = np.sqrt((x - self.object_dic[k][0])**2 + (y - self.object_dic[k][1])**2)

                    def helper(lst):
                        if len(lst) == 1:
                            return [lst]
                        res = []
                        for i in range(len(lst)):
                            lst_del = lst[:i] + lst[i + 1:]
                            pre_res = helper(lst_del)
                        for r in pre_res:
                            res.append(r + [lst[i]])
                            print(res)
                        return res

                    pts_lst = destination_list[:-1]
                    route = helper(pts_lst)  # possible route index
                    costs = []
                    for i in range(len(route)):
                        route[i] = [destination_list[-1]
                                    ] + route[i] + [destination_list[-1]]
                        cost = 0
                        for j in range(len(route[i]) - 1):
                            cost += point_mat[(route[i][j], route[i][j + 1])]
                        costs.append(cost)
                    opt_route = route[costs.index(min(costs))]
                    print(costs)
                    print(route)
                    #print(opt_route)
                    xlist, ylist, tlist = [], [], []
                    for i in range(len(opt_route)):
                        xlist.append(self.object_dic[opt_route[i]][0])
                        ylist.append(self.object_dic[opt_route[i]][1])
                        tlist.append(self.object_dic[opt_route[i]][2])
                    self.xlist = xlist
                    self.ylist = ylist
                    self.tlist = tlist

    def broccoli_callback(self, msg):
        self.object_dic['broccoli'] = (self.x, self.y, self.theta)
        print 'broccoli'
        print self.object_dic['broccoli']
        self.show(0, self.x, self.y, 1.0, 0.0, 0.0, 2)

    def cake_callback(self, msg):
        self.object_dic['cake'] = (self.x, self.y, self.theta)
        print 'cake'
        print self.object_dic['cake']
        self.show(1, self.x, self.y, 0.0, 1.0, 0.0, 2)

    def bowl_callback(self, msg):
        self.object_dic['bowl'] = (self.x, self.y, self.theta)
        print 'bowl'
        print self.object_dic['bowl']
        self.show(2, self.x, self.y, 0.0, 0.0, 1.0, 2)

    def banana_callback(self, msg):
        self.object_dic['banana'] = (self.x, self.y, self.theta)
        print 'banana'
        print self.object_dic['banana']
        self.show(3, self.x, self.y, 0.0, 1.0, 1.0, 2)

    def donut_callback(self, msg):
        self.object_dic['donut'] = (self.x, self.y, self.theta)
        print 'donut'
        print self.object_dic['donut']
        self.show(4, self.x, self.y, 1.0, 1.0, 0.0, 2)

    def stop_sign_detected_callback(self, msg):
        """ callback for when the detector has found a stop sign. Note that
        a distance of 0 can mean that the lidar did not pickup the stop sign at all """

        # distance of the stop sign
        # print "Stop Sign Destected"
        dist = msg.distance
        # if self.mode==Mode.TRACK:
        # if close enough and in nav mode, stop
        if dist > 0 and dist < self.stop_min_dist and self.mode == Mode.TRACK:
            print "Stop Sign Countdown"
            self.init_stop_sign()

    def init_stop_sign(self):
        """ initiates a stop sign maneuver """

        self.stop_sign_start = rospy.get_rostime()
        self.mode = Mode.STOP
        # print "initial stop sign"
        # print self.stop_sign_start

    def has_stopped(self):
        """ checks if stop sign maneuver is over """

        return self.mode == Mode.STOP and \
               rospy.get_rostime() - self.stop_sign_start > rospy.Duration.from_sec(self.stop_time)

    def init_crossing(self):
        """ initiates an intersection crossing maneuver """

        self.cross_start = rospy.get_rostime()
        self.mode = Mode.CROSS
        # print "Start crossing"
        # print self.cross_start

    def has_crossed(self):
        """ checks if crossing maneuver is over """

        return self.mode == Mode.CROSS and \
               rospy.get_rostime() - self.cross_start > rospy.Duration.from_sec(self.crossing_time)

    def dyn_cfg_callback(self, config, level):
        # rospy.loginfo("Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3},vm:{vm},sa:{sa},td:{td}".format(**config))
        self.pose_controller.k1 = config["k1"]
        self.pose_controller.k2 = config["k2"]
        self.pose_controller.k3 = config["k3"]
        self.v_max = config["vm"]
        self.spline_alpha = config["sa"]
        self.traj_dt = config["td"]

        self.auto = config["auto"]
        self.broccoli = config["broccoli"]
        self.cake = config["cake"]
        self.bowl = config["bowl"]
        self.banana = config["banana"]
        self.donut = config["donut"]
        return config

    def cmd_nav_callback(self, data):
        """
        loads in goal if different from current goal, and replans
        """
        if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
            self.x_g = data.x
            self.y_g = data.y
            self.theta_g = data.theta
            self.replan()
        # print(self.x_g,self.y_g,self.theta_g)

    def map_md_callback(self, msg):
        """
        receives maps meta data and stores it
        """
        self.map_width = msg.width
        self.map_height = msg.height
        self.map_resolution = msg.resolution
        self.map_origin = (msg.origin.position.x, msg.origin.position.y)

    def map_callback(self, msg):
        """
        receives new map info and updates the map
        """
        self.map_probs = msg.data
        # if we've received the map metadata and have a way to update it:
        if self.map_width > 0 and self.map_height > 0 and len(
                self.map_probs) > 0:
            self.occupancy = StochOccupancyGrid2D(self.map_resolution,
                                                  self.map_width,
                                                  self.map_height,
                                                  self.map_origin[0],
                                                  self.map_origin[1],
                                                  16,
                                                  self.map_probs,
                                                  thresh=0.3)
            if self.x_g is not None:
                # if we have a goal to plan to, replan
                # rospy.loginfo("replanning because of new map")
                self.replan()  # new map, need to replan

    def shutdown_callback(self):
        """
        publishes zero velocities upon rospy shutdown
        """
        cmd_vel = Twist()
        cmd_vel.linear.x = 0.0
        cmd_vel.angular.z = 0.0
        self.nav_vel_pub.publish(cmd_vel)

    def near_goal(self):
        """
        returns whether the robot is close enough in position to the goal to
        start using the pose controller
        """
        return linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g
                                     ])) < self.near_thresh

    def at_goal(self):
        """
        returns whether the robot has reached the goal position with enough
        accuracy to return to idle state
        """
        return (linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g])) <
                self.near_thresh and abs(wrapToPi(self.theta - self.theta_g)) <
                self.at_thresh_theta)

    def aligned(self):
        """
        returns whether robot is aligned with starting direction of path
        (enough to switch to tracking controller)
        """
        return (abs(wrapToPi(self.theta - self.th_init)) <
                self.theta_start_thresh)

    def close_to_plan_start(self):
        return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh
                and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)

    def snap_to_grid(self, x):
        return (self.plan_resolution * round(x[0] / self.plan_resolution),
                self.plan_resolution * round(x[1] / self.plan_resolution))

    def switch_mode(self, new_mode):
        rospy.loginfo("Switching from %s -> %s", self.mode, new_mode)
        self.mode = new_mode

    def publish_planned_path(self, path, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for state in path:
            pose_st = PoseStamped()
            pose_st.pose.position.x = state[0]
            pose_st.pose.position.y = state[1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_smoothed_path(self, traj, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for i in range(traj.shape[0]):
            pose_st = PoseStamped()
            pose_st.pose.position.x = traj[i, 0]
            pose_st.pose.position.y = traj[i, 1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_control(self):
        """
        Runs appropriate controller depending on the mode. Assumes all controllers
        are all properly set up / with the correct goals loaded
        """
        t = self.get_current_plan_time()

        if self.mode == Mode.PARK:
            V, om = self.pose_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.TRACK or self.mode == Mode.CROSS:
            V, om = self.traj_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.ALIGN:
            V, om = self.heading_controller.compute_control(
                self.x, self.y, self.theta, t)
        else:
            V = 0.
            om = 0.

        cmd_vel = Twist()
        cmd_vel.linear.x = V
        cmd_vel.angular.z = om
        self.nav_vel_pub.publish(cmd_vel)

    def get_current_plan_time(self):
        t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
        return max(0.0, t)  # clip negative time to 0

    def replan(self):
        """
        loads goal into pose controller
        runs planner based on current pose
        if plan long enough to track:
            smooths resulting traj, loads it into traj_controller
            sets self.current_plan_start_time
            sets mode to ALIGN
        else:
            sets mode to PARK
        """
        # Make sure we have a map
        if not self.occupancy:
            rospy.loginfo(
                "Navigator: replanning canceled, waiting for occupancy map.")
            self.switch_mode(Mode.IDLE)
            return

        # Attempt to plan a path
        state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
        state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
        x_init = self.snap_to_grid((self.x, self.y))
        self.plan_start = x_init
        x_goal = self.snap_to_grid((self.x_g, self.y_g))
        problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
                        self.plan_resolution)

        rospy.loginfo("Navigator: computing navigation plan")
        success = problem.solve()
        if not success:
            rospy.loginfo("Planning failed")
            return
        rospy.loginfo("Planning Succeeded")

        planned_path = problem.path

        # Check whether path is too short
        if len(planned_path) < 4:
            rospy.loginfo("Path too short to track")
            self.switch_mode(Mode.PARK)
            return

        # Smooth and generate a trajectory
        traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
                                                self.spline_alpha,
                                                self.traj_dt)

        # If currently tracking a trajectory, check whether new trajectory will take more time to follow
        if self.mode == Mode.TRACK:
            t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
            )

            # Estimate duration of new trajectory
            th_init_new = traj_new[0, 2]
            th_err = wrapToPi(th_init_new - self.theta)
            t_init_align = abs(th_err / self.om_max)
            t_remaining_new = t_init_align + t_new[-1]

            if t_remaining_new > t_remaining_curr:
                # rospy.loginfo("New plan rejected (longer duration than current plan)")
                self.publish_smoothed_path(traj_new,
                                           self.nav_smoothed_path_rej_pub)
                return

        # Otherwise follow the new plan
        self.publish_planned_path(planned_path, self.nav_planned_path_pub)
        self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)

        self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
        self.traj_controller.load_traj(t_new, traj_new)

        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = t_new[-1]

        self.th_init = traj_new[0, 2]
        self.heading_controller.load_goal(self.th_init)

        if self.mode == Mode.STOP or self.mode == Mode.CROSS:
            return

        if not self.aligned():
            rospy.loginfo("Not aligned with start direction")
            self.switch_mode(Mode.ALIGN)
            return

        rospy.loginfo("Ready to track")
        self.switch_mode(Mode.TRACK)

    def run(self):
        rate = rospy.Rate(10)  # 10 Hz
        while not rospy.is_shutdown():
            # try to get state information to update self.x, self.y, self.theta
            try:
                (translation, rotation) = self.trans_listener.lookupTransform(
                    '/map', '/base_footprint', rospy.Time(0))
                self.x = translation[0]
                self.y = translation[1]
                euler = tf.transformations.euler_from_quaternion(rotation)
                self.theta = euler[2]
                if self.initPos == False:
                    self.object_dic['start'] = (self.x, self.y, self.theta)
                    print 'start position'
                    print self.object_dic['start']
                    self.initPos = True
                    self.show(5, self.x, self.y, 1.0, 0.0, 1.0, 1)
            except (tf.LookupException, tf.ConnectivityException,
                    tf.ExtrapolationException) as e:
                self.current_plan = []
                # rospy.loginfo("Navigator: waiting for state info")
                self.switch_mode(Mode.IDLE)
                print e
                pass
            # STATE MACHINE LOGIC
            # some transitions handled by callbacks
            if self.mode == Mode.IDLE:
                if self.auto:
                    self.control = True
                    print "Delivery start"
                    self.startPlan()
                    self.auto = False
                if self.xlist:
                    print "Read next goal"
                    time.sleep(3)
                    self.x_g = self.xlist.pop(0)
                    self.y_g = self.ylist.pop(0)
                    self.theta_g = self.tlist.pop(0)
                    self.replan()
                else:
                    pass
            elif self.mode == Mode.ALIGN:
                if self.aligned():
                    self.current_plan_start_time = rospy.get_rostime()
                    self.switch_mode(Mode.TRACK)
            elif self.mode == Mode.TRACK:
                if self.near_goal():
                    self.switch_mode(Mode.PARK)
                elif not self.close_to_plan_start():
                    # rospy.loginfo("replanning because far from start")
                    self.replan()
                elif (rospy.get_rostime() - self.current_plan_start_time
                      ).to_sec() > self.current_plan_duration:
                    # rospy.loginfo("replanning because out of time")
                    self.replan(
                    )  # we aren't near the goal but we thought we should have been, so replan
            elif self.mode == Mode.PARK:
                if self.at_goal():
                    # forget about goal:
                    self.x_g = None
                    self.y_g = None
                    self.theta_g = None
                    self.switch_mode(Mode.IDLE)
            elif self.mode == Mode.STOP:
                if self.has_stopped():
                    print rospy.get_rostime()
                    self.init_crossing()
            elif self.mode == Mode.CROSS:
                # Crossing an intersection
                if self.has_crossed():
                    print rospy.get_rostime()
                    self.mode = Mode.TRACK
            self.publish_control()
            rate.sleep()
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE

        # WE CHANGED THIS STUFF
        # Time to stop at a stop sign
        self.stop_time = rospy.get_param("~stop_time", 3.)

        # Minimum distance from a stop sign to obey it (originally 0.5)
        self.stop_min_dist = rospy.get_param("~stop_min_dist", 1.)

        # Time taken to cross an intersection
        self.crossing_time = rospy.get_param("~crossing_time", 3.)

        # Stop sign detector
        rospy.Subscriber('/detector/stop_sign', DetectedObject,
                         self.stop_sign_detected_callback)

        rospy.Subscriber('/detector/hot_dog', DetectedObject,
                         self.hot_dog_detected_callback)

        # END OF WE CHANGED THIS STUFF

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0, 0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution = 0.1
        self.plan_horizon = 15

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0., 0.]

        # Robot limits
        #self.v_max = 0.2    # maximum velocity
        self.v_max = 0.5
        #self.om_max = 0.4   # maximum angular velocity
        self.om_max = 0.8

        self.v_des = 0.12  # desired cruising velocity
        self.theta_start_thresh = 0.05  # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2  # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05

        # trajectory smoothing
        self.spline_alpha = 0.15
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
                                                 self.kdy, self.v_max,
                                                 self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max,
                                              self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path',
                                                    Path,
                                                    queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
                                                     Path,
                                                     queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher(
            '/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)

        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)

        # ADD CURRENT STATE PUBLISHER (11/15/20 DEM)
        self.current_state_pub = rospy.Publisher('/current_state',
                                                 Pose2D,
                                                 queue_size=10)

        print "finished init"
Ejemplo n.º 9
0
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.EXPLORE
        self.delivery_locations = {}
        self.requests = []
        self.vendor_marker_ids = {}

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0, 0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution = 0.1
        self.plan_horizon = 20  # NOTE: Changed this to incraese the plan horizon for testing

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0., 0.]

        # Robot limits
        self.v_max = 0.2  # maximum velocity
        self.om_max = 0.4  # maximum angular velocity

        self.v_des = 0.12  # desired cruising velocity
        self.theta_start_thresh = 0.05  # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2  # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05
        #self.at_thresh_theta = 0.1

        # trajectory smoothing
        self.spline_alpha = 0.15
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
                                                 self.kdy, self.v_max,
                                                 self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max,
                                              self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path',
                                                    Path,
                                                    queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
                                                     Path,
                                                     queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher(
            '/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)

        #stop sign parameters
        self.stop_min_dist = 2
        self.stop_sign_roll_start = 0

        self.stop_sign_stop_time = 4
        self.wait_time = 1
        self.first_seen_time = -1

        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
        #For the stop sign
        rospy.Subscriber('/detector/stop_sign', DetectedObject,
                         self.stop_sign_detected_callback)

        # For ETA rviz markers
        self.vis_pub = rospy.Publisher('/eta', Marker, queue_size=10)
        # For landmark rviz markers
        self.vis_pub = rospy.Publisher('/marker_topic', Marker, queue_size=10)

        # Listen to object detector and save locations of interest
        rospy.Subscriber('/detector/objects',
                         DetectedObjectList,
                         self.detected_objects_callback,
                         queue_size=10)

        rospy.Subscriber('/delivery_request', String, self.request_callback)
        print "finished init"
Ejemplo n.º 10
0
class Navigator:
    """
    This node handles point to point turtlebot motion, avoiding obstacles.
    It is the sole node that should publish to cmd_vel
    """
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.EXPLORE
        self.delivery_locations = {}
        self.requests = []
        self.vendor_marker_ids = {}

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0, 0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution = 0.1
        self.plan_horizon = 20  # NOTE: Changed this to incraese the plan horizon for testing

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0., 0.]

        # Robot limits
        self.v_max = 0.2  # maximum velocity
        self.om_max = 0.4  # maximum angular velocity

        self.v_des = 0.12  # desired cruising velocity
        self.theta_start_thresh = 0.05  # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2  # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05
        #self.at_thresh_theta = 0.1

        # trajectory smoothing
        self.spline_alpha = 0.15
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
                                                 self.kdy, self.v_max,
                                                 self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max,
                                              self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path',
                                                    Path,
                                                    queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
                                                     Path,
                                                     queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher(
            '/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)

        #stop sign parameters
        self.stop_min_dist = 2
        self.stop_sign_roll_start = 0

        self.stop_sign_stop_time = 4
        self.wait_time = 1
        self.first_seen_time = -1

        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
        #For the stop sign
        rospy.Subscriber('/detector/stop_sign', DetectedObject,
                         self.stop_sign_detected_callback)

        # For ETA rviz markers
        self.vis_pub = rospy.Publisher('/eta', Marker, queue_size=10)
        # For landmark rviz markers
        self.vis_pub = rospy.Publisher('/marker_topic', Marker, queue_size=10)

        # Listen to object detector and save locations of interest
        rospy.Subscriber('/detector/objects',
                         DetectedObjectList,
                         self.detected_objects_callback,
                         queue_size=10)

        rospy.Subscriber('/delivery_request', String, self.request_callback)
        print "finished init"

    def dyn_cfg_callback(self, config, level):
        rospy.loginfo(
            "NAVIGATOR: Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}".format(
                **config))
        self.pose_controller.k1 = config["k1"]
        self.pose_controller.k2 = config["k2"]
        self.pose_controller.k3 = config["k3"]
        return config

    def cmd_nav_callback(self, data):
        """
        loads in goal if different from current goal, and replans
        """
        if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
            self.x_g = data.x
            self.y_g = data.y
            self.theta_g = data.theta
            self.replan()

    def map_md_callback(self, msg):
        """
        receives maps meta data and stores it
        """
        self.map_width = msg.width
        self.map_height = msg.height
        self.map_resolution = msg.resolution
        self.map_origin = (msg.origin.position.x, msg.origin.position.y)

    def map_callback(self, msg):
        """
        receives new map info and updates the map
        """
        self.map_probs = msg.data
        # if we've received the map metadata and have a way to update it:
        if self.map_width > 0 and self.map_height > 0 and len(
                self.map_probs) > 0:
            #self.occupancy = StochOccupancyGrid2D(self.map_resolution,
            #self.map_width,
            #self.map_height,
            #self.map_origin[0],
            #self.map_origin[1],
            #8,
            #self.map_probs)
            self.occupancy = StochOccupancyGrid2D(
                self.map_resolution,
                self.map_width,
                self.map_height,
                self.map_origin[0],
                self.map_origin[1],
                8,  # NOTE: Made the window size larger
                self.map_probs)  # NOTE: Made the probability lower

            if self.x_g is not None:
                # if we have a goal to plan to, replan
                rospy.loginfo("NAVIGATOR: replanning because of new map")
                self.replan()  # new map, need to replan

    def shutdown_callback(self):
        """
        publishes zero velocities upon rospy shutdown
        """
        cmd_vel = Twist()
        cmd_vel.linear.x = 0.0
        cmd_vel.angular.z = 0.0
        self.nav_vel_pub.publish(cmd_vel)

    def stop_sign_detected_callback(self, msg):
        """ callback for when the detector has found a stop sign. Note that
        a distance of 0 can mean that the lidar did not pickup the stop sign at all """

        # distance of the stop sign
        dist = msg.distance
        rospy.loginfo("Detected stop sign")
        rospy.loginfo(dist)

        if dist > 0 and dist < self.stop_min_dist and self.mode == Mode.TRACK and msg.confidence > OBJECT_CONFIDENCE_THESH:
            if self.first_seen_time == -1:
                self.first_seen_time = rospy.get_rostime()
            elif rospy.get_rostime(
            ) - self.first_seen_time > rospy.Duration.from_sec(
                    self.wait_time) and self.mode == Mode.TRACK:
                rospy.loginfo("Initializing roll and changing v_max and v_des")
                self.init_stop_sign()

    def calculate_bounding_box_area(self, corners):
        ymin, xmin, ymax, xmax = corners
        return (ymax - ymin) * (xmax - xmin)

    def detected_objects_callback(self, msg):
        # Iterate through all of the objects found by the detector
        for name, obj in zip(msg.objects, msg.ob_msgs):
            # Check to see if the object has not already been seen and if it is an object of interest
            if name not in self.delivery_locations.keys(
            ) and name in OBJECTS_OF_INTEREST:
                print(self.calculate_bounding_box_area(obj.corners))
                # Ensure that the object detected is of high confidence and close to the robot
                print('Detected object info')
                print(obj.confidence)
                print(obj.distance)
                print(self.calculate_bounding_box_area(obj.corners))
                if (obj.confidence < OBJECT_CONFIDENCE_THESH
                        and obj.distance < OBJECT_DISTANCE_THESH
                        and self.calculate_bounding_box_area(
                            obj.corners) > BOUNDING_BOX_AREA_THRESH):
                    # Add the object to the robot list
                    print('adding object')
                    currentPose = Pose2D()
                    currentPose.x = self.x
                    currentPose.y = self.y
                    currentPose.theta = self.theta
                    self.delivery_locations[name] = currentPose
                    print(self.delivery_locations)

                    # Once all objects have been found, then start the request cycle
                    if len(self.delivery_locations.keys()
                           ) == NUM_LOCATIONS_EXPLORED:
                        rospy.loginfo(
                            "NAVIGATOR: Found all delivery locations.")
                        self.switch_mode(Mode.IDLE)

            # to be tested: if object detected again, update location in case opponent moved it
            elif name in self.delivery_locations.keys():
                prev_loc = self.delivery_locations[name]
                dist = math.sqrt((prev_loc.x - self.x)**2 +
                                 (prev_loc.y - self.y)**2)
                if dist > 8:  # todo: tune this distance threshold
                    self.delivery_locations[name].x = self.x
                    self.delivery_locations[name].y = self.y
                    self.delivery_locations[name].theta = self.theta
                    rospy.loginfo(
                        "NAVIGATOR: Updating detected object location to current position"
                    )

    def request_callback(self, msg):
        rospy.loginfo("NAVIGATOR: Receiving request {}".format(msg.data))
        if msg.data == 'clear':
            self.requests = []
            self.switch_mode(Mode.IDLE)
            return

        if len(self.requests) == 0:
            for location in msg.data.split(','):
                if location not in self.delivery_locations.keys():
                    rospy.loginfo("NAVIGATOR: Location %s invalid. Skipping.",
                                  location)
                    continue
                else:
                    rospy.loginfo("NAVIGATOR: Processing request...")
                    self.requests.append(location)

            if len(self.requests) > 0:
                self.requests.append(HOME_LOCATION)
                self.go_to_next_request()

    def init_stop_sign(self):
        self.stop_sign_roll_start = rospy.get_rostime()
        self.traj_controller.V_max = 0.05
        self.pose_controller.V_max = 0.05
        self.v_des = 0.04
        self.switch_mode(Mode.ROLL)

    def has_rolled(self):
        return (self.mode == Mode.ROLL
                and rospy.get_rostime() - self.stop_sign_roll_start >
                rospy.Duration.from_sec(self.stop_sign_stop_time))

    def near_goal(self):
        """
        returns whether the robot is close enough in position to the goal to
        start using the pose controller
        """
        return linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g
                                     ])) < self.near_thresh

    def at_goal(self):
        """
        returns whether the robot has reached the goal position with enough
        accuracy to return to idle state
        """
        return (linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g])) <
                self.near_thresh and abs(wrapToPi(self.theta - self.theta_g)) <
                self.at_thresh_theta)

    def aligned(self):
        """
        returns whether robot is aligned with starting direction of path
        (enough to switch to tracking controller)
        """
        return (abs(wrapToPi(self.theta - self.th_init)) <
                self.theta_start_thresh)

    def close_to_plan_start(self):
        return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh
                and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)

    def snap_to_grid(self, x):
        return (self.plan_resolution * round(x[0] / self.plan_resolution),
                self.plan_resolution * round(x[1] / self.plan_resolution))

    def switch_mode(self, new_mode):
        rospy.loginfo("NAVIGATOR: Switching from %s -> %s", self.mode,
                      new_mode)
        self.mode = new_mode

    def publish_planned_path(self, path, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for state in path:
            pose_st = PoseStamped()
            pose_st.pose.position.x = state[0]
            pose_st.pose.position.y = state[1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_smoothed_path(self, traj, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for i in range(traj.shape[0]):
            pose_st = PoseStamped()
            pose_st.pose.position.x = traj[i, 0]
            pose_st.pose.position.y = traj[i, 1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_control(self):
        """
        Runs appropriate controller depending on the mode. Assumes all controllers
        are all properly set up / with the correct goals loaded
        """
        t = self.get_current_plan_time()

        if self.mode == Mode.PARK:
            V, om = self.pose_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.TRACK or self.mode == Mode.ROLL:
            V, om = self.traj_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.ALIGN:
            V, om = self.heading_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.EXPLORE:
            return
        else:
            V = 0.
            om = 0.

        cmd_vel = Twist()
        cmd_vel.linear.x = V
        cmd_vel.angular.z = om
        self.nav_vel_pub.publish(cmd_vel)

    def get_current_plan_time(self):
        t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
        return max(0.0, t)  # clip negative time to 0

    def go_to_next_request(self):
        goal_pose = self.delivery_locations[self.requests[0]]
        if goal_pose.x != self.x_g or goal_pose.y != self.y_g or goal_pose.theta != self.theta_g:
            rospy.loginfo("NAVIGATOR: Going to a new location")
            self.x_g = goal_pose.x
            self.y_g = goal_pose.y
            self.theta_g = goal_pose.theta
            self.replan()

    def replan(self):
        """
        loads goal into pose controller
        runs planner based on current pose
        if plan long enough to track:
            smooths resulting traj, loads it into traj_controller
            sets self.current_plan_start_time
            sets mode to ALIGN
        else:
            sets mode to PARK
        """
        # Make sure we have a map
        if not self.occupancy:
            rospy.loginfo(
                "NAVIGATOR: replanning canceled, waiting for occupancy map.")
            self.switch_mode(Mode.IDLE)
            return

        # Attempt to plan a path
        state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
        state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
        x_init = self.snap_to_grid((self.x, self.y))
        self.plan_start = x_init
        x_goal = self.snap_to_grid((self.x_g, self.y_g))
        problem = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
                        self.plan_resolution)

        rospy.loginfo("NAVIGATOR: computing navigation plan")
        success = problem.solve()
        if not success:
            rospy.loginfo("NAVIGATOR: Planning failed")
            return
        rospy.loginfo("NAVIGATOR: Planning Succeeded")

        planned_path = problem.path

        # Check whether path is too short
        try:
            if len(planned_path) < 4:
                rospy.loginfo("NAVIGATOR: Path too short to track")
                self.switch_mode(Mode.PARK)
                return
        except:
            rospy.loginfo(
                "NAVIGATOR: len(path_planned) attempt failed. Switching to park. Try a new path."
            )
            self.switch_mode(Mode.PARK)
            return

        # Smooth and generate a trajectory
        traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des,
                                                self.spline_alpha,
                                                self.traj_dt)

        # If currently tracking a trajectory, check whether new trajectory will take more time to follow
        if self.mode == Mode.TRACK:
            t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
            )

            # Estimate duration of new trajectory
            th_init_new = traj_new[0, 2]
            th_err = wrapToPi(th_init_new - self.theta)
            t_init_align = abs(th_err / self.om_max)
            t_remaining_new = t_init_align + t_new[-1]

            if t_remaining_new > t_remaining_curr:
                rospy.loginfo(
                    "NAVIGATOR: New plan rejected (longer duration than current plan)"
                )
                self.publish_smoothed_path(traj_new,
                                           self.nav_smoothed_path_rej_pub)
                return

        # Otherwise follow the new plan
        self.publish_planned_path(planned_path, self.nav_planned_path_pub)
        self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)

        self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
        self.traj_controller.load_traj(t_new, traj_new)

        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = t_new[-1]

        self.th_init = traj_new[0, 2]
        self.heading_controller.load_goal(self.th_init)

        if not self.aligned():
            rospy.loginfo("NAVIGATOR: Not aligned with start direction")
            self.switch_mode(Mode.ALIGN)
            return

        rospy.loginfo("NAVIGATOR: Ready to track")
        self.switch_mode(Mode.TRACK)

    def run(self):
        rate = rospy.Rate(10)  # 10 Hz
        while not rospy.is_shutdown():

            # try to get state information to update self.x, self.y, self.theta
            try:
                (translation, rotation) = self.trans_listener.lookupTransform(
                    '/map', '/base_footprint', rospy.Time(0))
                self.x = translation[0]
                self.y = translation[1]
                euler = tf.transformations.euler_from_quaternion(rotation)
                self.theta = euler[2]
            except (tf.LookupException, tf.ConnectivityException,
                    tf.ExtrapolationException) as e:
                self.current_plan = []
                rospy.loginfo("NAVIGATOR: waiting for state info")
                if (self.mode != Mode.EXPLORE):
                    self.switch_mode(Mode.IDLE)
                print e
                pass

            # STATE MACHINE LOGIC
            # some transitions handled by callbacks
            if self.mode == Mode.IDLE:
                pass
            elif self.mode == Mode.ALIGN:
                if self.aligned():
                    self.current_plan_start_time = rospy.get_rostime()
                    self.switch_mode(Mode.TRACK)
            elif self.mode == Mode.TRACK:
                if self.near_goal():
                    self.switch_mode(Mode.PARK)
                elif not self.close_to_plan_start():
                    rospy.loginfo(
                        "NAVIGATOR: replanning because far from start")
                    self.replan()
                elif (rospy.get_rostime() - self.current_plan_start_time
                      ).to_sec() > self.current_plan_duration:
                    rospy.loginfo("NAVIGATOR: replanning because out of time")
                    self.replan(
                    )  # we aren't near the goal but we thought we should have been, so replan
                self.displayETA()
            elif self.mode == Mode.PARK:
                if self.at_goal():
                    # Forget about the current goal
                    self.requests.pop(0)
                    self.x_g = None
                    self.y_g = None
                    self.theta_g = None
                    self.switch_mode(Mode.IDLE)
                    # Check to see if there are more places that must be visited
                    if len(self.requests) > 0:
                        self.go_to_next_request()
            elif self.mode == Mode.ROLL:
                rospy.loginfo("Rolling...")
                if self.has_rolled():
                    rospy.loginfo("Finishd rolling")
                    self.traj_controller.V_max = 0.2
                    self.pose_controller.V_max = 0.2
                    self.v_des = 0.12
                    self.mode = Mode.TRACK
                    self.first_seen_time = -1

            # publish vendor and robot locations
            self.publish_vendor_locs()
            self.publish_robot_loc()

            self.publish_control()
            rate.sleep()

    ########## RVIZ VISUALIZATION ##########

    def publish_vendor_locs(self):

        for name, loc in self.delivery_locations.items():
            # add marker
            marker = Marker()

            marker.header.frame_id = "/map"
            marker.header.stamp = rospy.Time()

            # so we don't create millions of markers over time
            if name in self.vendor_marker_ids.keys():
                marker.id = self.vendor_marker_ids[name]
            else:
                next_avail_id = len(self.vendor_marker_ids
                                    ) + 1  # robot is 0, so increment from 1
                marker.id = next_avail_id
                self.vendor_marker_ids[name] = next_avail_id

            marker.type = 1  # cube
            marker.pose.position.x = loc.x
            marker.pose.position.y = loc.y
            #marker.pose.position.x = loc[0]
            #marker.pose.position.y = loc[1]
            marker.pose.position.z = 0.1

            marker.pose.orientation.x = 0.0
            marker.pose.orientation.y = 0.0
            marker.pose.orientation.z = 0.0
            marker.pose.orientation.w = 1.0

            marker.scale.x = 0.2
            marker.scale.y = 0.2
            marker.scale.z = 0.2
            if name == HOME_LOCATION:
                marker.color.a = 1.0
                marker.color.r = 1.0
                marker.color.g = 0.0
                marker.color.b = 0.0
            else:
                marker.color.a = 1.0  # Don't forget to set the alpha!
                marker.color.r = 0.5
                marker.color.g = 0.0
                marker.color.b = 1.0

            self.vis_pub.publish(marker)

    def publish_robot_loc(self):
        marker = Marker()

        marker.header.frame_id = "base_footprint"
        marker.header.stamp = rospy.Time()

        marker.id = 0  # robot marker id is 0

        marker.type = 0  # arrow

        marker.pose.position.x = 0.0
        marker.pose.position.y = 0.0
        marker.pose.position.z = 0.0

        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 1.0

        marker.scale.x = 0.4
        marker.scale.y = 0.1
        marker.scale.z = 0.1

        marker.color.a = 1.0  # Don't forget to set the alpha!
        marker.color.r = 0.5
        marker.color.g = 1.0
        marker.color.b = 0.5
        self.vis_pub.publish(marker)

    def displayETA(self):
        marker = Marker()

        marker.header.frame_id = "base_footprint"
        marker.header.stamp = rospy.Time()

        marker.id = 1234

        marker.type = Marker.TEXT_VIEW_FACING

        marker.pose.position.x = 0.0
        marker.pose.position.y = 0.0
        marker.pose.position.z = 0.0

        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 1.0

        marker.scale.x = 0.4
        marker.scale.y = 0.1
        marker.scale.z = 0.2

        marker.color.a = 1.0  # Don't forget to set the alpha!
        marker.color.r = 0.02
        marker.color.g = 0.84
        marker.color.b = 1.0
        marker.text = "ETA to destination: {}".format(
            self.current_plan_duration)
        self.vis_pub.publish(marker)
Ejemplo n.º 11
0
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.vendor_queue = []
        self.energy = 65

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0,0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False
        self.vendors = {'apple': None, 'banana': None, 'broccoli': None, 'pizza': None, 'donut': None, 'home': None}
        #self.vendors = {'apple': (0.27, 1), 'banana': (1, 0.27), 'broccoli': (1.85, 2.8), 'pizza': (1.15, 1.65), 'donut': (2.2, 1.85), 'home': (3.3, 1.6)}
        
        self.intermediate_left = (2.5, 0.3)
        self.intermediate_right = (2.5, 2.7)
        
        # plan parameters
        self.plan_resolution =  0.1
        self.plan_horizon = 15
        self.fail_count = 0
        # Stop sign related parameters
        # Time to stop at a stop sign
        self.stop_time = rospy.get_param("~stop_time", 3.)
        # Minimum distance from a stop sign to obey it
        self.stop_min_dist = rospy.get_param("~stop_min_dist", 0.4)
        # Time taken to cross an intersection
        self.crossing_time = rospy.get_param("~crossing_time", 3.5)
        self.crossing_vel = 0.125

        self.vendor_detector_dist = 0.4
        self.vendor_time = rospy.get_param("~vendor_stop_time", 5.)
        self.vendor_stop_start_time = rospy.get_rostime()

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0.,0.]
        
        # Robot limits
        self.v_max = 0.2    # maximum velocity
        self.om_max = 0.4   # maximum angular velocity
	#self.v_max = rospy.get_param("~v_max")
	#self.om_max = rospy.get_param("~om_max")

        self.v_des = 0.12   # desired cruising velocity
        self.theta_start_thresh = 0.05   # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2     # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05 # cfg

        # trajectory smoothing
        self.spline_alpha = 0.1 #need to change in cfg
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path', Path, queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path', Path, queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher('/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.marker_pub = rospy.Publisher('/vendor_loc', Pose2D, queue_size=10)
        self.goal_marker_pub = rospy.Publisher('/goal_loc', Pose2D, queue_size=10)
        self.hungry_pub = rospy.Publisher('/hungry', String, queue_size=10)
        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)

        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)

        # Stop sign detector
        rospy.Subscriber('/detector/stop_sign', DetectedObject, self.stop_sign_detected_callback)
        # Vendor detector
        rospy.Subscriber('/detector/objects', DetectedObjectList, self.vendor_detected_callback)

        rospy.Subscriber('/delivery_request', String, self.order_callback)

        print "finished init"
Ejemplo n.º 12
0
class Navigator:
    """
    This node handles point to point turtlebot motion, avoiding obstacles.
    It is the sole node that should publish to cmd_vel
    """
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.vendor_queue = []
        self.energy = 65

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0,0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False
        self.vendors = {'apple': None, 'banana': None, 'broccoli': None, 'pizza': None, 'donut': None, 'home': None}
        #self.vendors = {'apple': (0.27, 1), 'banana': (1, 0.27), 'broccoli': (1.85, 2.8), 'pizza': (1.15, 1.65), 'donut': (2.2, 1.85), 'home': (3.3, 1.6)}
        
        self.intermediate_left = (2.5, 0.3)
        self.intermediate_right = (2.5, 2.7)
        
        # plan parameters
        self.plan_resolution =  0.1
        self.plan_horizon = 15
        self.fail_count = 0
        # Stop sign related parameters
        # Time to stop at a stop sign
        self.stop_time = rospy.get_param("~stop_time", 3.)
        # Minimum distance from a stop sign to obey it
        self.stop_min_dist = rospy.get_param("~stop_min_dist", 0.4)
        # Time taken to cross an intersection
        self.crossing_time = rospy.get_param("~crossing_time", 3.5)
        self.crossing_vel = 0.125

        self.vendor_detector_dist = 0.4
        self.vendor_time = rospy.get_param("~vendor_stop_time", 5.)
        self.vendor_stop_start_time = rospy.get_rostime()

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0.,0.]
        
        # Robot limits
        self.v_max = 0.2    # maximum velocity
        self.om_max = 0.4   # maximum angular velocity
	#self.v_max = rospy.get_param("~v_max")
	#self.om_max = rospy.get_param("~om_max")

        self.v_des = 0.12   # desired cruising velocity
        self.theta_start_thresh = 0.05   # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2     # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05 # cfg

        # trajectory smoothing
        self.spline_alpha = 0.1 #need to change in cfg
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path', Path, queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path', Path, queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher('/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.marker_pub = rospy.Publisher('/vendor_loc', Pose2D, queue_size=10)
        self.goal_marker_pub = rospy.Publisher('/goal_loc', Pose2D, queue_size=10)
        self.hungry_pub = rospy.Publisher('/hungry', String, queue_size=10)
        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)

        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)

        # Stop sign detector
        rospy.Subscriber('/detector/stop_sign', DetectedObject, self.stop_sign_detected_callback)
        # Vendor detector
        rospy.Subscriber('/detector/objects', DetectedObjectList, self.vendor_detected_callback)

        rospy.Subscriber('/delivery_request', String, self.order_callback)

        print "finished init"
        
    def dyn_cfg_callback(self, config, level):
        rospy.loginfo("Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}, spline_alpha:{spline_alpha}".format(**config))
        self.pose_controller.k1 = config["k1"] #default: 0.8
        self.pose_controller.k2 = config["k2"] #default: 0.4
        self.pose_controller.k3 = config["k3"] #default: 0.4
	self.spline_alpha = config["spline_alpha"] #default: 0.15
        self.at_thresh_theta = config["at_thresh_theta"] #default: 0.05
        return config

    def cmd_nav_callback(self, data):
        """
        loads in goal if different from current goal, and replans
        """
        if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
            self.x_g = data.x
            self.y_g = data.y
            self.theta_g = data.theta
            self.replan()

    def map_md_callback(self, msg):
        """
        receives maps meta data and stores it
        """
        self.map_width = msg.width
        self.map_height = msg.height
        self.map_resolution = msg.resolution
        self.map_origin = (msg.origin.position.x,msg.origin.position.y)

    def map_callback(self,msg):
        """
        receives new map info and updates the map
        """
        self.map_probs = msg.data
        # if we've received the map metadata and have a way to update it:
        if self.map_width>0 and self.map_height>0 and len(self.map_probs)>0:
            self.occupancy = StochOccupancyGrid2D(self.map_resolution,
                                                  self.map_width,
                                                  self.map_height,
                                                  self.map_origin[0],
                                                  self.map_origin[1],
                                                  10,
                                                  self.map_probs)
            if self.x_g is not None:
                # if we have a goal to plan to, replan
                rospy.loginfo("replanning because of new map")
                self.replan() # new map, need to replan

    def stop_sign_detected_callback(self, msg):
        """ callback for when the detector has found a stop sign. Note that
        a distance of 0 can mean that the lidar did not pickup the stop sign at all """

        # distance of the stop sign
        thetaleft = msg.thetaleft - 2*np.pi if msg.thetaleft > np.pi else msg.thetaleft
        thetaright = msg.thetaright - 2*np.pi if msg.thetaright > np.pi else msg.thetaright
        camera_theta = (thetaleft + thetaright)/2
        dist = msg.distance*np.cos(camera_theta)

        # if close enough and in nav mode, stop
        if dist > 0 and dist < self.stop_min_dist and self.mode == Mode.TRACK and msg.confidence > 0.8:
            self.init_stop_sign()
	    print('stop sign detected')
   
    def vendor_detected_callback(self, msg):
        '''
        callback for when the detector has found a vendor logo it is looking for
        '''
        msg = msg.ob_msgs[-1]
        print(msg.name, msg.distance)
        if msg.name in self.vendors.keys() and msg.distance < self.vendor_detector_dist and self.vendors[msg.name] == None:
            print("Vendor detected")
            thetaleft = msg.thetaleft - 2*np.pi if msg.thetaleft > np.pi else msg.thetaleft
            thetaright = msg.thetaright - 2*np.pi if msg.thetaright > np.pi else msg.thetaright
            camera_theta = (thetaleft + thetaright)/2
            dist = msg.distance
            self.vendors[msg.name] = (self.x+dist*np.cos(self.theta+camera_theta), self.y+dist*np.sin(self.theta+camera_theta))
            pub_msg = Pose2D()
            pub_msg.x, pub_msg.y = self.vendors[msg.name]
            self.marker_pub.publish(pub_msg)           
            self.vendors[msg.name] = self.snap_to_grid((self.x, self.y))
            


    def order_callback(self, msg):
        '''
        callback for when there is an order request
        '''
        vendorList = str(msg)[7:-1].split(',')
        if vendorList == ['']:
            self.vendor_queue.append(self.vendors['home'])
        else:
            locations = [self.vendors['home']]
            for v in vendorList:
                if v != '':
                    locations.append(self.vendors[v])
            state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
            state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
            edges, cost_matrix = tsp_solver(state_min, state_max, self.occupancy, self.plan_resolution, self.v_des, self.spline_alpha, self.traj_dt, locations)
            print(edges, cost_matrix)
            queue_temp = []
            cost_list = [0]
            for edge in edges[1:]:
                queue_temp.append(locations[edge[1]])
                cost_list.append(cost_list[-1]+cost_matrix[edge[0]][edge[1]])
            print(cost_list)
            energy_fill_times = 0
            while cost_list[-1] > self.energy:
                index = len(cost_list)-1
                while index >= 0 and cost_list[index] > self.energy:
                    index -= 1
                while index >= 0 and cost_list[index] + compute_path_cost(state_min, state_max, self.occupancy, self.plan_resolution, self.v_des, self.spline_alpha, self.traj_dt, self.vendors['donut'], locations[edges[index][1]]) > self.energy:
                    index -= 1
                queue_temp.insert(index+energy_fill_times, self.vendors['donut'])
                energy_fill_times += 1
                # update cost_list
                cost_list[index+1:] = cost_list[index+1:] - cost_list[index+1] + compute_path_cost(state_min, state_max, self.occupancy, self.plan_resolution, self.v_des, self.spline_alpha, self.traj_dt, self.vendors['donut'], locations[edges[index+1][1]])
            print(queue_temp)
            self.vendor_queue = queue_temp

    def shutdown_callback(self):
        """
        publishes zero velocities upon rospy shutdown
        """
        cmd_vel = Twist()
        cmd_vel.linear.x = 0.0
        cmd_vel.angular.z = 0.0
        self.nav_vel_pub.publish(cmd_vel)

    def stay_idle(self):
        """ sends zero velocity to stay put """

        vel_g_msg = Twist()
        self.nav_vel_pub.publish(vel_g_msg)

    def init_stop_sign(self):
        """ initiates a stop sign maneuver """

        self.stop_sign_start = rospy.get_rostime()
        self.mode = Mode.STOP

    def has_stopped(self):
        """ checks if stop sign maneuver is over """

        return self.mode == Mode.STOP and \
               rospy.get_rostime() - self.stop_sign_start > rospy.Duration.from_sec(self.stop_time)

    def init_crossing(self):
        """ initiates an intersection crossing maneuver """

        self.cross_start = rospy.get_rostime()
        self.mode = Mode.CROSS
        print("init crossing")

    def has_crossed(self):
        """ checks if crossing maneuver is over """

        return self.mode == Mode.CROSS and \
               rospy.get_rostime() - self.cross_start > rospy.Duration.from_sec(self.crossing_time)

    def near_goal(self):
        """
        returns whether the robot is close enough in position to the goal to
        start using the pose controller
        """
        return linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])) < self.near_thresh

    def at_goal(self):
        """
        returns whether the robot has reached the goal position with enough
        accuracy to return to idle state
        """
	if self.x_g == None or self.y_g == None or self.theta_g == None:
            return True
        if (self.vendor_queue and linalg.norm(np.array([self.x-self.vendor_queue[0][0], self.y-self.vendor_queue[0][1]])) < self.near_thresh) or (linalg.norm(np.array([self.x-self.intermediate_left[0], self.y-self.intermediate_left[1]])) < self.near_thresh) or (linalg.norm(np.array([self.x-self.intermediate_right[0], self.y-self.intermediate_right[1]])) < self.near_thresh):
            return linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])) < self.at_thresh
        else:
            return (linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])) < self.at_thresh and abs(wrapToPi(self.theta - self.theta_g)) < self.at_thresh_theta)

    def aligned(self):
        """
        returns whether robot is aligned with starting direction of path
        (enough to switch to tracking controller)
        """
        return (abs(wrapToPi(self.theta - self.th_init)) < self.theta_start_thresh)
        
    def close_to_plan_start(self):
        return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)

    def snap_to_grid(self, x):
        return (self.plan_resolution*round(x[0]/self.plan_resolution), self.plan_resolution*round(x[1]/self.plan_resolution))

    def switch_mode(self, new_mode):
        rospy.loginfo("Switching from %s -> %s", self.mode, new_mode)
        self.mode = new_mode

    def publish_planned_path(self, path, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for state in path:
            pose_st = PoseStamped()
            pose_st.pose.position.x = state[0]
            pose_st.pose.position.y = state[1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_smoothed_path(self, traj, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for i in range(traj.shape[0]):
            pose_st = PoseStamped()
            pose_st.pose.position.x = traj[i,0]
            pose_st.pose.position.y = traj[i,1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_control(self):
        """
        Runs appropriate controller depending on the mode. Assumes all controllers
        are all properly set up / with the correct goals loaded
        """
        t = self.get_current_plan_time()

        if self.mode == Mode.PARK:
            V, om = self.pose_controller.compute_control(self.x, self.y, self.theta, t)
        elif self.mode == Mode.TRACK:
            V, om = self.traj_controller.compute_control(self.x, self.y, self.theta, t)
        elif self.mode == Mode.ALIGN:
            V, om = self.heading_controller.compute_control(self.x, self.y, self.theta, t)
        elif self.mode == Mode.CROSS:
            V = self.crossing_vel
            om = 0.
        else:
            V = 0.
            om = 0.

        cmd_vel = Twist()
        cmd_vel.linear.x = V
        cmd_vel.angular.z = om
        self.nav_vel_pub.publish(cmd_vel)

    def get_current_plan_time(self):
        t = (rospy.get_rostime()-self.current_plan_start_time).to_sec()
        return max(0.0, t)  # clip negative time to 0

    def replan(self):
        """
        loads goal into pose controller
        runs planner based on current pose
        if plan long enough to track:
            smooths resulting traj, loads it into traj_controller
            sets self.current_plan_start_time
            sets mode to ALIGN
        else:
            sets mode to PARK
        """
        # Make sure we have a map
        if not self.occupancy:
            rospy.loginfo("Navigator: replanning canceled, waiting for occupancy map.")
            self.switch_mode(Mode.IDLE)
            return

        # Attempt to plan a path
        state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
        state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
        x_init = self.snap_to_grid((self.x, self.y))
        self.plan_start = x_init
        x_goal = self.snap_to_grid((self.x_g, self.y_g))
        problem = AStar(state_min,state_max,x_init,x_goal,self.occupancy,self.plan_resolution)

        rospy.loginfo("Navigator: computing navigation plan")
        success =  problem.solve()
        # deal with goal that is too far from where we are
        if not success:
            rospy.loginfo("Planning failed")
            self.fail_count += 1
	    if self.fail_count > 3:
                self.fail_count = 0
                left = abs(self.y - self.intermediate_left[1])
                right = abs(self.y - self.intermediate_right[1])
                self.x_g = 2.5
                if left < right:
                    self.y_g = self.intermediate_left[1]
                else:
                    self.y_g = self.intermediate_right[1]
                rospy.loginfo("Goal position reset due to failed plan")
            return
        rospy.loginfo("Planning Succeeded")

        planned_path = problem.path
        

        # Check whether path is too short
        if len(planned_path) < 4:
            rospy.loginfo("Path too short to track")
            self.switch_mode(Mode.PARK)
            self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
            return

        # Smooth and generate a trajectory
        traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des, self.spline_alpha, self.traj_dt)

        # If currently tracking a trajectory, check whether new trajectory will take more time to follow
        if self.mode == Mode.TRACK:
            t_remaining_curr = self.current_plan_duration - self.get_current_plan_time()

            # Estimate duration of new trajectory
            th_init_new = traj_new[0,2]
            th_err = wrapToPi(th_init_new - self.theta)
            t_init_align = abs(th_err/self.om_max)
            t_remaining_new = t_init_align + t_new[-1]

            if t_remaining_new > t_remaining_curr:
                rospy.loginfo("New plan rejected (longer duration than current plan)")
                self.publish_smoothed_path(traj_new, self.nav_smoothed_path_rej_pub)
                return

        # Otherwise follow the new plan
        self.publish_planned_path(planned_path, self.nav_planned_path_pub)
        self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)

        self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
        self.traj_controller.load_traj(t_new, traj_new)

        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = t_new[-1]

        self.th_init = traj_new[0,2]
        self.heading_controller.load_goal(self.th_init)

        if not self.aligned():
            rospy.loginfo("Not aligned with start direction")
            if not self.mode == Mode.STOP:
                if not (self.mode == Mode.CROSS and not self.has_crossed()):
            	    self.switch_mode(Mode.ALIGN)
            return

        if not self.mode == Mode.STOP:
            if not (self.mode == Mode.CROSS and not self.has_crossed()):
                rospy.loginfo("Ready to track")
                self.switch_mode(Mode.TRACK)

    def run(self):
        rate = rospy.Rate(10) # 10 Hz
        while not rospy.is_shutdown():
            # try to get state information to update self.x, self.y, self.theta
            try:
                (translation,rotation) = self.trans_listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
                self.x = translation[0]
                self.y = translation[1]
                euler = tf.transformations.euler_from_quaternion(rotation)
                self.theta = euler[2]
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
                self.current_plan = []
                rospy.loginfo("Navigator: waiting for state info")
                self.switch_mode(Mode.IDLE)
                print e
                pass

            # STATE MACHINE LOGIC
            # some transitions handled by callbacks
            if self.mode == Mode.IDLE:
                if self.vendors['home'] == None and self.x != 0 and self.y != 0:
                    self.vendors['home'] = self.snap_to_grid((self.x + 0.2, self.y))
                    msg = Pose2D()
                    msg.x, msg.y = self.vendors['home']
                    self.marker_pub.publish(msg)

                if self.vendor_queue and (rospy.get_rostime() - self.vendor_stop_start_time) > rospy.Duration.from_sec(self.vendor_time):
                    self.x_g = self.vendor_queue[0][0]
                    self.y_g = self.vendor_queue[0][1]
                    #start from home
                    if (linalg.norm(np.array([self.x-self.vendors['home'][0], self.y-self.vendors['home'][1]])) < self.at_thresh):
                        left = abs(self.y_g - self.intermediate_left[1])
                        right = abs(self.y_g - self.intermediate_right[1])
                        self.x_g = 2.5
                        if left < right:
                            self.y_g = self.intermediate_left[1]
                        else:
                            self.y_g = self.intermediate_right[1]
                    #return to home
                    elif ((self.x_g==self.vendors['home'][0]) and (self.y_g==self.vendors['home'][1])) and not (linalg.norm(np.array([self.x-self.intermediate_left[0], self.y-self.intermediate_left[1]])) < self.at_thresh or linalg.norm(np.array([self.x-self.intermediate_right[0], self.y-self.intermediate_right[1]])) < self.at_thresh) and self.x < 3: 
                        left = abs(self.y - self.intermediate_left[1])
                        right = abs(self.y - self.intermediate_right[1])
                        self.x_g = 2.5
                        if left < right:
                            self.y_g = self.intermediate_left[1]
                        else:
                            self.y_g = self.intermediate_right[1]
                    self.theta_g = 0
                    self.replan()

            elif self.mode == Mode.ALIGN:
                if self.aligned():
                    self.current_plan_start_time = rospy.get_rostime()
                    self.switch_mode(Mode.TRACK)
            elif self.mode == Mode.TRACK:
                if self.near_goal():
                    self.switch_mode(Mode.PARK)
                elif not self.close_to_plan_start():
                    rospy.loginfo("replanning because far from start")
                    self.replan()
                elif (rospy.get_rostime() - self.current_plan_start_time).to_sec() > self.current_plan_duration:
                    rospy.loginfo("replanning because out of time")
                    self.replan() # we aren't near the goal but we thought we should have been, so replan
            elif self.mode == Mode.PARK:
                if self.at_goal():
                    if self.vendor_queue and linalg.norm(np.array([self.x-self.vendor_queue[0][0], self.y-self.vendor_queue[0][1]])) < self.at_thresh:
                        self.vendor_stop_start_time = rospy.get_rostime()
                        self.vendor_queue.pop(0)
                    # forget about goal:
                    self.x_g = None
                    self.y_g = None
                    self.theta_g = None
                    self.switch_mode(Mode.IDLE)

            elif self.mode == Mode.STOP:
                # At a stop sign
                if self.has_stopped():
            	    self.init_crossing()
                else:
                    self.stay_idle()

            elif self.mode == Mode.CROSS:
                # Crossing an intersection
	        if self.at_goal():
                    self.mode = Mode.IDLE
                elif self.has_crossed():
                    print("crossed")
                    self.replan()
	        else:
                    print("crossing") 
                    cmd_vel = Twist()
                    cmd_vel.linear.x = self.crossing_vel
                    cmd_vel.angular.z = 0
                    self.nav_vel_pub.publish(cmd_vel)


            self.publish_control()

            # Publish goal marker
            if self.x_g and self.y_g:
            	goal = Pose2D()
            	goal.x, goal.y = self.x_g, self.y_g
            	self.goal_marker_pub.publish(goal)
            
            # Publish hungry message
            if self.vendors['donut'] and self.x_g == self.vendors['donut'][0] and self.y_g == self.vendors['donut'][1]:
                msg = String()
                msg.data = "I am so hungry! I have to grab some donuts before I starve to die!!!"
                self.hungry_pub.publish(msg) 

            rate.sleep()
Ejemplo n.º 13
0
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0,0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution =  0.1
        self.plan_horizon = 15

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0.,0.]
        
        # Robot limits
        self.v_max = rospy.get_param("~v_max", 0.2)    # 0.2    # maximum velocity
        self.om_max = rospy.get_param("~om_max", 0.4)   # 0.4   # maximum angular velocity

        self.v_des = 0.12   # desired cruising velocity
        self.theta_start_thresh = 0.05   # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2     # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05

        # trajectory smoothing
        self.spline_alpha = 0.15
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path', Path, queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path', Path, queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher('/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
        
        self.detected_objects = {}

        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)

        # Food delivery
        rospy.Subscriber('/delivery_request', String, self.delivery_request_callback)
        rospy.Subscriber('/detector/objects', DetectedObjectList, self.detected_objects_name_callback, queue_size=10)

        # Vendor state
        self.vendor_start  = None  # Starting time of vendor state
        self.vendor_time   = 4.0   # 3-5s stop time
        self.gotovendor    = False
        self.home_location = [3.15,1.6,0.0]

        #############################################################
        # Stop state
        # Time to stop at a stop sign
        self.stop_time = rospy.get_param("~stop_time", 5.)

        # Minimum distance from a stop sign to obey it
        self.stop_min_dist = rospy.get_param("~stop_min_dist", 0.6)  # original value 0.5

        # Time taken to cross an intersection
        self.crossing_time = rospy.get_param("~crossing_time", 3.)

        # Stop sign detector
        rospy.Subscriber('/detector/stop_sign', DetectedObject, self.stop_sign_detected_callback)
        self.stopped = False
        self.v = 0.0
        self.stop_sign_start = None
        #############################################################

        print "finished init"
Ejemplo n.º 14
0
class Navigator:
    """
    This node handles point to point turtlebot motion, avoiding obstacles.
    It is the sole node that should publish to cmd_vel
    """
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0,0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution =  0.1
        self.plan_horizon = 15

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0.,0.]
        
        # Robot limits
        self.v_max = rospy.get_param("~v_max", 0.2)    # 0.2    # maximum velocity
        self.om_max = rospy.get_param("~om_max", 0.4)   # 0.4   # maximum angular velocity

        self.v_des = 0.12   # desired cruising velocity
        self.theta_start_thresh = 0.05   # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2     # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05

        # trajectory smoothing
        self.spline_alpha = 0.15
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path', Path, queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path', Path, queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher('/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)
        
        self.detected_objects = {}

        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)

        # Food delivery
        rospy.Subscriber('/delivery_request', String, self.delivery_request_callback)
        rospy.Subscriber('/detector/objects', DetectedObjectList, self.detected_objects_name_callback, queue_size=10)

        # Vendor state
        self.vendor_start  = None  # Starting time of vendor state
        self.vendor_time   = 4.0   # 3-5s stop time
        self.gotovendor    = False
        self.home_location = [3.15,1.6,0.0]

        #############################################################
        # Stop state
        # Time to stop at a stop sign
        self.stop_time = rospy.get_param("~stop_time", 5.)

        # Minimum distance from a stop sign to obey it
        self.stop_min_dist = rospy.get_param("~stop_min_dist", 0.6)  # original value 0.5

        # Time taken to cross an intersection
        self.crossing_time = rospy.get_param("~crossing_time", 3.)

        # Stop sign detector
        rospy.Subscriber('/detector/stop_sign', DetectedObject, self.stop_sign_detected_callback)
        self.stopped = False
        self.v = 0.0
        self.stop_sign_start = None
        #############################################################

        print "finished init"

    def stop_sign_detected_callback(self, msg):
        """ callback for when the detector has found a stop sign. Note that
        a distance of 0 can mean that the lidar did not pickup the stop sign at all """

        #rospy.loginfo("Detected stop sign")
        # distance of the stop sign
        dist = msg.distance
        # if close enough and in track mode, stop
        #print("stop sign dust: %s, stopped: %s " % (dist, self.stopped))
        if dist > 0 and dist < self.stop_min_dist and (self.mode == Mode.TRACK or self.mode == Mode.VENDOR) and not self.stopped:
            #rospy.loginfo("Start stopping")
            self.init_stop_sign()
            

    def init_stop_sign(self):
        """ initiates a stop sign maneuver """
        self.stop_sign_start = rospy.get_rostime()
        self.stopped = True
        self.switch_mode(Mode.STOP)

    def has_stopped(self):
        """ checks if stop sign maneuver is over """

        return self.mode == Mode.STOP and \
               rospy.get_rostime() - self.stop_sign_start > rospy.Duration.from_sec(self.stop_time)

    def stay_idle(self):
        """ sends zero velocity to stay put """

        vel_g_msg = Twist()
        self.nav_vel_pub.publish(vel_g_msg)

    def dyn_cfg_callback(self, config, level):
        rospy.loginfo("Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}, spline_alpha:{spline_alpha}, traj_dt:{traj_dt}".format(**config))
        self.pose_controller.k1 = config["k1"]
        self.pose_controller.k2 = config["k2"]
        self.pose_controller.k3 = config["k3"]

        self.spline_alpha = config["spline_alpha"]
        self.traj_dt = config["traj_dt"]
        
        return config

    def detected_objects_name_callback(self, msg):
        #rospy.loginfo("There are %i detected objects" % len(msg.objects))
        #self.detected_objects = msg
        for obj in msg.objects:
            #rospy.loginfo("obj1: %s" % obj)
            self.detected_objects[obj] = (self.x, self.y, self.theta)
        self.last_box_time = rospy.get_rostime()

    def deliver_one(self):
        if not self.orderlist:
            self.vendor_start,self.gotovendor = None,False       
            return

        data = self.orderlist.pop(0)    
        if data in self.detected_objects:    
            rospy.loginfo("Popping order: %s, set goal: %s" % (data, str(self.detected_objects[data])))
            self.x_g, self.y_g, self.theta_g = self.detected_objects[data]
            self.gotovendor = True            
            self.replan()
        else:
            rospy.loginfo("New order: %s, no goal found. " % (msg.data))

    def delivery_request_callback(self, msg):
        self.orderlist = msg.data.split(',')
        self.deliver_one()
        # plan the robot
        self.last_box_time = rospy.get_rostime()


    def cmd_nav_callback(self, data):
        """
        loads in goal if different from current goal, and replans
        """
        if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
            self.x_g = data.x
            self.y_g = data.y
            self.theta_g = data.theta

            # start over everytime we change the pose
            #self.switch_mode(Mode.ALIGN)
            self.replan()

    def map_md_callback(self, msg):
        """
        receives maps meta data and stores it
        """
        self.map_width = msg.width
        self.map_height = msg.height
        self.map_resolution = msg.resolution
        self.map_origin = (msg.origin.position.x,msg.origin.position.y)

    def map_callback(self,msg):
        """
        receives new map info and updates the map
        """
        self.map_probs = msg.data
        # if we've received the map metadata and have a way to update it:
        if self.map_width>0 and self.map_height>0 and len(self.map_probs)>0:
            self.occupancy = StochOccupancyGrid2D(self.map_resolution,
                                                  self.map_width,
                                                  self.map_height,
                                                  self.map_origin[0],
                                                  self.map_origin[1],
                                                  6, # TODO: was 8
                                                  self.map_probs)
            if self.x_g is not None and self.mode != Mode.STOP:
                # if we have a goal to plan to, replan
                rospy.loginfo("replanning because of new map")
                self.replan() # new map, need to replan

    def shutdown_callback(self):
        """
        publishes zero velocities upon rospy shutdown
        """
        cmd_vel = Twist()
        cmd_vel.linear.x = 0.0
        cmd_vel.angular.z = 0.0
        self.nav_vel_pub.publish(cmd_vel)

    def near_goal(self):
        """
        returns whether the robot is close enough in position to the goal to
        start using the pose controller
        """
        return linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])) < self.near_thresh

    def at_goal(self):
        """
        returns whether the robot has reached the goal position with enough
        accuracy to return to idle state
        """
        #print(linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])))
        return (linalg.norm(np.array([self.x-self.x_g, self.y-self.y_g])) < self.at_thresh and abs(wrapToPi(self.theta - self.theta_g)) < self.at_thresh_theta)

    def aligned(self):
        """
        returns whether robot is aligned with starting direction of path
        (enough to switch to tracking controller)
        """
        return (abs(wrapToPi(self.theta - self.th_init)) < self.theta_start_thresh)
        
    def close_to_plan_start(self):
        return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)

    def snap_to_grid(self, x):
        return (self.plan_resolution*round(x[0]/self.plan_resolution), self.plan_resolution*round(x[1]/self.plan_resolution))

    def switch_mode(self, new_mode):
        rospy.loginfo("Switching from %s -> %s", self.mode, new_mode)
        self.mode = new_mode

    def publish_planned_path(self, path, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for state in path:
            pose_st = PoseStamped()
            pose_st.pose.position.x = state[0]
            pose_st.pose.position.y = state[1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_smoothed_path(self, traj, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for i in range(traj.shape[0]):
            pose_st = PoseStamped()
            pose_st.pose.position.x = traj[i,0]
            pose_st.pose.position.y = traj[i,1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_control(self):
        """
        Runs appropriate controller depending on the mode. Assumes all controllers
        are all properly set up / with the correct goals loaded
        """
        t = self.get_current_plan_time()

        if self.mode == Mode.PARK:
            V, om = self.pose_controller.compute_control(self.x, self.y, self.theta, t)
        elif self.mode == Mode.TRACK:
            V, om = self.traj_controller.compute_control(self.x, self.y, self.theta, t)
        elif self.mode == Mode.ALIGN:
            V, om = self.heading_controller.compute_control(self.x, self.y, self.theta, t)
        else:
            V = 0.
            om = 0.

        self.v = V
        cmd_vel = Twist()
        cmd_vel.linear.x = V
        cmd_vel.angular.z = om
        # rospy.loginfo("Reconfigure Request: V: %f, om:%f, mode: %s" % (V, om, self.mode))
        self.nav_vel_pub.publish(cmd_vel)

    def get_current_plan_time(self):
        t = (rospy.get_rostime()-self.current_plan_start_time).to_sec()
        return max(0.0, t)  # clip negative time to 0

    def replan(self):
        """
        loads goal into pose controller
        runs planner based on current pose
        if plan long enough to track:
            smooths resulting traj, loads it into traj_controller
            sets self.current_plan_start_time
            sets mode to ALIGN
        else:
            sets mode to PARK
        """
        # Make sure we have a map
        if not self.occupancy:
            rospy.loginfo("Navigator: replanning canceled, waiting for occupancy map.")
            self.switch_mode(Mode.IDLE)
            return

        # Attempt to plan a path
        state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
        state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
        x_init = self.snap_to_grid((self.x, self.y))
        self.plan_start = x_init
        x_goal = self.snap_to_grid((self.x_g, self.y_g))
        problem = AStar(state_min,state_max,x_init,x_goal,self.occupancy,self.plan_resolution)

        rospy.loginfo("Navigator: computing navigation plan")
        success =  problem.solve()
        if success:
            rospy.loginfo("Navigator: computing navigation plan SUCCESS!!!!!!!!")
        else:
            rospy.loginfo("Navigator: computing navigation plan FAILLLLL!!")
        if not success:
            self.gotovendor = False
            rospy.loginfo("Planning failed")
            return
        rospy.loginfo("Planning Succeeded")

        planned_path = problem.path
        

        # Check whether path is too short
        if len(planned_path) < 4:
            rospy.loginfo("Path too short to track")
            self.switch_mode(Mode.PARK)
            return

        # Smooth and generate a trajectory
        traj_new, t_new = compute_smoothed_traj(planned_path, self.v_des, self.spline_alpha, self.traj_dt)

        # If currently tracking a trajectory, check whether new trajectory will take more time to follow
        if self.mode == Mode.TRACK:
            t_remaining_curr = self.current_plan_duration - self.get_current_plan_time()

            # Estimate duration of new trajectory
            th_init_new = traj_new[0,2]
            th_err = wrapToPi(th_init_new - self.theta)
            t_init_align = abs(th_err/self.om_max)
            t_remaining_new = t_init_align + t_new[-1]

            if t_remaining_new > t_remaining_curr:
                rospy.loginfo("New plan rejected (longer duration than current plan)")
                self.publish_smoothed_path(traj_new, self.nav_smoothed_path_rej_pub)
                return

        # Otherwise follow the new plan
        self.publish_planned_path(planned_path, self.nav_planned_path_pub)
        self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)

        self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)
        self.traj_controller.load_traj(t_new, traj_new)

        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = t_new[-1]

        self.th_init = traj_new[0,2]
        self.heading_controller.load_goal(self.th_init)

        if not self.aligned():
            rospy.loginfo("Not aligned with start direction")
            self.switch_mode(Mode.ALIGN)
            return

        rospy.loginfo("Ready to track")
        self.switch_mode(Mode.TRACK)

    def run(self):
        rate = rospy.Rate(10) # 10 Hz
        while not rospy.is_shutdown():
            # try to get state information to update self.x, self.y, self.theta
            try:
                (translation,rotation) = self.trans_listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
                self.x = translation[0]
                self.y = translation[1]
                euler = tf.transformations.euler_from_quaternion(rotation)
                self.theta = euler[2]
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
                self.current_plan = []
                rospy.loginfo("Navigator: waiting for state info")
                self.switch_mode(Mode.IDLE)
                print e
                pass

            # STATE MACHINE LOGIC
            # some transitions handled by callbacks
            if self.mode == Mode.IDLE:
                self.stopped = False
            elif self.mode == Mode.ALIGN:
                if self.aligned():
                    self.current_plan_start_time = rospy.get_rostime()
                    self.switch_mode(Mode.TRACK)
            elif self.mode == Mode.TRACK:
                if self.near_goal():
                    self.switch_mode(Mode.PARK)
                elif not self.close_to_plan_start():
                    rospy.loginfo("replanning because far from start")
                    self.replan()
                elif (rospy.get_rostime() - self.current_plan_start_time).to_sec() > self.current_plan_duration:
                    rospy.loginfo("replanning because out of time")
                    self.replan() # we aren't near the goal but we thought we should have been, so replan
            elif self.mode == Mode.PARK:
                if self.at_goal():
                    # forget about goal:
                    self.x_g = None
                    self.y_g = None
                    self.theta_g = None
                    if self.gotovendor:
                        self.switch_mode(Mode.VENDOR)
                    else:
                        self.switch_mode(Mode.IDLE)
            elif self.mode == Mode.VENDOR:
                if self.vendor_start == None:
                    self.vendor_start = rospy.get_rostime()
                t_vendor = (rospy.get_rostime()-self.vendor_start).to_sec()
                if t_vendor > self.vendor_time:
                    if not self.orderlist:
                        self.x_g,self.y_g,self.theta_g = self.home_location
                        self.vendor_start,self.gotovendor = None,False                    
                        self.replan()  
                    else:
                        self.deliver_one()
                    
            elif self.mode == Mode.STOP:
                # At a stop sign
                if self.has_stopped():
                    self.replan()
                    self.stop_sign_start = None
                else:
                    self.stay_idle()
                
            self.publish_control()
            rate.sleep()
Ejemplo n.º 15
0
class Navigator:
    """
    This node handles point to point turtlebot motion, avoiding obstacles.
    It is the sole node that should publish to cmd_vel
    """
    def __init__(self):
        rospy.init_node('turtlebot_navigator', anonymous=True)
        self.mode = Mode.IDLE

        # WE CHANGED THIS STUFF
        # Time to stop at a stop sign
        self.stop_time = rospy.get_param("~stop_time", 3.)

        # Minimum distance from a stop sign to obey it (originally 0.5)
        self.stop_min_dist = rospy.get_param("~stop_min_dist", 0.7)

        # Time taken to cross an intersection
        self.crossing_time = rospy.get_param("~crossing_time", 3.)

        # Number of stop sign detections in a row
        self.stop_detections = 0

        # Stop sign detector
        rospy.Subscriber('/detector/stop_sign', DetectedObject,
                         self.stop_sign_detected_callback)

        # END OF WE CHANGED THIS STUFF

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0, 0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution = 0.1
        self.plan_horizon = 15

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0., 0.]

        # Robot limits
        self.v_max = 0.2  # maximum velocity
        self.om_max = 0.4  # maximum angular velocity

        self.v_des = 0.12  # desired cruising velocity
        #self.v_des = 0.2
        self.theta_start_thresh = 0.05  # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2  # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2

        # goal state threshold
        self.at_thresh = 0.09  #0.05
        self.at_thresh_theta = 0.2  #0.1

        # trajectory smoothing
        self.spline_alpha = 0.005  #0.01   #0.05 # 0.15
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5  # 0.5
        self.kpy = 0.5  # 0.5
        self.kdx = 1.5  # 1.5
        self.kdy = 1.5  # 1.5

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx,
                                                 self.kdy, self.v_max,
                                                 self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max,
                                              self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path',
                                                    Path,
                                                    queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path',
                                                     Path,
                                                     queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher(
            '/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)

        rospy.Subscriber('/map', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)

        # ADD CURRENT STATE PUBLISHER (11/15/20 DEM)
        self.current_state_pub = rospy.Publisher('/current_state',
                                                 Pose2D,
                                                 queue_size=10)

        print "finished init"

    ######## CALLBACK FUNCTIONS ########

    def stop_sign_detected_callback(self, msg):
        """ callback for when the detector has found a stop sign. Note that
        a distance of 0 can mean that the lidar did not pickup the stop sign at all """

        # distance of the stop sign
        dist = msg.distance

        # confidence of stop sign
        conf = msg.confidence

        # if close enough and in nav mode, stop
        if dist > 0 and dist < self.stop_min_dist and self.mode == Mode.TRACK and conf > 0.8:
            self.stop_detections = self.stop_detections + 1
            rospy.loginfo("Our stop sign distance is " + str(dist))
            rospy.loginfo("Our stop sign confidence is " + str(conf))
        else:
            self.stop_detections = 0  # reset (might want to delete this??)

        # if we've made a detection more than twice in a row
        if self.stop_detections > 2:
            self.stop_detections = 0
            self.init_stop_sign()

    def dyn_cfg_callback(self, config, level):
        rospy.loginfo(
            "Reconfigure Request: k1:{k1}, k2:{k2}, k3:{k3}".format(**config))
        self.pose_controller.k1 = config["k1"]
        self.pose_controller.k2 = config["k2"]
        self.pose_controller.k3 = config["k3"]
        return config

    def cmd_nav_callback(self, data):
        """
        loads in goal if different from current goal, and replans
        """
        if data.x != self.x_g or data.y != self.y_g or data.theta != self.theta_g:
            self.x_g = data.x
            self.y_g = data.y
            self.theta_g = data.theta
            rospy.loginfo("Replanning because of new cmd nav goal")
            self.replan()

    def map_md_callback(self, msg):
        """
        receives maps meta data and stores it
        """
        self.map_width = msg.width
        self.map_height = msg.height
        self.map_resolution = msg.resolution
        self.map_origin = (msg.origin.position.x, msg.origin.position.y)

    def map_callback(self, msg):
        """
        receives new map info and updates the map
        """
        self.map_probs = msg.data
        # if we've received the map metadata and have a way to update it:
        if self.map_width > 0 and self.map_height > 0 and len(
                self.map_probs) > 0:
            self.occupancy = StochOccupancyGrid2D(
                self.map_resolution, self.map_width, self.map_height,
                self.map_origin[0], self.map_origin[1], 8, self.map_probs)
            if self.x_g is not None:
                # if we have a goal to plan to, replan

                # EREZ added condition 11/11/20
                if self.mode == Mode.TRACK:
                    rospy.loginfo("replanning because of new map")
                    self.replan()  # new map, need to replan
                    rospy.loginfo("replanning at state:" + str(self.mode))

    def shutdown_callback(self):
        """
        publishes zero velocities upon rospy shutdown
        """
        cmd_vel = Twist()
        cmd_vel.linear.x = 0.0
        cmd_vel.angular.z = 0.0
        self.nav_vel_pub.publish(cmd_vel)

    ######## STATE MACHINE ACTIONS ########

    def init_stop_sign(self):
        """ initiates a stop sign maneuver """
        self.stop_sign_start = rospy.get_rostime()
        self.switch_mode(Mode.STOP)

    def has_stopped(self):
        """ checks if stop sign maneuver is over """
        return self.mode == Mode.STOP and \
               rospy.get_rostime() - self.stop_sign_start > rospy.Duration.from_sec(self.stop_time)

    def init_crossing(self):
        """ initiates an intersection crossing maneuver """
        self.cross_start = rospy.get_rostime()
        self.switch_mode(Mode.CROSS)

    def has_crossed(self):
        """ checks if crossing maneuver is over """
        return self.mode == Mode.CROSS and \
               rospy.get_rostime() - self.cross_start > rospy.Duration.from_sec(self.crossing_time)

    def near_goal(self):
        """
        returns whether the robot is close enough in position to the goal to
        start using the pose controller
        """
        return linalg.norm(np.array([self.x - self.x_g, self.y - self.y_g
                                     ])) < self.near_thresh

    def at_goal(self):
        """
        returns whether the robot has reached the goal position with enough
        accuracy to return to idle state
        """
        # CHANGED FROM NEAR_THRESH TO AT_THRESH
        return (linalg.norm(np.array(
            [self.x - self.x_g, self.y - self.y_g])) < self.at_thresh and abs(
                wrapToPi(self.theta - self.theta_g)) < self.at_thresh_theta)

    def aligned(self):
        """
        returns whether robot is aligned with starting direction of path
        (enough to switch to tracking controller)
        """
        return (abs(wrapToPi(self.theta - self.th_init)) <
                self.theta_start_thresh)

    def close_to_plan_start(self):
        return (abs(self.x - self.plan_start[0]) < self.start_pos_thresh
                and abs(self.y - self.plan_start[1]) < self.start_pos_thresh)

    def snap_to_grid(self, x):
        return (self.plan_resolution * round(x[0] / self.plan_resolution),
                self.plan_resolution * round(x[1] / self.plan_resolution))

    def switch_mode(self, new_mode):
        rospy.loginfo("NAVIGatOR: switching from %s -> %s", self.mode,
                      new_mode)
        self.mode = new_mode

    def publish_planned_path(self, path, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for state in path:
            pose_st = PoseStamped()
            pose_st.pose.position.x = state[0]
            pose_st.pose.position.y = state[1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_smoothed_path(self, traj, publisher):
        # publish planned plan for visualization
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        for i in range(traj.shape[0]):
            pose_st = PoseStamped()
            pose_st.pose.position.x = traj[i, 0]
            pose_st.pose.position.y = traj[i, 1]
            pose_st.pose.orientation.w = 1
            pose_st.header.frame_id = 'map'
            path_msg.poses.append(pose_st)
        publisher.publish(path_msg)

    def publish_control(self):
        """
        Runs appropriate controller depending on the mode. Assumes all controllers
        are all properly set up / with the correct goals loaded
        """
        t = self.get_current_plan_time()

        if self.mode == Mode.PARK:
            V, om = self.pose_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.TRACK or self.mode == Mode.CROSS:
            V, om = self.traj_controller.compute_control(
                self.x, self.y, self.theta, t)
        elif self.mode == Mode.ALIGN:
            V, om = self.heading_controller.compute_control(
                self.x, self.y, self.theta, t)
        else:  # STOP/IDLE
            V = 0.
            om = 0.

        cmd_vel = Twist()
        cmd_vel.linear.x = V
        cmd_vel.angular.z = om
        self.nav_vel_pub.publish(cmd_vel)

    def get_current_plan_time(self):
        t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
        return max(0.0, t)  # clip negative time to 0

    def replan(self):
        """
        loads goal into pose controller
        runs planner based on current pose
        if plan long enough to track:
            smooths resulting traj, loads it into traj_controller
            sets self.current_plan_start_time
            sets mode to ALIGN
        else:
            sets mode to PARK
        """
        # Make sure we have a map
        if not self.occupancy:
            rospy.loginfo(
                "Navigator: replanning canceled, waiting for occupancy map.")
            self.switch_mode(Mode.IDLE)
            return

        # Attempt to plan a path
        state_min = self.snap_to_grid((-self.plan_horizon, -self.plan_horizon))
        state_max = self.snap_to_grid((self.plan_horizon, self.plan_horizon))
        x_init = self.snap_to_grid((self.x, self.y))
        self.plan_start = x_init
        x_goal = self.snap_to_grid((self.x_g, self.y_g))
        astar_problem = AStar(state_min, state_max, x_init, x_goal,
                              self.occupancy, self.plan_resolution)

        rrt_problem = GeometricRRTConnect(state_min, state_max, x_init, x_goal,
                                          self.occupancy, self.plan_resolution)

        rospy.loginfo("Navigator: computing navigation plan")
        rospy.loginfo("Current mode: " + str(self.mode))

        astar_success = astar_problem.solve()
        astar_path = astar_problem.path

        rrt_success = rrt_problem.solve()
        rrt_path = rrt_problem.path

        success = (astar_success or rrt_success)
        if not success:
            rospy.loginfo("Planning failed")
            return
        else:
            rospy.loginfo("Planning Succeeded")

        if astar_success:
            traj_new_astar, t_new_astar, path_length_astar = compute_smoothed_traj(
                astar_path, self.v_des, self.spline_alpha, self.traj_dt)
        else:
            path_length_astar = np.Inf

        if rrt_success:
            traj_new_rrt, t_new_rrt, path_length_rrt = compute_smoothed_traj(
                rrt_path, self.v_des, self.spline_alpha, self.traj_dt)
        else:
            path_length_rrt = np.Inf

        if path_length_astar <= path_length_rrt:
            traj_new = traj_new_astar
            t_new = t_new_astar
            planned_path = astar_path
            rospy.loginfo("AStar Gave Shortest Path")
        else:
            traj_new = traj_new_rrt
            t_new = t_new_rrt
            planned_path = rrt_path
            rospy.loginfo("RRT Gave Shortest Path")

        # load goal into pose controller
        self.pose_controller.load_goal(self.x_g, self.y_g, self.theta_g)

        # Check whether path is too short
        '''
        if len(planned_path) < 4:
            rospy.loginfo("Path too short to track")
            self.switch_mode(Mode.PARK)
            return
        '''
        # Smooth and generate a trajectory

        # If currently tracking a trajectory, check whether new trajectory will take more time to follow
        if self.mode == Mode.TRACK:
            t_remaining_curr = self.current_plan_duration - self.get_current_plan_time(
            )

            # Estimate duration of new trajectory
            th_init_new = traj_new[0, 2]
            th_err = wrapToPi(th_init_new - self.theta)
            t_init_align = abs(th_err / self.om_max)
            t_remaining_new = t_init_align + t_new[-1]

            if t_remaining_new > t_remaining_curr:
                rospy.loginfo(
                    "New plan rejected (longer duration than current plan)")
                self.publish_smoothed_path(traj_new,
                                           self.nav_smoothed_path_rej_pub)
                return

        # Otherwise follow the new plan
        self.publish_planned_path(planned_path, self.nav_planned_path_pub)
        self.publish_smoothed_path(traj_new, self.nav_smoothed_path_pub)

        self.traj_controller.load_traj(t_new, traj_new)

        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = t_new[-1]

        self.th_init = traj_new[0, 2]
        self.heading_controller.load_goal(self.th_init)

        if (not self.aligned() and self.mode != Mode.CROSS and self.mode != Mode.STOP) \
            or (self.mode == Mode.IDLE):
            rospy.loginfo("Not aligned with start direction")
            self.switch_mode(Mode.ALIGN)
            return

# COMMENTED OUT
#rospy.loginfo("Ready to track")
#self.switch_mode(Mode.TRACK)

    def run(self):
        rate = rospy.Rate(10)  # 10 Hz
        while not rospy.is_shutdown():
            # try to get state information to update self.x, self.y, self.theta
            try:
                (translation, rotation) = self.trans_listener.lookupTransform(
                    '/map', '/base_footprint', rospy.Time(0))
                self.x = translation[0]
                self.y = translation[1]
                euler = tf.transformations.euler_from_quaternion(rotation)
                self.theta = euler[2]
                # PUBLISH CURRENT STATE (11/15/20 DEM)
                current_state = Pose2D()
                current_state.x = self.x
                current_state.y = self.y
                current_state.theta = self.theta
                self.current_state_pub.publish(current_state)

            except (tf.LookupException, tf.ConnectivityException,
                    tf.ExtrapolationException) as e:
                self.current_plan = []
                rospy.loginfo("Navigator: waiting for state info")
                self.switch_mode(Mode.IDLE)
                print e
                pass

            # STATE MACHINE LOGIC
            # some transitions handled by callbacks
            if self.mode == Mode.IDLE:
                pass
            elif self.mode == Mode.ALIGN:
                if self.aligned():
                    self.current_plan_start_time = rospy.get_rostime()
                    self.switch_mode(Mode.TRACK)
            elif self.mode == Mode.TRACK:
                # if near goal switch to PARK mode
                if self.near_goal():
                    self.switch_mode(Mode.PARK)
                elif not self.close_to_plan_start():
                    rospy.loginfo("replanning because far from start")
                    self.replan()
                elif (rospy.get_rostime() - self.current_plan_start_time
                      ).to_sec() > self.current_plan_duration:
                    rospy.loginfo("replanning because out of time")
                    self.replan(
                    )  # we aren't near the goal but we thought we should have been, so replan
            elif self.mode == Mode.PARK:
                if self.at_goal():
                    # forget about goal:

                    # EREZ COMMENTED OUT 11/18/20
                    #self.x_g = None
                    #self.y_g = None
                    #self.theta_g = None

                    self.switch_mode(Mode.IDLE)
            elif self.mode == Mode.STOP:
                if self.has_stopped():
                    self.init_crossing()
                # otherwise we will be commanding 0 velocity
            elif self.mode == Mode.CROSS:
                # crossing the intersection
                if self.has_crossed():
                    self.switch_mode(Mode.ALIGN)
            else:
                raise Exception("This mode is not supported: {}".format(
                    str(self.mode)))

            self.publish_control()
            rate.sleep()
Ejemplo n.º 16
0
    def __init__(self):
    
        rospack = rospkg.RosPack()
        package_dir = rospack.get_path("final_project")
        self.points_path = package_dir + "/scripts/points.txt"
        rospy.init_node('turtlebot_navigator', anonymous=True)
        if SAVE_POINTS:
            with open(self.points_path, "w") as f:
                f.write("x, y, theta\n")
        self.mode = Mode.IDLE

        # current state
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0

        # goal state
        self.x_g = None
        self.y_g = None
        self.theta_g = None

        self.th_init = 0.0

        # map parameters
        self.map_width = 0
        self.map_height = 0
        self.map_resolution = 0
        self.map_origin = [0,0]
        self.map_probs = []
        self.occupancy = None
        self.occupancy_updated = False

        # plan parameters
        self.plan_resolution =  0.1
        self.plan_horizon = 15

        # time when we started following the plan
        self.current_plan_start_time = rospy.get_rostime()
        self.current_plan_duration = 0
        self.plan_start = [0.,0.]
        
        # Robot limits
        self.v_max = 0.2    # maximum velocity
        self.om_max = 0.4   # maximum angular velocity

        self.v_des = 0.12   # desired cruising velocity
        self.theta_start_thresh = 0.05   # threshold in theta to start moving forward when path-following
        self.start_pos_thresh = 0.2     # threshold to be far enough into the plan to recompute it

        # threshold at which navigator switches from trajectory to pose control
        self.near_thresh = 0.2
        self.at_thresh = 0.02
        self.at_thresh_theta = 0.05

        # trajectory smoothing
        self.spline_alpha = 0.05
        self.traj_dt = 0.1

        # trajectory tracking controller parameters
        self.kpx = 0.5
        self.kpy = 0.5
        self.kdx = 1.5
        self.kdy = 1.5

        self.stop_time = 0.0

        # heading controller parameters
        self.kp_th = 2.

        self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max)
        self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max)
        self.heading_controller = HeadingController(self.kp_th, self.om_max)

        self.nav_planned_path_pub = rospy.Publisher('/planned_path', Path, queue_size=10)
        self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path', Path, queue_size=10)
        self.nav_smoothed_path_rej_pub = rospy.Publisher('/cmd_smoothed_path_rejected', Path, queue_size=10)
        self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.nav_mode_pub = rospy.Publisher('/nav_mode', Int16, queue_size=10)
        # Publish robot's perception of own state
        self.robot_pose_current_pub = rospy.Publisher('/robot/pose/current', Pose2D, queue_size=10)
        self.trans_listener = tf.TransformListener()

        self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback)

        rospy.Subscriber('/map_inflated', OccupancyGrid, self.map_callback)
        rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback)
        rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback)
        rospy.Subscriber('/detector/objects', DetectedObjectList, self.object_detected_callback)

        print "finished init"