def __init__(self):
#self.rate = rospy.Rate(10)
self.subs=Subscribers()
self.commands=Commands()
self.pose = PoseStamped()
self.comp=Computations()
self.setpoint_publisher = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10)
def __init__(self):
rospy.init_node("test_mission",log_level=rospy.INFO) #initialize the pixhawk node
print('node initialized')
self.rate=rospy.Rate(10)
self.subs=Subscribers() #initialize streaming of subscribers
self.comp=Computations()
self.commands=Commands()
self.navigation=Navigation()
self.arrow=Arrow() #object for arrow detection class
self.road_follow=RoadFollow() #object for road detection class
#initialize the publishers
self.setpoint_publisher = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10)
self.setvel_publisher = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size = 10)
self.setaccel_publisher = rospy.Publisher("/mavros/setpoint_accel/accel",Vector3Stamped,queue_size=10)
print('Publishers initialized')
def test_schlick_approximation_with_perpendicular_viewing_angle(self):
shape = GlassSphere()
r = Ray(Point(0, 0, 0), Vector(0, 1, 0))
xs = Intersection.intersections(Intersection(-1,shape), Intersection(1, shape))
comps = Computations.prepare_computations(xs[1], r, xs)
reflectance = World.schlick(comps)
self.assertAlmostEqual(reflectance, 0.04, delta = Constants.epsilon)
def test_schlick_approximation_under_total_internal_reflection(self):
shape = GlassSphere()
r = Ray(Point(0, 0, math.sqrt(2) / 2), Vector(0, 1, 0))
xs = Intersection.intersections(Intersection(-math.sqrt(2) / 2, shape), Intersection(math.sqrt(2) / 2, shape))
comps = Computations.prepare_computations(xs[1], r, xs)
reflectance = World.schlick(comps)
self.assertEqual(reflectance, 1.0)
def test_findinf_n1_n2_at_various_intersections(self):
a = GlassSphere()
a.transform = Transformations.scaling(2, 2, 2)
a.material.refractive_index = 1.5
b = GlassSphere()
b.transform = Transformations.translation(0, 0, -0.25)
b.material.refractive_index = 2.0
c = GlassSphere()
c.transform = Transformations.translation(0, 0, 0.25)
c.material.refractive_index = 2.5
r = Ray(Point(0, 0, -4), Vector(0, 0, 1))
xs = Intersection.intersections(Intersection(2, a), Intersection(2.75, b), Intersection(3.25, c), Intersection(4.75, b), Intersection(5.25, c), Intersection(6, a))
RefractiveIndices = namedtuple("RefractiveIndices", ["n1", "n2"])
refractive_indices_list = [
RefractiveIndices(1.0, 1.5),
RefractiveIndices(1.5, 2.0),
RefractiveIndices(2.0, 2.5),
RefractiveIndices(2.5, 2.5),
RefractiveIndices(2.5, 1.5),
RefractiveIndices(1.5, 1.0)
]
for index, refractive_index in enumerate(refractive_indices_list):
comps = Computations.prepare_computations(xs[index], r, xs)
print(comps.n1)
print(comps.n2)
self.assertEqual(comps.n1, refractive_index.n1)
self.assertEqual(comps.n2, refractive_index.n2)
def test_schlick_approximation_with_small_angle(self):
shape = GlassSphere()
r = Ray(Point(0, 0.99, -2), Vector(0, 0, 1))
xs = Intersection.intersections(Intersection(1.8589, shape))
comps = Computations.prepare_computations(xs[0], r, xs)
reflectance = World.schlick(comps)
self.assertAlmostEqual(reflectance, 0.48873, delta = Constants.epsilon)
def test_shading_intersection(self):
w = World.default_world()
r = Ray(Point(0, 0, -5), Vector(0, 0, 1))
shape = w.objects[0]
i = Intersection(4, shape)
comps = Computations.prepare_computations(i, r)
c = World.shade_hit(w, comps)
self.assertEqual(c, Color(0.38066, 0.47583, 0.2855))
def test_hit_offset_point(self):
r = Ray(Point(0, 0, -5), Vector(0, 0, 1))
shape = Sphere()
shape.transform = Transformations.translation(0, 0, 1)
i = Intersection(5, shape)
comps = Computations.prepare_computations(i, r)
self.assertLess(comps.over_point.z, -Constants.epsilon / 2)
self.assertGreater(comps.point.z, comps.over_point.z)
def test_refracted_color_with_opaque_surface(self):
w = World.default_world()
shape = w.objects[0]
r = Ray(Point(0, 0, -5), Vector(0, 0, 1))
xs = Intersection.intersections(Intersection(4, shape),
Intersection(6, shape))
comps = Computations.prepare_computations(xs[0], r, xs)
c = World.refracted_color(w, comps, 5)
self.assertEqual(c, Color(0, 0, 0))
def test_under_point_offset_below_surface(self):
r = Ray(Point(0, 0, -5), Vector(0, 0, 1))
shape = GlassSphere()
shape.transform = Transformations.translation(0, 0, 1)
i = Intersection(5, shape)
xs = Intersection.intersections(i)
comps = Computations.prepare_computations(i, r, xs)
self.assertGreater(comps.under_point.z, Constants.epsilon / 2)
self.assertLess(comps.point.z, comps.under_point.z)
def test_shade_intersection_inside(self):
w = World.default_world()
w.light = PointLight(Point(0, 0.25, 0), Color(1, 1, 1))
r = Ray(Point(0, 0, 0), Vector(0, 0, 1))
shape = w.objects[1]
i = Intersection(0.5, shape)
comps = Computations.prepare_computations(i, r)
c = World.shade_hit(w, comps)
self.assertEqual(c, Color(0.90498, 0.90498, 0.90498))
def test_reflected_color_nonreflective_material(self):
w = World.default_world()
r = Ray(Point(0, 0, 0), Vector(0, 0, 1))
shape = w.objects[1]
shape.material.ambient = 1
i = Intersection(1, shape)
comps = Computations.prepare_computations(i, r)
color = World.reflected_color(w, comps)
self.assertEqual(color, Color(0, 0, 0))
def test_hit_inside(self):
r = Ray(Point(0, 0, 0), Vector(0, 0, 1))
shape = Sphere()
i = Intersection(1, shape)
comps = Computations.prepare_computations(i, r)
self.assertEqual(comps.point, Point(0, 0, 1))
self.assertEqual(comps.eyev, Vector(0, 0, -1))
self.assertTrue(comps.inside)
# normal would have been (0, 0, 1), but is inverted!
self.assertEqual(comps.normalv, Vector(0, 0, -1))
def test_precompute_state_intersection(self):
r = Ray(Point(0, 0, -5), Vector(0, 0, 1))
shape = Sphere()
i = Intersection(4, shape)
comps = Computations.prepare_computations(i, r)
self.assertEqual(comps.t, i.t)
self.assertEqual(comps.object, i.object)
self.assertEqual(comps.point, Point(0, 0, -1))
self.assertEqual(comps.eyev, Vector(0, 0, -1))
self.assertEqual(comps.normalv, Vector(0, 0, -1))
def test_refracted_color_at_max_recursive_depth(self):
w = World.default_world()
shape = w.objects[0]
shape.material.transparency = 1.0
shape.material.refractive_index = 1.5
r = Ray(Point(0, 0, -5), Vector(0, 0, 1))
xs = Intersection.intersections(Intersection(4, shape),
Intersection(6, shape))
comps = Computations.prepare_computations(xs[0], r, xs)
c = World.refracted_color(w, comps, 0)
self.assertEqual(c, Color(0, 0, 0))
class Navigation():
#initialize
def __init__(self):
#self.rate = rospy.Rate(10)
self.subs=Subscribers()
self.commands=Commands()
self.pose = PoseStamped()
self.comp=Computations()
self.setpoint_publisher = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=10)
def stillActive(self):
return self.subs.state.mode == 'OFFBOARD'
def assign(self,t): #function to assign values
self.pose.pose.position.x = t[0]
self.pose.pose.position.y = t[1]
self.pose.pose.position.z = t[2]
a = t[0] - self.subs.pose.pose.position.x
b = t[1] - self.subs.pose.pose.position.y
tanin = (atan2(b,a))
quat = quaternion_from_euler(0,0,tanin)
self.pose.pose.orientation.x = quat[0]
self.pose.pose.orientation.y = quat[1]
self.pose.pose.orientation.z = quat[2]
self.pose.pose.orientation.w = quat[3]
#self.pose.pose.orientation=self.subs.pose.pose.orientation
def waypoint(self,way_x,way_y,way_z): #function to publish and travel to the next waypoint
rate = rospy.Rate(10)
x=way_x
y=way_y
z=way_z
wp_list_example=[(x,y,z)]
while not len(wp_list_example) == 0:
t = wp_list_example.pop()
self.assign(t)
print('going to', self.pose)
while abs(self.comp.compute_distance(self.pose,self.subs.pose)) > 0.5:
if(self.stillActive()):
self.setpoint_publisher.publish(self.pose)
rate.sleep()
else:
print('exiting coz mode changed')
return
'''if(self.subs.state.mode =='AUTO.LOITER' or self.subs.state.mode =='STABILIZED'):
def test_reflected_color_at_maximum_recursive_depth(self):
w = World.default_world()
shape = Plane()
shape.material.reflective = 0.5
shape.transform = Transformations.translation(0, -1, 0)
w.objects.append(shape)
r = Ray(Point(0, 0, -3), Vector(0, -math.sqrt(2) / 2,
math.sqrt(2) / 2))
i = Intersection(math.sqrt(2), shape)
comps = Computations.prepare_computations(i, r)
color = World.reflected_color(w, comps, 0)
self.assertEqual(color, Color(0, 0, 0))
def test_shade_hit_reflective_material(self):
w = World.default_world()
shape = Plane()
shape.material.reflective = 0.5
shape.transform = Transformations.translation(0, -1, 0)
w.objects.append(shape)
r = Ray(Point(0, 0, -3), Vector(0, -math.sqrt(2) / 2,
math.sqrt(2) / 2))
i = Intersection(math.sqrt(2), shape)
comps = Computations.prepare_computations(i, r)
color = World.shade_hit(w, comps)
self.assertEqual(color, Color(0.87677, 0.92436, 0.82918))
def test_refracted_color_under_total_internal_reflection(self):
w = World.default_world()
shape = w.objects[0]
shape.material.transparency = 1.0
shape.material.refractive_index = 1.5
r = Ray(Point(0, 0, math.sqrt(2) / 2), Vector(0, 1, 0))
xs = Intersection.intersections(Intersection(-math.sqrt(2) / 2, shape),
Intersection(math.sqrt(2) / 2, shape))
# NOTE: this time you're inside the sphere, so you need
# to look at the second intersection, xs[1], not xs[0]
comps = Computations.prepare_computations(xs[1], r, xs)
c = World.refracted_color(w, comps, 5)
self.assertEqual(c, Color(0, 0, 0))
def test_shade_hit_intersection_in_shadow(self):
w = World()
w.light = PointLight(Point(0, 0, -10), Color(1, 1, 1))
s1 = Sphere()
w.objects.append(s1)
s2 = Sphere()
s2.transform = Transformations.translation(0, 0, 10)
w.objects.append(s2)
r = Ray(Point(0, 0, 5), Vector(0, 0, 1))
i = Intersection(4, s2)
comps = Computations.prepare_computations(i, r)
c = World.shade_hit(w, comps)
self.assertEqual(c, Color(0.1, 0.1, 0.1))
def test_refracted_color_with_refracted_ray(self):
w = World.default_world()
a = w.objects[0]
a.material.ambient = 1.0
a.material.pattern = Pattern.test_pattern()
b = w.objects[1]
b.material.transparency = 1.0
b.material.refractive_index = 1.5
r = Ray(Point(0, 0, 0.1), Vector(0, 1, 0))
xs = Intersection.intersections(Intersection(-0.9899, a),
Intersection(-0.4899, b),
Intersection(0.4899, b),
Intersection(0.9899, a))
comps = Computations.prepare_computations(xs[2], r, xs)
c = World.refracted_color(w, comps, 5)
self.assertEqual(c, Color(0, 0.99888, 0.04725))
def test_shade_hit_with_transparent_material(self):
w = World.default_world()
floor = Plane()
floor.transform = Transformations.translation(0, -1, 0)
floor.material.transparency = 0.5
floor.material.refractive_index = 1.5
w.objects.append(floor)
ball = Sphere()
ball.material.color = Color(1, 0, 0)
ball.material.ambient = 0.5
ball.transform = Transformations.translation(0, -3.5, -0.5)
w.objects.append(ball)
r = Ray(Point(0, 0, -3), Vector(0, -math.sqrt(2) / 2,
math.sqrt(2) / 2))
xs = Intersection.intersections(Intersection(math.sqrt(2), floor))
comps = Computations.prepare_computations(xs[0], r, xs)
color = World.shade_hit(w, comps, 5)
self.assertEqual(color, Color(0.93642, 0.68642, 0.68642))
def test_preparing_normal_smooth_triangle(self):
i = Intersection(1, self.tri, 0.45, 0.25)
r = Ray(Point(-0.2, 0.3, -2), Vector(0, 0, 1))
xs = Intersection.intersections(i)
comps = Computations.prepare_computations(i, r, xs)
self.assertEqual(comps.normalv, Vector(-0.5547, 0.83205, 0))
class Mission():
def __init__(self):
rospy.init_node("test_mission",
log_level=rospy.INFO) #initialize the pixhawk node
print('node initialized')
self.rate = rospy.Rate(10)
self.subs = Subscribers() #initialize streaming of subscribers
self.comp = Computations()
self.commands = Commands()
self.navigation = Navigation()
self.arrow = Arrow() #object for arrow detection class
self.road_follow = RoadFollow() #object for road detection class
#initialize the publishers
self.setpoint_publisher = rospy.Publisher(
"/mavros/setpoint_position/local", PoseStamped, queue_size=10)
self.setvel_publisher = rospy.Publisher(
"/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10)
self.setaccel_publisher = rospy.Publisher(
"/mavros/setpoint_accel/accel", Vector3Stamped, queue_size=10)
print('Publishers initialized')
def stillActive(self):
return (self.subs.state.mode == 'OFFBOARD')
def arrowDetection(self):
#arrow detection
'''
delta_orientation=1000 #initialize the change in orientation
self.commands.set_mode('AUTO.LOITER') #change mode to hold mode
rate.sleep()
cv2.imwrite('Images123/image1.jpg', self.subs.cv_image )
while(delta_orientation ==1000): #loop to wait for drone to see the arrow
print(self.subs.state.mode)
#failsafe
if(self.subs.state.mode != 'AUTO.LOITER' and self.subs.state.mode != 'OFFBOARD'):
return
delta_orientation=self.arrow.arrow_angle(self.subs.cv_image)#get change in orientation
self.comp.send_arb_waypoints()
self.commands.set_mode('OFFBOARD') #move to offboard mode
print(delta_orientation)
self.comp.change_orientation(delta_orientation) #rotate the drone to the desired arrow orientation
rate.sleep()
cv2.imwrite('Images123/image.jpg', self.subs.cv_image) #save image for scrutiny
rate.sleep()
#road following
self.delta=1000 #initialize the next waypoint angle
#self.commands.set_mode('AUTO.LOITER') #change later after making road following robust
#rate.sleep()
while(self.delta==1000): #loop to go through to check if image is received
#failsafe
if(self.subs.state.mode != 'AUTO.LOITER' and self.subs.state.mode != 'OFFBOARD'):
return
cv2.imwrite('Images123/image.jpg', self.subs.cv_image) #save image for scrutiny
self.delta=self.road_follow.getRoadAngle(self.subs.cv_image)#get the change in the road's orientation
print(self.delta)
#self.comp.send_arb_waypoints()
#self.commands.set_mode('OFFBOARD') #change later after making road following robust
self.comp.road_next_waypoint(self.delta) #compute the next waypoint and the orientation
if(True):
self.commands.land()
else:
return
'''
def choose_mission(self):
print(
'Choose a mission:\n1. Drone take off and land\n2.Drone make a square\n3. Arrow detectiona and road follow'
)
val = input('Enter a value:')
self.way_mission(val)
#main function for the drone mission
def way_mission(self, val):
self.comp.send_arb_waypoints(
) #send arbitary number of waypoints for changing to offboard mode
self.commands.set_mode('OFFBOARD') #set mode to offboard
if (self.stillActive()):
print('going to waypoint')
if val == 1:
self.navigation.waypoint(self.subs.pose.pose.position.x,
self.subs.pose.pose.position.y,
self.subs.pose.pose.position.z + 3)
print('Takeoff completed')
if val == 2:
self.navigation.waypoint(self.subs.pose.pose.position.x,
self.subs.pose.pose.position.y,
self.subs.pose.pose.position.z + 3)
print('Takeoff completed')
self.navigation.waypoint(self.subs.pose.pose.position.x + 10,
self.subs.pose.pose.position.y,
self.subs.pose.pose.position.z)
self.navigation.waypoint(self.subs.pose.pose.position.x,
self.subs.pose.pose.position.y + 10,
self.subs.pose.pose.position.z)
self.navigation.waypoint(self.subs.pose.pose.position.x - 10,
self.subs.pose.pose.position.y,
self.subs.pose.pose.position.z)
self.navigation.waypoint(self.subs.pose.pose.position.x,
self.subs.pose.pose.position.y - 10,
self.subs.pose.pose.position.z)
#if val == 3:
# self.navigation.waypoint(self.subs.pose.pose.position.x,self.subs.pose.pose.position.y,self.subs.pose.pose.position.z+3)
# print('Takeoff completed')
# arrowDetection()
else:
print('exiting coz mode not changed')
return
'''
#arrow detection
delta_orientation=1000 #initialize the change in orientation
self.commands.set_mode('AUTO.LOITER') #change mode to hold mode
rate.sleep()
cv2.imwrite('Images123/image1.jpg', self.subs.cv_image )
while(delta_orientation ==1000): #loop to wait for drone to see the arrow
print(self.subs.state.mode)
#failsafe
if(self.subs.state.mode != 'AUTO.LOITER' and self.subs.state.mode != 'OFFBOARD'):
return
delta_orientation=self.arrow.arrow_angle(self.subs.cv_image)#get change in orientation
self.comp.send_arb_waypoints()
self.commands.set_mode('OFFBOARD') #move to offboard mode
print(delta_orientation)
self.comp.change_orientation(delta_orientation) #rotate the drone to the desired arrow orientation
rate.sleep()
cv2.imwrite('Images123/image.jpg', self.subs.cv_image) #save image for scrutiny
rate.sleep()
#road following
self.delta=1000 #initialize the next waypoint angle
#self.commands.set_mode('AUTO.LOITER') #change later after making road following robust
#rate.sleep()
while(self.delta==1000): #loop to go through to check if image is received
#failsafe
if(self.subs.state.mode != 'AUTO.LOITER' and self.subs.state.mode != 'OFFBOARD'):
return
cv2.imwrite('Images123/image.jpg', self.subs.cv_image) #save image for scrutiny
self.delta=self.road_follow.getRoadAngle(self.subs.cv_image)#get the change in the road's orientation
print(self.delta)
#self.comp.send_arb_waypoints()
#self.commands.set_mode('OFFBOARD') #change later after making road following robust
self.comp.road_next_waypoint(self.delta) #compute the next waypoint and the orientation
if(True):
self.commands.land()
else:
return
'''
if (self.stillActive()):
self.commands.land()
else:
return
def main(self):
#rate = rospy.Rate(10)
while (self.subs.state.armed != True): #check if armed
continue
print("Armed")
while (1):
if (self.subs.state.mode == 'AUTO.LOITER'
): #failsafe(will enter only if position)
self.choose_mission() #call the main mission
break
else:
continue
def test_precomputing_reflection_vector(self):
shape = Plane()
r = Ray(Point(0, 1, -1), Vector(0, -math.sqrt(2) / 2, math.sqrt(2) / 2))
i = Intersection(math.sqrt(2), shape)
comps = Computations.prepare_computations(i, r)
self.assertEqual(comps.reflectv, Vector(0, math.sqrt(2) / 2, math.sqrt(2) / 2))
def test_hit_outside(self):
r = Ray(Point(0, 0, -5), Vector(0, 0, 1))
shape = Sphere()
i = Intersection(4, shape)
comps = Computations.prepare_computations(i, r)
self.assertFalse(comps.inside)