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
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()
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()