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'
