def test_cross_readings(self):
particle = FilterParticle()
old_reading = (
Odometry(),
Blob(),
)
new_odom = Odometry()
new_odom.pose.pose.orientation = heading_to_quaternion(math.pi / 2)
new_reading = (
new_odom,
Blob(),
)
result = particle.cross_readings(old_reading, new_reading)
self.assertIsNotNone(result)
self.assertEqual(result[0], 0.0)
self.assertEqual(result[1], 0.0)
new_odom = Odometry()
new_odom.pose.pose.orientation = heading_to_quaternion(0.0)
new_odom.pose.pose.position.y = -1.0
new_reading = (
new_odom,
Blob(),
)
result = particle.cross_readings(old_reading, new_reading)
self.assertIsNone(result)
def easy_odom(self):
x, y, heading = self.core.summary()
otto = Odometry()
otto.header.frame_id = 'odom'
otto.header.stamp = rospy.Time.now()
otto.pose.pose.position.x = x
otto.pose.pose.position.y = y
otto.pose.pose.orientation = heading_to_quaternion(heading)
return otto
def easy_odom(self):
x, y, heading = self.core.summary()
otto = Odometry()
otto.header.frame_id = 'odom'
otto.header.stamp = rospy.Time.now()
otto.pose.pose.position.x = x
otto.pose.pose.position.y = y
otto.pose.pose.orientation = heading_to_quaternion(heading)
return otto
def generate_plan(self, start, goal):
# debug = None
debug = rospy.loginfo
if debug is not None:
debug('potential generate plane\n'+str(goal))
deck = deque()
deck.append(start)
next_ = deepcopy(start)
distance = (math.pow(next_.position.x-goal.position.x, 2)
+ math.pow(next_.position.y-goal.position.y, 2))
if distance < .01:
debug('the start distance is very small')
return []
debug('dist: %f' % (distance,))
step_size = 0.2
count = max(10, int(1.0/step_size))
while(distance > .01 and count >= 0):
debug('start calculating obs_force')
obs_force = self.calc_potential(next_)
debug('end calculating obs_force')
goal_force = self.goal_force(next_, goal)
total_force = addv(obs_force, goal_force)
total_force = unit(total_force)
debug('%s\n%s\n%s' % (obs_force, goal_force, total_force))
dx = total_force[0]*step_size
dy = total_force[1]*step_size
debug('d: %f , %f' % (dx, dy,))
new_pose = Pose()
new_pose.position.x = next_.position.x+dx
new_pose.position.y = next_.position.y+dy
new_pose.orientation = heading_to_quaternion(math.atan2(dy, dx))
deck.append(new_pose)
next_ = deepcopy(new_pose)
distance = (math.pow(next_.position.x-goal.position.x, 2)
+ math.pow(next_.position.y-goal.position.y, 2))
debug('dist: %f' % (distance,))
count += -1
if (distance > .01):
debug('count break')
else:
deck.append(goal)
return list(deck)
def __init__(self, state=None):
if state is None:
state = Odometry()
state.pose.pose.position.x = 0.0
state.pose.pose.position.y = 0.0
state.pose.pose.orientation = heading_to_quaternion(0.0)
self.state = state
self.feature_set = {}
self.potential_features = {}
self.weight = 1
self.hypothesis_set = {}
self.next_id = 1
def __init__(self, state=None):
if state is None:
state = Odometry()
state.pose.pose.position.x = 0.0
state.pose.pose.position.y = 0.0
state.pose.pose.orientation = heading_to_quaternion(0.0)
self.state = state
self.feature_set = {}
self.potential_features = {}
self.weight = 1
self.hypothesis_set = {}
self.next_id = 1
def motion_model(particle, twist, dt):
# pS h1 |
# 0-----------------0
# | h2
v = twist.linear.x
w = twist.angular.z
# new_particle = FilterParticle()
new_particle = copy_module.deepcopy(particle)
print(new_particle.state.pose.pose.position.y)
print(particle.state.pose.pose.position.y)
dheading = twist.angular.z * dt
drive_noise = normal(0, abs(.005*v)+abs(.001*w)+.0001, 1)
print('original '+str(twist.linear.x * dt))
print('drive noise '+str(drive_noise))
ds = twist.linear.x * dt + drive_noise
print('and now, ds '+str(ds))
prev_heading = quaternion_to_heading(particle.state.pose.pose.orientation)
print('prev_heading '+str(prev_heading))
heading_noise = normal(0, abs(.005*w)+abs(.001*v)+.0001, 1)
heading_1 = prev_heading+dheading/2+heading_noise
print('heading_1 '+str(heading_1))
print('heading noise '+str(heading_noise))
heading_noise = normal(0, abs(.005*w)+abs(.001*v)+.0001, 1)
heading_2 = heading_1+dheading/2+heading_noise
print('heading_2 '+str(heading_1))
print('heading noise '+str(heading_noise))
dx = ds*cos(heading_1)
dy = ds*sin(heading_1)
new_particle.state.pose.pose.position.x += dx
new_particle.state.pose.pose.position.y += dy
# pylint: disable=line-too-long
new_particle.state.pose.pose.orientation = heading_to_quaternion(heading_2)
return new_particle
def motion_model(particle, twist, dt):
# pS h1 |
# 0-----------------0
# | h2
v = twist.linear.x
w = twist.angular.z
# new_particle = FilterParticle()
new_particle = copy_module.deepcopy(particle)
print(new_particle.state.pose.pose.position.y)
print(particle.state.pose.pose.position.y)
dheading = twist.angular.z * dt
drive_noise = normal(0, abs(.005 * v) + abs(.001 * w) + .0001, 1)
print('original ' + str(twist.linear.x * dt))
print('drive noise ' + str(drive_noise))
ds = twist.linear.x * dt + drive_noise
print('and now, ds ' + str(ds))
prev_heading = quaternion_to_heading(particle.state.pose.pose.orientation)
print('prev_heading ' + str(prev_heading))
heading_noise = normal(0, abs(.005 * w) + abs(.001 * v) + .0001, 1)
heading_1 = prev_heading + dheading / 2 + heading_noise
print('heading_1 ' + str(heading_1))
print('heading noise ' + str(heading_noise))
heading_noise = normal(0, abs(.005 * w) + abs(.001 * v) + .0001, 1)
heading_2 = heading_1 + dheading / 2 + heading_noise
print('heading_2 ' + str(heading_1))
print('heading noise ' + str(heading_noise))
dx = ds * cos(heading_1)
dy = ds * sin(heading_1)
new_particle.state.pose.pose.position.x += dx
new_particle.state.pose.pose.position.y += dy
# pylint: disable=line-too-long
new_particle.state.pose.pose.orientation = heading_to_quaternion(heading_2)
return new_particle
def process_position(self, odom):
odom.header.frame_id = 'map'
self.original_odom.publish(odom)
# print('add noise')
# noise = np.random.normal(0,1)
## noise params ##
# pose.pose.position.[x,y] 2
# pose.pose.orientation.[x,y,z,w] (heading) 3
# twist.twist.linear.[x] 4
# twist.twist.angular.[z] 5
x = odom.pose.pose.position.x
y = odom.pose.pose.position.y
v = abs(odom.twist.twist.linear.x)
w = abs(odom.twist.twist.angular.z)
odom.pose.pose.position.x += noise(0,self.position_variation) # 1
odom.pose.pose.position.y += noise(0,self.position_variation) # 2
if odom.pose.pose.position.z: # if it is not 0
odom.pose.pose.position.z += noise(0,self.position_variation)
heading = quaternion_to_heading(odom.pose.pose.orientation)
heading += noise(0,self.heading_variation+.05*w+.01*v)
odom.pose.pose.orientation = heading_to_quaternion(heading) # 3
odom.twist.twist.linear.x += noise(0,self.linear_vel+.05*v+.01*w) # 4
if odom.twist.twist.linear.y: # if it is not 0
odom.twist.twist.linear.y += noise(0,self.linear_vel)
if odom.twist.twist.linear.z: # if it is not 0
odom.twist.twist.linear.z += noise(0,self.linear_vel)
odom.twist.twist.angular.z += noise(0,self.angular_vel+.05*w+.01*v) # 5
if odom.twist.twist.angular.x: # if it is not 0
odom.twist.twist.angular.x += noise(0,self.angular_vel)
if odom.twist.twist.angular.y: # if it is not 0
odom.twist.twist.angular.y += noise(0,self.angular_vel)
self.noisy_odom.publish(odom)
def motion_model(self, particle, twist, dt):
# pS h1 |
# 0-----------------0
# | h2
# dt = dt.secs + dt.nsecs*math.pow(10,-9)
# rospy.loginfo('dt secs: '+str(dt.to_sec()))
dt = dt.to_sec()
v = twist.linear.x
w = twist.angular.z
new_particle = FilterParticle()
new_particle = copy_module.deepcopy(particle)
dheading = twist.angular.z * dt
drive_noise = normal(0, abs(.05 * v) + abs(.005 * w) + .0005, 1)
ds = twist.linear.x * dt + drive_noise
prev_heading = quaternion_to_heading(
particle.state.pose.pose.orientation)
heading_noise = normal(0, abs(.025 * w) + abs(.005 * v) + .0005, 1)
heading_1 = prev_heading + dheading / 2 + heading_noise
heading_noise = normal(0, abs(.025 * w) + abs(.005 * v) + .0005, 1)
heading_2 = heading_1 + dheading / 2 + heading_noise
# rospy.loginfo('asdf;kjasdf; '+str(heading_1 ))
dx = ds * cos(heading_1)
dy = ds * sin(heading_1)
# rospy.loginfo('ds delta: '+str(ds - v*dt))
new_particle.state.pose.pose.position.x += dx
new_particle.state.pose.pose.position.y += dy
# pylint: disable=line-too-long
new_particle.state.pose.pose.orientation = heading_to_quaternion(
heading_2)
return new_particle
def generate_initial_path(self):
"""
Path creation for node
"""
rospy.loginfo('generating generate_initial_path')
# Note: this is called once during node initialization
end = self.path_goal().goal # Odometry
start = self.path_start().goal # Odometry
start.header.frame_id = 'odom'
self.targets = []
self.targets.append(start)
# pylint: disable=invalid-name
# dt, dx, dy properly express what I'm trying to get across
# i.e. differential time, x, y
dt = .1
des_speed = .5 # m/s
dx = end.pose.pose.position.x - start.pose.pose.position.x
dy = end.pose.pose.position.y - start.pose.pose.position.y
# total dx above
heading = math.atan2(dy, dx)
step_x = des_speed*math.cos(heading)*dt
step_y = des_speed*math.sin(heading)*dt
rospy.loginfo('step_x: '+str(step_x))
distance = math.sqrt(dx*dx+dy*dy)
steps = math.floor(distance/(des_speed*dt))
rospy.loginfo('steps generated? '+str(steps))
for i in range(1, int(steps)+1):
rospy.loginfo('a;sdf '+str(i))
odo = Odometry()
odo.header.frame_id = 'odom'
odo.pose.pose.position = Point(x=start.pose.pose.position.x+i*step_x, y=start.pose.pose.position.y+i*step_y)
rospy.loginfo('gen x: '+str(start.pose.pose.position.x+i*step_x))
rospy.loginfo('gen y: '+str(start.pose.pose.position.y+i*step_y))
odo.pose.pose.orientation = heading_to_quaternion(heading)
odo.twist.twist.linear = Vector3(x=des_speed)
odo.twist.twist.angular = Vector3()
self.targets.append(odo)
self.index = 0
def motion_model_noisy(self, previous_state, twist, dt):
'''
A method that returns a noisy (default) version of the motion model for
evolving a previous state based on a twist and a time step
Inputs:
previous_state - ROS Odometry-like
twist - ROS Twist message
dt - a time delta
Outputs:
next_state - ROS Odometry-like
'''
# pS h1 |
# 0-----------------0
# | h2
dheading = twist.twist.angular.z * dt
drive_noise = normal(0, .05, 1)
ds = twist.twist.linear.x * dt + drive_noise
# prev_x = previous_state.pose.pose.position.x
# prev_y = previous_state.pose.pose.position.y
# pylint: disable=line-too-long
prev_heading = quaternion_to_heading(
previous_state.pose.pose.orientation)
heading_noise = normal(0, .05, 1)
heading_1 = prev_heading + dheading / 2 + heading_noise
heading_noise = normal(0, .05, 1)
heading_2 = heading_1 + dheading / 2 + heading_noise
dx = ds * math.cos(heading_1)
dy = ds * math.cos(heading_1)
previous_state.pose.pose.position.x += dx
previous_state.pose.pose.position.y += dy
previous_state.pose.pose.orientation = heading_to_quaternion(heading_2)
return previous_state
def motion_model(self, particle, twist, dt):
# pS h1 |
# 0-----------------0
# | h2
# dt = dt.secs + dt.nsecs*math.pow(10,-9)
# rospy.loginfo('dt secs: '+str(dt.to_sec()))
dt = dt.to_sec()
v = twist.linear.x
w = twist.angular.z
new_particle = FilterParticle()
new_particle = copy_module.deepcopy(particle)
dheading = twist.angular.z * dt
drive_noise = normal(0, abs(.05*v)+abs(.005*w)+.0005, 1)
ds = twist.linear.x * dt + drive_noise
prev_heading = quaternion_to_heading(particle.state.pose.pose.orientation)
heading_noise = normal(0, abs(.025*w)+abs(.005*v)+.0005, 1)
heading_1 = prev_heading+dheading/2+heading_noise
heading_noise = normal(0, abs(.025*w)+abs(.005*v)+.0005, 1)
heading_2 = heading_1+dheading/2+heading_noise
# rospy.loginfo('asdf;kjasdf; '+str(heading_1 ))
dx = ds*cos(heading_1)
dy = ds*sin(heading_1)
# rospy.loginfo('ds delta: '+str(ds - v*dt))
new_particle.state.pose.pose.position.x += dx
new_particle.state.pose.pose.position.y += dy
# pylint: disable=line-too-long
new_particle.state.pose.pose.orientation = heading_to_quaternion(heading_2)
return new_particle
def motion_model_noisy(self, previous_state, twist, dt):
'''
A method that returns a noisy (default) version of the motion model for
evolving a previous state based on a twist and a time step
Inputs:
previous_state - ROS Odometry-like
twist - ROS Twist message
dt - a time delta
Outputs:
next_state - ROS Odometry-like
'''
# pS h1 |
# 0-----------------0
# | h2
dheading = twist.twist.angular.z * dt
drive_noise = normal(0, .05, 1)
ds = twist.twist.linear.x * dt + drive_noise
# prev_x = previous_state.pose.pose.position.x
# prev_y = previous_state.pose.pose.position.y
# pylint: disable=line-too-long
prev_heading = quaternion_to_heading(previous_state.pose.pose.orientation)
heading_noise = normal(0, .05, 1)
heading_1 = prev_heading+dheading/2+heading_noise
heading_noise = normal(0, .05, 1)
heading_2 = heading_1+dheading/2+heading_noise
dx = ds*math.cos(heading_1)
dy = ds*math.cos(heading_1)
previous_state.pose.pose.position.x += dx
previous_state.pose.pose.position.y += dy
previous_state.pose.pose.orientation = heading_to_quaternion(heading_2)
return previous_state
def sample_motion_model(self, v, w, dt):
'''
Return an odometry message sampled from the distribution defined by the
probabilistic motion model based on this statemodel
Does not yet take full advantage of state stored in above
And does not check acceleration bounds for example
'''
# TODO(buckbaskin): use the v,w data stored above to add accel limits
# TODO(buckbaskin): use the v,w data to create more accurate v_hat
noise = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) # no noise for now, ideal motion
v_hat = v + self.sample_normal(noise[0]*v+noise[1]*w)
w_hat = w + self.sample_normal(noise[2]*v+noise[3]*w)
y_hat = self.sample_normal(noise[4]*v+noise[5]*w)
if w_hat < .001:
# rospy.loginfo('what is too small')
x_new = self.x + v_hat*math.cos(self.theta)
y_new = self.y + v_hat*math.sin(self.theta)
else:
# rospy.loginfo('smm: '+str(v)+' , '+str(w)+' , '+str(dt))
x_new = (self.x - v_hat/w_hat*math.sin(self.theta)
+ v_hat/w_hat*math.sin(self.theta+w_hat*dt))
# rospy.loginfo('smm: dx '+str(x_new-self.x))
y_new = (self.y - v_hat/w_hat*math.cos(self.theta)
- v_hat/w_hat*math.cos(self.theta+w_hat*dt))
# rospy.loginfo('smm: dy '+str(y_new-self.y))
theta_new = self.theta + w_hat*dt + y_hat*dt
new_odom = Odometry()
new_odom.pose.pose.position.x = x_new
new_odom.pose.pose.position.y = y_new
new_odom.pose.pose.orientation = heading_to_quaternion(theta_new)
new_odom.twist.twist.linear.x = v_hat
new_odom.twist.twist.angular.z = w_hat
new_odom.header.frame_id = self.frame_id
return new_odom
def generate_next_path(self):
"""
generate a new path, either forwards or backwards (rvs == True)
"""
end = self.path_next().goal
start = self.path_start().goal
self.targets = []
self.targets.append(start)
# pylint: disable=invalid-name
# dt, dx, dy properly express what I'm trying to get across
# i.e. differential time, x, y
dt = .1
des_speed = .5 # m/s
dx = end.pose.pose.position.x - start.pose.pose.position.x
dy = end.pose.pose.position.y - start.pose.pose.position.y
heading = math.atan2(dy, dx)
dx = des_speed*math.cos(heading)*dt
dy = des_speed*math.sin(heading)*dt
distance = math.sqrt(dx*dx+dy*dy)
steps = math.floor(distance/des_speed)
for i in range(1, int(steps)):
odo = Odometry()
odo.header.frame_id = 'odom'
odo.pose.pose.point = Point(x=start.x+i*dx, y=start.y+i*dy)
odo.pose.pose.orientation = heading_to_quaternion(heading)
odo.twist.twist.linear = Vector3(x=des_speed)
odo.twist.twist.angular = Vector3()
self.targets.append(odo)
if rvs:
self.index = len(self.targets)-2
else:
self.index = 0
def generate_initial_path(self):
"""
Path creation for node
"""
rospy.loginfo('generate_initial_path!!!')
# Note: this is called once during node initialization
end = self.path_goal().goal # Odometry
start = self.path_start().goal # Odometry
self.targets = []
self.targets.append(start)
# pylint: disable=invalid-name
# dt, dx, dy properly express what I'm trying to get across
# i.e. differential time, x, y
dt = .1
des_speed = .5 # m/s
dx = end.pose.pose.position.x - start.pose.pose.position.x
dy = end.pose.pose.position.y - start.pose.pose.position.y
heading = math.atan2(dy, dx)
dx = des_speed*math.cos(heading)*dt
dy = des_speed*math.sin(heading)*dt
distance = math.sqrt(dx*dx+dy*dy)
steps = math.floor(distance/des_speed)
for i in range(1, int(steps)):
odo = Odometry()
odo.pose.pose.point = Point(x=start.x+i*dx, y=start.y+i*dy)
odo.pose.pose.orientation = heading_to_quaternion(heading)
odo.twist.twist.linear = Vector3(x=des_speed)
odo.twist.twist.angular = Vector3()
self.targets.append(odo)
self.index = 0
def send_current_odom(self):
"""
send_current_odom: publish the current state as an Odometry message
input: None
output: (ROS publish) nav_msgs.msg.Odometry
"""
o = Odometry()
o.header.seq = self.seq
o.header.stamp = rospy.Time.now()
o.header.frame_id = '/odom'
o.pose.pose.position.x = self.x
o.pose.pose.position.y = self.y
o.pose.pose.orientation = heading_to_quaternion(self.heading)
o.twist.twist.linear.x = self.v
o.twist.twist.angular.z = self.omega
self.seq += 1
self.pub.publish(o)
def sample_motion_model2(self, v, w, dt):
'''
Return an odometry message sampled from the distribution defined by the
probabilistic motion model based on this statemodel
Does not yet take full advantage of state stored in above
And does not check acceleration bounds for example
'''
accel_max = 1
alpha_max = 1
delta_v_req = v - self.v
delta_v_max = accel_max*dt
if delta_v_req > 0:
if delta_v_max > delta_v_req:
accel_time = delta_v_req / accel_max
v_avg = v*(dt-accel_time) + (v+accel_max*2.0)*accel_time
v_new = v
else:
v_avg = self.v+accel_max/2.0
v_new = self.v+accel_max
else:
if delta_v_max > abs(delta_v_req):
accel_time = abs(delta_v_req) / accel_max
v_avg = v*(dt-accel_time) + (v-accel_max*2.0)*accel_time
v_new = v
else:
v_avg = self.v-accel_max/2.0
v_new = self.v-accel_max
ds = v_avg*dt
# rospy.loginfo('smm2: ds: '+str(ds))
delta_w_req = w - self.w
delta_w_max = alpha_max*dt
if delta_w_req > 0:
if delta_w_max > delta_w_req:
accel_time = delta_w_req / alpha_max
w_avg = w*(dt-accel_time) + (w+alpha_max*2.0)*accel_time
w_new = w
else:
w_avg = self.w+alpha_max/2.0
w_new = self.w+alpha_max
else:
if delta_w_max > abs(delta_w_req):
accel_time = abs(delta_w_req) / alpha_max
w_avg = w*(dt-accel_time) + (w-alpha_max*2.0)*accel_time
w_new = w
else:
w_avg = self.w-alpha_max/2.0
w_new = self.w-alpha_max
dtheta = w_avg*dt
theta_avg = self.theta + dtheta/2.0
new_odom = Odometry()
new_odom.pose.pose.position.x = self.x+ds*math.cos(theta_avg)
new_odom.pose.pose.position.y = self.y+ds*math.sin(theta_avg)
new_odom.pose.pose.orientation = heading_to_quaternion(self.theta + dtheta)
new_odom.twist.twist.linear.x = v_new
new_odom.twist.twist.angular.z = w_new
new_odom.header.frame_id = self.frame_id
return new_odom
def expand_tree(self):
IAddedThisMany = 0
if self.reached_goal():
# don't expand
return
if self.goal is not None and random.uniform(0.0, 1.0) < .10:
# choose the goal with 10% certainty
rospy.loginfo('greedy step')
expand_to_pose = self.goal
else:
# put the expand_to_pose in random free space
# rospy.loginfo('minx: %f maxx: %f' % (self.minx, self.maxx,))
x = random.uniform(self.minx, self.maxx)
y = random.uniform(self.miny, self.maxy)
expand_to_pose = Pose()
expand_to_pose.position.x = x
expand_to_pose.position.y = y
while self.obstacles.check_collision(expand_to_pose):
x = random.uniform(self.minx, self.maxx)
y = random.uniform(self.miny, self.maxy)
expand_to_pose.position.x = x
expand_to_pose.position.y = y
# try to expand up to that node, storing the expand_from node
# rospy.loginfo('expand_tree fnn: %s' % expand_to_pose)
expand_from_id = self.find_nearest_node(expand_to_pose)
expand_from_pose = self[expand_from_id]
collision_step = 0.1
plan_step = 1.0
collision_steps_per_plan = int(plan_step/collision_step)
count = 1
dx = expand_to_pose.position.x - expand_from_pose.position.x
dy = expand_to_pose.position.y - expand_from_pose.position.y
distance = math.sqrt(math.pow(dx, 2) + math.pow(dy, 2))
collision_steps = int(distance/collision_step)
dx = (dx / distance) * collision_step
dy = (dy / distance) * collision_step
bearing_to_next = math.atan2(dy, dx)
collision_pose = Pose()
collision_pose.position.x = expand_from_pose.position.x + dx
collision_pose.position.y = expand_from_pose.position.y + dy
collision_pose.orientation = heading_to_quaternion(bearing_to_next)
expand_to_pose.orientation = collision_pose.orientation
# check collision
# self.obstacles.check_collision(choose_pose)
# if that node is less than one step away, check collision and add it
# else, maintain the last step and second to last step.
# if there is a collision in the last step, add the second to last
# as an element in the rrt
# otherwise, loop until the first step reaches the goal, see first if
while count <= collision_steps:
if self.obstacles.check_collision(collision_pose):
# if there was a collision
if not (count % collision_steps_per_plan) == 1:
# if I've advanced past the first node after a plan step
# step back
collision_pose.position.x += -dx
collision_pose.position.y += -dy
# make a new node at that point
self.add_node_rrt(collision_pose)
IAddedThisMany += 1
break
else:
# I just added a plan-step node
break
else:
# there isn't an obstacle, I can keep going
if (count % collision_steps_per_plan) == 0:
# add a planner-step pose (every meter)
self.add_node_rrt(collision_pose)
IAddedThisMany += 1
collision_pose.position.x += dx
collision_pose.position.y += dy
distance += -collision_step
count += 1
if distance <= collision_step:
# the loop ran all the way through
if not self.obstacles.check_collision(expand_to_pose):
# if there isn't a collision at the expand_to pose
self.add_node_rrt(expand_to_pose)
IAddedThisMany += 1
rospy.loginfo('expanded %d. %f' % (IAddedThisMany, self.reached_goal_dist(),))
