def __init__(self, world, space, position):
'''Yes, right now we're approximating the sub as a box
Not taking any parameters because this is the literal Sub8
TODO:
- Make the thruster list a parameter
- Stop the sub from being able to fly (Only thrust if thruster_z < 0)
See Annie for how the thrusters work
'''
lx, ly, lz = 0.5588, 0.5588, 0.381 # Meters
self.radius = max((lx, ly, lz)) * 0.5 # For spherical approximation
volume = lx * ly * lz * 0.25
# self.mass = 32.75 # kg
self.mass = 25.0 # kg
density = self.mass / volume
super(self.__class__, self).__init__(world, space, position, density, lx, ly, lz)
self.truth_odom_pub = rospy.Publisher('truth/odom', Odometry, queue_size=1)
self.thruster_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=2)
self.imu_sensor_pub = rospy.Publisher('imu', Imu, queue_size=1)
self.dvl_sensor_pub = rospy.Publisher('dvl', VelocityMeasurements, queue_size=1)
self.thruster_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=2)
self.thrust_dict = {}
self.last_vel = np.array([0.0, 0.0, 0.0], dtype=np.float32)
self.cur_vel = np.array([0.0, 0.0, 0.0], dtype=np.float32)
self.space = space
# Define 4 rays to implement a DVL sensor and the relative position of the sensor on the sub
# TODO: Fix position of DVL
self.dvl_position = np.array([0.0, 0.0, 0.0])
self.dvl_rays = []
self.dvl_rays.append(ode.GeomRay(None, 1e3))
self.dvl_rays.append(ode.GeomRay(None, 1e3))
self.dvl_rays.append(ode.GeomRay(None, 1e3))
self.dvl_rays.append(ode.GeomRay(None, 1e3))
self.dvl_ray_orientations = (np.array([0.866, -.5, -1]),
np.array([0.866, .5, -1]),
np.array([-0.866, .5, -1]),
np.array([-0.866, -.5, -1]))
# Make this a parameter
# name, relative direction, relative position (COM)
self.thruster_list = [
("FLV", np.array([ 0.000, 0.0, -1]), np.array([ 0.1583, 0.16900, 0.0142])), # noqa
("FLL", np.array([-0.866, 0.5, 0]), np.array([ 0.2678, 0.27950, 0.0000])), # noqa
("FRV", np.array([ 0.000, 0.0, -1]), np.array([ 0.1583, -0.1690, 0.0142])), # noqa
("FRL", np.array([-0.866, -0.5, 0]), np.array([ 0.2678, -0.2795, 0.0000])), # noqa
("BLV", np.array([ 0.000, 0.0, 1]), np.array([-0.1583, 0.16900, 0.0142])), # noqa
("BLL", np.array([ 0.866, 0.5, 0]), np.array([-0.2678, 0.27950, 0.0000])), # noqa
("BRV", np.array([ 0.000, 0.0, 1]), np.array([-0.1583, -0.1690, 0.0142])), # noqa
("BRL", np.array([ 0.866, -0.5, 0]), np.array([-0.2678, -0.2795, 0.0000])), # noqa
]
self.last_cmd_time = time()
self.set_up_ros()
def create_ray(self, key, pos, rot, length=5.0):
""" Create a ray geom.
Arguments:
key: key identifying the geom.
pos: position to start the ray from.
rot: list of pitch,roll,yaw in degrees (determines where it points)
"""
# Create body
body = ode.Body(self.world)
M = ode.Mass()
M.setBox(0.1, 0.1, 0.1, 0.1)
body.setMass(M)
# Set parameters for drawing the body
body.shape = "box"
body.boxsize = (0.1, 0.1, 0.1)
body.setPosition((pos[0], pos[1], pos[2]))
geom = ode.GeomRay(self.space, length)
geom.setPosition((pos))
#geom.setRotation(self.form_rotation(rot))
body.setRotation(self.form_rotation(rot))
geom.setBody(body)
# Set min_dist for sensor.
geom.min_dist = -1
# Append to the manager lists.
self.bodies[key] = body
self.geoms[key] = geom
def process_cutter_movement(self, location_start, location_end):
# TODO: fix this workaround in the cutters shape defintions (or in ODE?)
# for now we may only move from low x/y values to higher x/y values
if (location_start.x > location_end.x) \
or (location_start.y > location_end.y):
location_start, location_end = location_end, location_start
cutter_body = ode.Body(self.world)
cutter_shape, cutter_position_func = self.cutter.get_shape("ODE")
self.space.add(cutter_shape)
cutter_shape.space = self.space
cutter_shape.setBody(cutter_body)
cutter_position_func(location_start.x, location_start.y,
location_start.z)
cutter_shape.extend_shape(location_end.x - location_start.x,
location_end.y - location_start.y,
location_end.z - location_start.z)
aabb = cutter_shape.getAABB()
cutter_height = aabb[5] - aabb[4]
# add a ray along the drill to work around an ODE bug in v0.11.1
# http://sourceforge.net/tracker/index.php?func=detail&aid=2973876&group_id=24884&atid=382799
currx, curry, currz = cutter_shape.getPosition()
ray = ode.GeomRay(self.space, cutter_height)
ray.set((currx, curry, aabb[5]), (0.0, 0.0, -1.0))
# check for collisions
all_cutter_shapes = [cutter_shape] + cutter_shape.children + [ray]
def check_collision(item):
for one_cutter_shape in all_cutter_shapes:
if ode.collide(item, one_cutter_shape):
return True
return False
for index in range(len(self.boxes)):
if check_collision(self.boxes[index]):
self.shrink_box_avoiding_collision(index, check_collision)
def loadModel(self, filename):
mesh = loadObj(filename)
body = ode.Body(self.world)
self.model = ode.GeomTriMesh(mesh, self.space[0])
self.model.setBody(body)
# rotate so that z becomes top
self.addGeom(self.model, "model")
self.model.setQuaternion(
(0.7071067811865476, 0.7071067811865475, 0, 0))
body = ode.Body(self.world)
ray = ode.GeomRay(self.space[0], 10000)
ray.setBody(body)
ray.set((0, 0, 1), (0, 0, 100000))
self.addGeom(ray, "XXXXXXX")
self.GetProperty(ray).SetColor(1, 0, 0)
self.GetProperty(ray).SetLineWidth(30)
print(self.GetProperty(ray))
self.update()
def handleReflectionForRays(self, rayContacts):
# determine if we are making more reflections, and if there are intersections with objects
newRayList = []
newRaySpace = ode.HashSpace()
intersectionList = defaultdict(list)
for ray, contactList in rayContacts.items():
for contact in contactList:
pos, normal, depth, g1, g2 = contact.getContactGeomParams()
# is it in our object list? then we need to record this intersection
obj = self.environment.getObjectFromGeom(g2)
sensor = self.findSensorForObject(obj)
if sensor is not None:
# don't be intersected by rays coming from us...
if ray.getPosition() != sensor.getPosition():
intersectionList[sensor].append(ray)
else:
try:
if obj.isObstacle:
# reflect
reflectDir = normal
reflectFrom = pos
newLength = ray.getLength() - depth
newRay = ode.GeomRay(newRaySpace, newLength)
newRay.set(reflectFrom, reflectDir)
newRay.intensity = ray.intensity
newRayList.append(newRay)
except AttributeError:
pass
# is it an obstacle?
return newRayList, newRaySpace, intersectionList
def _get_rays_for_geom(self, geom):
""" TODO: this is necessary due to a bug in the trimesh collision
detection code of ODE v0.11.1. Remove this as soon as the code is fixed.
http://sourceforge.net/tracker/index.php?func=detail&aid=2973876&group_id=24884&atid=382799
"""
minz, maxz = geom.getAABB()[-2:]
currx, curry = geom.getPosition()[0:2]
ray = ode.GeomRay(self._space, maxz - minz)
ray.set((currx, curry, maxz), (0.0, 0.0, -1.0))
return [ray]
def _parseGeomRay(self, attrs):
def start(name, attrs):
if (name == 'ext'):
pass
else:
raise errors.ChildError('ray', name)
def end(name):
if (name == 'ray'):
self._parser.pop()
length = float(attrs['length'])
self.setODEObject(ode.GeomRay(self._space, length))
self._parser.push(startElement=start, endElement=end)
def createRaysForObject(self, origin, emissionTimes, now, space):
rayList = []
# for now, just generate 8 rays:
# radiating to cube corners
directions = [(1, 1, 1), (-1, -1, -1), (1, 1, -1), (-1, -1, 1),
(1, -1, 1), (-1, 1, -1), (-1, 1, 1), (1, -1, -1)]
for et in emissionTimes:
for d in directions:
(t, pw) = et
radius = self.speed * (now - t)
newRay = ode.GeomRay(space, radius)
newRay.set(origin, d)
newRay.intensity = pw
#newRay = FieldRay(radius, origin, d, pw, space)
rayList.append(newRay)
return rayList
def initGPS(self,
gps_ops_file,
lat=0,
lon=0,
ele=0,
pressure=0,
horizon="0:0"): #'-0:32'):
self.observer.lat = str(lat) #np.deg2rad(lat)
self.observer.long = str(lon) #np.deg2rad(lon)
self.observer.elevation = ele
self.observer.pressure = pressure
self.observer.horizon = horizon
f = open(gps_ops_file)
l1 = f.readline()
while l1:
l2 = f.readline()
l3 = f.readline()
# ephem.readtle() creates a PyEphem Body object from that TLE
sat = ephem.readtle(l1, l2, l3)
self.satellites[sat.name] = {
"ephem": sat,
"visible": False,
"position": (0, 0, 0),
"ray": None
}
l1 = f.readline()
f.close()
for name in self.satellites.keys():
body = ode.Body(self.world)
ray = ode.GeomRay(self.space[0], 10000)
ray.setBody(body)
self.addGeom(ray, name)
self.GetProperty(ray).SetColor(1, 0, 0)
self.GetProperty(ray).SetLineWidth(3)
self.GetActor(ray).SetVisibility(True)
self.satellites[name]["ray"] = ray
def __init__(self, world, space, position):
'''Yes, right now we're approximating the sub as a box
Not taking many parameters because this is the literal Sub8
thruster_layout: A list of tuples, each tuple formed as...
(thruster_name, relative_direction, position_wrt_sub_center-of-mass)
All of these in the FLU body frame
TODO:
- Independent publishing rates
See Annie for how the thrusters work
'''
lx, ly, lz = 0.5588, 0.5588, 0.381 # Meters
self.radius = max((lx, ly, lz)) * 0.32 # For spherical approximation
self.mass = 32.75 # kg
density = self.mass / (lx * ly * lz) # This is a leaky fix
super(self.__class__, self).__init__(world, space, position, density, lx, ly, lz)
self.truth_odom_pub = rospy.Publisher('truth/odom', Odometry, queue_size=1)
self.imu_sensor_pub = rospy.Publisher('imu', Imu, queue_size=1)
self.dvl_sensor_pub = rospy.Publisher('dvl', VelocityMeasurements, queue_size=1)
self.thruster_sub = rospy.Subscriber('thrusters/thrust', Thrust, self.thrust_cb, queue_size=2)
self.thrust_dict = {}
self.last_force = (0.0, 0.0, 0.0)
self.last_vel = np.array([0.0, 0.0, 0.0], dtype=np.float32)
self.cur_vel = np.array([0.0, 0.0, 0.0], dtype=np.float32)
self.space = space
self.g = np.array(world.getGravity())
# TODO: Fix position of DVL, IMU
# We assume that the DVL and IMU are xyz-FLU aligned (No rotation w.r.t sub)
self.dvl_position = np.array([0.0, 0.0, 0.0])
self.imu_position = np.array([0.0, 0.0, 0.0])
# Define 4 rays to implement a DVL sensor and the relative position of the sensor on the sub
self.dvl_ray_geoms = (
ode.GeomRay(None, 1e3),
ode.GeomRay(None, 1e3),
ode.GeomRay(None, 1e3),
ode.GeomRay(None, 1e3),
)
self.dvl_ray_orientations = (
np.array([+0.866, -.5, -1]),
np.array([+0.866, +.5, -1]),
np.array([-0.866, +.5, -1]),
np.array([-0.866, -.5, -1])
)
self.dvl_rays = zip(self.dvl_ray_geoms, self.dvl_ray_orientations)
# Make this a parameter
# name, relative direction, relative position (COM)
self.thruster_list = [
("FLV", np.array([+0.000, +0.0, -1.]), np.array([+0.1583, +0.1690, +0.0142])),
("FLL", np.array([-0.866, +0.5, +0.]), np.array([+0.2678, +0.2795, +0.0000])),
("FRV", np.array([+0.000, +0.0, -1.]), np.array([+0.1583, -0.1690, +0.0142])),
("FRL", np.array([-0.866, -0.5, +0.]), np.array([+0.2678, -0.2795, +0.0000])),
("BLV", np.array([+0.000, +0.0, +1.]), np.array([-0.1583, +0.1690, +0.0142])),
("BLL", np.array([+0.866, +0.5, +0.]), np.array([-0.2678, +0.2795, +0.0000])),
("BRV", np.array([+0.000, +0.0, +1.]), np.array([-0.1583, -0.1690, +0.0142])),
("BRL", np.array([+0.866, -0.5, +0.]), np.array([-0.2678, -0.2795, +0.0000])),
]
self.thrust_range = (-70, 100)
self.last_cmd_time = rospy.Time.now()
self.set_up_ros()
def __init__(self):
Satellites.__init__(self)
body = ode.Body(self.world)
self.scan_ray = ode.GeomRay(self.space[0], 10000)
self.scan_ray.setBody(body)