def get_a_list_of_pose_to_goal(self, start, goal, tf_listener, dist_limit=None, pathpub=None, wppub=None, skip=1): resolution = self.og.info.resolution width = self.og.info.width height = self.og.info.height offsetX = self.og.info.origin.position.x offsetY = self.og.info.origin.position.y tf_listener.waitForTransform('odom', 'map', rospy.Time(0), rospy.Duration(10.0)) tstart = tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(start)) tgoal = tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(goal)) if dist_limit is not None: tgoal = self.limit_max_dist(tstart, tgoal, dist_limit) if wppub is not None: ttgoal = tf_listener.transformPose('map', fuck_the_time(tgoal)) wppub.publish(ttgoal) start_grid_pos = pose2gridpos(tstart.pose, resolution, offsetX, offsetY) goal_grid_pos = pose2gridpos(tgoal.pose, resolution, offsetX, offsetY) try: path = aStar(self.make_navigable_gridpos(), start_grid_pos, goal_grid_pos, self.astarnodescb) wp = self.getWaypoints(path, tgoal, publisher=pathpub) list_of_pose = [] # magic number. skip a few waypoints in the beginning for stuff in wp: retp = PoseStamped() retp.pose = stuff.pose retp.header = self.og.header list_of_pose.append(retp) return list_of_pose except NoPathFoundException as e: raise e
def limit_max_dist(self, fr_posestamped, to_posestamped, max_dist): assert fr_posestamped.header.frame_id == to_posestamped.header.frame_id assert max_dist > 0 angle = get_direction(fr_posestamped, to_posestamped) dist = get_distance(fr_posestamped, to_posestamped) if dist > max_dist: print 'next waypoint at dist ' + str(dist) + ' limiting the dist to ' + str(max_dist) ps = PoseStamped() ps.pose.position.x = (fr_posestamped.pose.position.x + math.cos(angle) * max_dist) ps.pose.position.y = (fr_posestamped.pose.position.y + math.sin(angle) * max_dist) # ps.pose.position.x = 1 # ps.pose.position.y = 1 ps.header.frame_id = fr_posestamped.header.frame_id quaternion = tf.transformations.quaternion_from_euler(0, 0, angle) ps.pose.orientation.x = quaternion[0] ps.pose.orientation.y = quaternion[1] ps.pose.orientation.z = quaternion[2] ps.pose.orientation.w = quaternion[3] ps = fuck_the_time(ps) print 'actual dist ' + str(get_distance(fr_posestamped, ps)) return ps else: return to_posestamped
def getWaypoints(self, list_of_gridpos, goal_pose, publisher=None): resolution = self.og.info.resolution width = self.og.info.width height = self.og.info.height offsetX = self.og.info.origin.position.x offsetY = self.og.info.origin.position.y wp = Path() wp.header.frame_id = self.og.header.frame_id poses = [] lastdirection = -999 for i in range(0, len(list_of_gridpos) - 1): direction = getDirection(list_of_gridpos[i], list_of_gridpos[i + 1]) if direction != lastdirection: lastdirection = copy.deepcopy(direction) newPose = PoseStamped() newPose.header.frame_id = self.og.header.frame_id newPose.pose = Pose() newPose.pose.position = getPoint(list_of_gridpos[i], resolution, offsetX, offsetY) quaternion = tf.transformations.quaternion_from_euler(0, 0, direction) newPose.pose.orientation.x = quaternion[0] newPose.pose.orientation.y = quaternion[1] newPose.pose.orientation.z = quaternion[2] newPose.pose.orientation.w = quaternion[3] poses.append(newPose) newPose = self.tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(goal_pose)) poses.append(newPose) wp.poses = poses if publisher is not None: publisher.publish(wp) return wp.poses
def get_my_posestamped(self): self.tf_listener.waitForTransform('map', 'odom', rospy.Time(0), rospy.Duration(10.0)) ps = PoseStamped() ps.pose = self.odom.pose.pose ps.header = self.odom.header current_pose_stamped_in_map = self.tf_listener.transformPose('/map', fuck_the_time(ps)) return current_pose_stamped_in_map
def navToPose_but_dont_care_end_orientation(whatever_frame_goal): """Drive to a goal subscribed to from /move_base_simple/goal""" # compute angle required to make straight-line move to desired pose global xPosition global yPosition global theta global odom_list odom_list.waitForTransform('odom', 'map', rospy.Time(0), rospy.Duration(10.0)) goal = odom_list.transformPose('map', fuck_the_time(whatever_frame_goal)) initialX = xPosition initialY = yPosition initialT = theta # capture desired x and y positions desiredY = goal.pose.position.y desiredX = goal.pose.position.x # capture desired angle quat = goal.pose.orientation q = [quat.x, quat.y, quat.z, quat.w] roll, pitch, yaw = euler_from_quaternion(q) desiredT = yaw dx = desiredX - initialX dy = desiredY - initialY distance = math.sqrt((dx) ** 2 + (dy) ** 2) pathT = math.atan2(dy, dx) initialTurn = getMinimalAngleDifference(pathT, initialT) finalTurn = getMinimalAngleDifference(desiredT, pathT) rotate(initialTurn) driveSmooth(0.25, distance)
def navToPose_but_dont_care_end_orientation(whatever_frame_goal): """Drive to a goal subscribed to from /move_base_simple/goal""" # compute angle required to make straight-line move to desired pose global xPosition global yPosition global theta global odom_list odom_list.waitForTransform('odom', 'map', rospy.Time(0), rospy.Duration(10.0)) goal = odom_list.transformPose('map', fuck_the_time(whatever_frame_goal)) initialX = xPosition initialY = yPosition initialT = theta # capture desired x and y positions desiredY = goal.pose.position.y desiredX = goal.pose.position.x # capture desired angle quat = goal.pose.orientation q = [quat.x, quat.y, quat.z, quat.w] roll, pitch, yaw = euler_from_quaternion(q) desiredT = yaw dx = desiredX - initialX dy = desiredY - initialY distance = math.sqrt((dx)**2 + (dy)**2) pathT = math.atan2(dy, dx) initialTurn = getMinimalAngleDifference(pathT, initialT) finalTurn = getMinimalAngleDifference(desiredT, pathT) rotate(initialTurn) driveSmooth(0.25, distance)
def test_get_goal_points(self, event): print "test get goal points" points = self.fe.get_nav_goal_point_candidates() next_goal = PoseStamped() next_goal.pose.position = points.pop(0) next_goal.header.frame_id = 'map' next_goal = fuck_the_time(next_goal) self.next_goal_pub.publish(next_goal)
def test_get_goal_points(self,event): print "test get goal points" points = self.fe.get_nav_goal_point_candidates() next_goal = PoseStamped() next_goal.pose.position = points.pop(0) next_goal.header.frame_id = 'map' next_goal = fuck_the_time(next_goal) self.next_goal_pub.publish(next_goal)
def point2pose(point): next_goal = PoseStamped() next_goal.pose.position = point q = tf.transformations.quaternion_from_euler(0, 0, 0) next_goal.pose.orientation.x = q[0] next_goal.pose.orientation.y = q[1] next_goal.pose.orientation.z = q[2] next_goal.pose.orientation.w = q[3] next_goal.header.frame_id = 'map' next_goal = fuck_the_time(next_goal) return next_goal
def point2pose(point): next_goal = PoseStamped() next_goal.pose.position = point q = tf.transformations.quaternion_from_euler(0, 0, 0) next_goal.pose.orientation.x = q[0] next_goal.pose.orientation.y = q[1] next_goal.pose.orientation.z = q[2] next_goal.pose.orientation.w = q[3] next_goal.header.frame_id = 'map' next_goal = fuck_the_time(next_goal) return next_goal
def plan_a_path_and_nav_to_goal(goal): global pose global globalCostmapThing global localCostmapThing global odom_list global pubintermediategoalpose global where_the_f_am_i_going global pubglobalwp global pubglobalpath ps = PoseStamped() ps.pose = pose ps.header.frame_id = 'map' ps.header.stamp = rospy.Time(0) localdone = False globaldone = False nextwp = None local_feasible = True global_feasible = True while not globaldone: try: print 'planning for global map' nextwp, globaldone = globalCostmapThing.getNextWaypoint(ps, goal, odom_list, wppub=pubglobalwp, pathpub=pubglobalpath, skip=1) global_feasible = True localdone = False while not localdone: try: print 'tryin to replan in the local map' # nextwp_l, localdone = localCostmapThing.getNextWaypoint(ps, nextwp, odom_list, pathpub=pubrealpath, # dist_limit=1, # wppub=pubintermediategoalpose, skip=2) # nextwp = localCostmapThing.getNextWaypoint(ps, goal, odom_list, pathpub=pubrealpath, # wppub=pubintermediategoalpose, dist_limit=0.5) if global_feasible and not local_feasible: print 'found path in global but no path in local. go to global path.' navToPose_but_dont_care_end_orientation(nextwp) list_of_pose = localCostmapThing.get_a_list_of_pose_to_goal(ps, nextwp, odom_list, pathpub=pubrealpath, dist_limit=0.8, wppub=pubintermediategoalpose) local_feasible = True for i, nextwp_l in enumerate(list_of_pose): if 0 < i < 2: odom_list.waitForTransform('odom', 'map', rospy.Time(0), rospy.Duration(10.0)) nextwp_lt = odom_list.transformPose('map', fuck_the_time(nextwp_l)) where_the_f_am_i_going.publish(nextwp_lt) print 'naving the local waypoints' navToPose_but_dont_care_end_orientation(nextwp_lt) localdone = True # nextwp = nextwp_l except NoPathFoundException: local_feasible = False print 'no path found in local. replan in global' try: nextwp, globaldone = globalCostmapThing.getNextWaypoint(ps, goal, odom_list, wppub=pubglobalwp, pathpub=pubglobalpath, skip=1) global_feasible = True except: global_feasible = False print 'no path found to goal. give up' return print 'Reached local goal' except NoPathFoundException: global_feasible = False print 'cannot find a path in global map' return navToPose(goal) print 'Reached global goal'
def plan_a_path_and_nav_to_goal(goal): global pose global globalCostmapThing global localCostmapThing global odom_list global pubintermediategoalpose global where_the_f_am_i_going global pubglobalwp global pubglobalpath ps = PoseStamped() ps.pose = pose ps.header.frame_id = 'map' ps.header.stamp = rospy.Time(0) localdone = False globaldone = False nextwp = None local_feasible = True global_feasible = True while not globaldone: try: print 'planning for global map' nextwp, globaldone = globalCostmapThing.getNextWaypoint( ps, goal, odom_list, wppub=pubglobalwp, pathpub=pubglobalpath, skip=1) global_feasible = True localdone = False while not localdone: try: print 'tryin to replan in the local map' # nextwp_l, localdone = localCostmapThing.getNextWaypoint(ps, nextwp, odom_list, pathpub=pubrealpath, # dist_limit=1, # wppub=pubintermediategoalpose, skip=2) # nextwp = localCostmapThing.getNextWaypoint(ps, goal, odom_list, pathpub=pubrealpath, # wppub=pubintermediategoalpose, dist_limit=0.5) if global_feasible and not local_feasible: print 'found path in global but no path in local. go to global path.' navToPose_but_dont_care_end_orientation(nextwp) list_of_pose = localCostmapThing.get_a_list_of_pose_to_goal( ps, nextwp, odom_list, pathpub=pubrealpath, dist_limit=0.8, wppub=pubintermediategoalpose) local_feasible = True for i, nextwp_l in enumerate(list_of_pose): if 0 < i < 2: odom_list.waitForTransform('odom', 'map', rospy.Time(0), rospy.Duration(10.0)) nextwp_lt = odom_list.transformPose( 'map', fuck_the_time(nextwp_l)) where_the_f_am_i_going.publish(nextwp_lt) print 'naving the local waypoints' navToPose_but_dont_care_end_orientation(nextwp_lt) localdone = True # nextwp = nextwp_l except NoPathFoundException: local_feasible = False print 'no path found in local. replan in global' try: nextwp, globaldone = globalCostmapThing.getNextWaypoint( ps, goal, odom_list, wppub=pubglobalwp, pathpub=pubglobalpath, skip=1) global_feasible = True except: global_feasible = False print 'no path found to goal. give up' return print 'Reached local goal' except NoPathFoundException: global_feasible = False print 'cannot find a path in global map' return navToPose(goal) print 'Reached global goal'