def _get_done(self):
names = ['collision/any', 'gps/latlong', 'joy']
obs = AttrDict.from_dict(self._jackal_subscriber.get(names=names))
is_collision = obs.collision.any
is_close_to_goal = np.linalg.norm(
latlong_to_utm(self._goal_latlong) -
latlong_to_utm(obs.gps.latlong)) < 2.0
return is_collision or is_close_to_goal
def _get_observation(self):
obs_names = set(self.spec.observation_names)
obs_names.add('gps/utm')
obs = AttrDict.from_dict(self._jackal_subscriber.get(names=obs_names))
if 'images/rgb_left' in obs.get_leaf_keys():
obs.images.rgb_left = self.spec.process_image(
'images/rgb_left', obs.images.rgb_left)
if 'images/rgb_right' in obs.get_leaf_keys():
obs.images.rgb_right = self.spec.process_image(
'images/rgb_right', obs.images.rgb_right)
obs.modify_recursive(lambda v: np.asarray(v))
return obs
def _get_goal(self, obs):
goal_utm = latlong_to_utm(self._goal_latlong)
goal_utm -= obs.gps.utm
goal_utm = np.append(goal_utm, 0.)
cost_weights = rospy.get_param(
'/cost_weights', {
'collision': 1.0,
'position': 0.0,
'action_magnitude': 0.001,
'action_smooth': 0.0,
'bumpy': 0.8,
'position_sigmoid_center': 0.6,
'position_sigmoid_scale': 100.
})
for key, weight in cost_weights.items():
cost_weights[key] = np.ravel(weight).astype(np.float32)
return AttrDict(position=goal_utm,
cost_weights=AttrDict.from_dict(cost_weights))