def battery_state(self, battery):
'''Checks the discharge state of the battery and gives a warning
when the battery voltage gets low.
'''
if ((battery.percent <= 20) and ((battery.percent % 5) == 0)):
print 'battery.percent', battery.percent, (battery.percent % 5)
print highlight_yellow(
' Battery voltage low -- ', battery.percent,
'% left, switch to a freshly charged battery! ')
def configure(self, data):
"""Configures the motionplanner. Creates omgtools Point2point problem
with room, static and dynamic obstacles.
Args:
data :
static_obstacles
dyn_obstacles
difficult_obst
"""
mp_configured = False
self._obstacles = []
self.n_stat_obst = len(data.static_obstacles)
self.n_dyn_obst = len(data.dyn_obstacles)
if data.difficult_obst:
self.omg_update_time = rospy.get_param(
'controller/omg_update_time_slow', 0.5)
safety_margin = rospy.get_param(
'motionplanner/safety_margin_small', 0.1)
safety_weight = rospy.get_param('motionplanner/safety_weight_slow',
10.)
drone_radius = rospy.get_param('motionplanner/drone_radius_small',
0.20)
vmax = rospy.get_param('motionplanner/omg_vmax_low', 0.2)
amax = rospy.get_param('motionplanner/omg_amax_low', 0.3)
else:
self.omg_update_time = rospy.get_param(
'controller/omg_update_time', 0.5)
safety_margin = rospy.get_param('motionplanner/safety_margin', 0.2)
safety_weight = rospy.get_param('motionplanner/safety_weight', 10.)
drone_radius = rospy.get_param('motionplanner/drone_radius', 0.225)
vmax = rospy.get_param('motionplanner/omg_vmax', 0.2)
amax = rospy.get_param('motionplanner/omg_amax', 0.3)
print("amax = " + str(amax) + "vmax =" + str(vmax))
if self.n_dyn_obst:
safety_margin = rospy.get_param(
'motionplanner/safety_margin_dyn_obst', 0.2)
safety_weight = rospy.get_param(
'motionplanner/safety_weight_dyn_obst', 10.)
self._vehicle = omg.Holonomic3D(shapes=omg.Sphere(drone_radius),
bounds={
'vmax': vmax,
'vmin': -vmax,
'amax': amax,
'amin': -amax
})
self._vehicle.define_knots(knot_intervals=self.knots)
self._vehicle.set_options({
'safety_distance': safety_margin,
'safety_weight': safety_weight,
'syslimit': 'norm_2'
})
self._vehicle.set_initial_conditions([0., 0., 0.])
self._vehicle.set_terminal_conditions([0., 0., 0.])
# Environment.
room_width = rospy.get_param('motionplanner/room_width', 1.)
room_depth = rospy.get_param('motionplanner/room_depth', 1.)
room_height = rospy.get_param('motionplanner/room_height', 1.)
room_origin_x = 0.
room_origin_y = 0.
room_origin_z = room_height / 2
room = {
'shape': omg.Cuboid(room_width, room_depth, room_height),
'position': [room_origin_x, room_origin_y, room_origin_z]
}
# Static obstacles.
for k, obst in enumerate(data.static_obstacles):
if obst.obst_type.data == "inf cylinder":
# 2D shape is extended infinitely along z.
shape = omg.Circle(obst.shape[0])
position = [obst.pose[0], obst.pose[1]]
elif obst.obst_type.data == "slalom plate":
shape = omg.Beam(obst.shape[1] - 0.1, 0.1, np.pi / 2)
position = [obst.pose[0], obst.pose[1]]
elif obst.obst_type.data == "hexagon":
shape = omg.RegularPrisma(obst.shape[0], obst.shape[1], 6)
position = [obst.pose[0], obst.pose[1], obst.pose[2]]
elif obst.obst_type.data == "window plate":
if (k % 4) <= 1: # Side plates 1 and 2.
shape = omg.Beam(obst.shape[1] - 0.1, 0.1, np.pi / 2)
position = [obst.pose[0], obst.pose[1]]
else: # Upper and lower plates 3 and 4.
shape = omg.Plate(shape2d=omg.Rectangle(
obst.shape[0], obst.shape[1]),
height=obst.shape[2],
orientation=[0., np.pi / 2, 0.])
position = [obst.pose[0], 0., obst.pose[2]]
elif obst.obst_type.data == "plate":
shape = omg.Plate(shape2d=omg.Rectangle(
obst.shape[0], obst.shape[1]),
height=obst.shape[2],
orientation=[0., np.pi / 2, obst.direction])
position = [obst.pose[0], obst.pose[1], obst.pose[2]]
else:
print highlight_yellow(' Warning: invalid obstacle type ')
self._obstacles.append(
omg.Obstacle({'position': position}, shape=shape))
# Dynamic obstacles.
for obst in data.dyn_obstacles:
shape = omg.Circle(obst.shape[0])
position = [obst.pose[0], obst.pose[1]]
self._obstacles.append(
omg.Obstacle({'position': position}, shape=shape))
# Create the environment as room with obstacles.
environment = omg.Environment(room=room)
environment.add_obstacle(self._obstacles)
# Create problem.
problem = omg.Point2point(self._vehicle, environment, freeT=False)
problem.set_options({
'solver_options': {
'ipopt': {
'ipopt.linear_solver': 'ma57',
'ipopt.max_iter': 1000,
'ipopt.print_level': 0,
'ipopt.tol': 1e-4,
'ipopt.warm_start_init_point': 'yes',
'ipopt.warm_start_bound_push': 1e-8,
'ipopt.warm_start_mult_bound_push': 1e-8,
'ipopt.mu_init': 1e-5,
'ipopt.hessian_approximation': 'limited-memory'
}
}
})
if self.n_dyn_obst:
problem.set_options({
'hard_term_con': False,
'horizon_time': self.horizon_time,
'verbose': 1.
})
else:
problem.set_options({
'hard_term_con': True,
'horizon_time': self.horizon_time,
'verbose': 1.
})
problem.init()
# problem.fullstop = True
self._deployer = omg.Deployer(problem, self._sample_time,
self.omg_update_time)
self._deployer.reset()
mp_configured = True
print green('---- Motionplanner running ----')
return ConfigMotionplannerResponse(mp_configured)