Exemple #1
0
    def iterate(self):
        global plotter

        curPos = self.curPos
        heading = self.heading
        cloud = self.cloud
        goalPos = self.goalPos
        curTwist = self.curTwist

        msg = Twist()

        if len(self.pdata.ranges) == 0:
            print 'got invalid data from GetPlanarData service...',\
                  'so um is anything being published to /planar_data?'
            return msg

        if goalPos.z == TIMEOUT_ERROR:
            # this indicates a goal data timeout in the DataServiceProvider
            # making us stop if GoalMaker has stopped publishing to /goal
            rospy.loginfo("stopping because of a goal data timeout")
            return msg

        distToGoal = GLib.euclid(goalPos.x, goalPos.y, curPos.x, curPos.y)

        print "distance to goal:", distToGoal

        if distToGoal < CLOSE_ENOUGH_TO_GOAL:
            rospy.loginfo("stopping because we're close enough to the goal")
            return msg

        if plotter:
            plotEverything(
                curPos.x,
                curPos.y,
                heading,
                goalPos.x,
                goalPos.y,
                cloud
                )

        (msg.angular.z, msg.linear.x) = calcDynamicWindow(
            curPos.x,
            curPos.y,
            heading,
            curTwist.linear.x,
            curTwist.angular.z,
            cloud,
            goalPos.x,
            goalPos.y,
            1/RATE,
            plotter
            )

        return msg
Exemple #2
0
def step():
    global cur_x, cur_y, cur_dir, cur_lin, cur_ang, goal_x, goal_y

    plotEverything()

    mycloud = []
    for cloudPoint in cloud:
        if GLib.euclid(cloudPoint.x, cloudPoint.y, cur_x, cur_y) < CLEARANCE_MAX:
            mycloud.append(cloudPoint);

    (cur_ang, cur_lin) = calcDynamicWindow(
        cur_x,
        cur_y,
        cur_dir,
        cur_lin,
        cur_ang,
        mycloud,
        goal_x,
        goal_y,
        DT,
        plotter
        )

    kine = GLib.calcTrajectoryStepFromTime(
        cur_x,
        cur_y,
        cur_dir,
        cur_lin,
        cur_ang,
        DT
        )

    cur_x = kine[0]
    cur_y = kine[1]
    cur_dir = kine[2]

    plotter.plotArrow(cur_x, cur_y, cur_dir, "red")
    plotter.display()
Exemple #3
0
def step():
    global cur_x, cur_y, cur_dir, cur_lin, cur_ang, goal_x, goal_y

    plotEverything()

    mycloud = []
    for cloudPoint in cloud:
        if GLib.euclid(cloudPoint.x, cloudPoint.y, cur_x,
                       cur_y) < CLEARANCE_MAX:
            mycloud.append(cloudPoint)

    (cur_ang, cur_lin) = calcDynamicWindow(cur_x, cur_y, cur_dir, cur_lin,
                                           cur_ang, mycloud, goal_x, goal_y,
                                           DT, plotter)

    kine = GLib.calcTrajectoryStepFromTime(cur_x, cur_y, cur_dir, cur_lin,
                                           cur_ang, DT)

    cur_x = kine[0]
    cur_y = kine[1]
    cur_dir = kine[2]

    plotter.plotArrow(cur_x, cur_y, cur_dir, "red")
    plotter.display()