def set_navigation(self, req):
print "Setting new navigation..."
#ndims = self.slam_worker.vars.ndims
nparticles = self.slam_worker.vars.nparticles
if nparticles == 7:#2*ndims + 1:
#pose_angle = tf.transformations.euler_from_quaternion(
# self.vehicle.pose_orientation)
mean_state = np.array([[req.north, req.east, 0]])
sigma_states = self._make_sigma_states_(self.slam_worker,
mean_state, nparticles)
self.slam_worker.set_states(states=sigma_states)
else:
self.slam_worker.vehicle.states[:, 0] = req.north
self.slam_worker.vehicle.states[:, 1] = req.east
self.slam_worker.vehicle.states[:, 2] = self.vehicle.pose_position[2]
ret = SetNEResponse()
ret.success = True
return ret
def setNavigation(self, req):
print "Setting new navigation..."
#rospy.loginfo("%s: Set Navigation to: \n%s", self.name, req)
#self.slam_worker.states[:,0] = req.north
#self.slam_worker.states[:,1] = req.east
ndims = self.slam_worker.parameters.state_parameters["ndims"]
nparticles = self.slam_worker.parameters.state_parameters["nparticles"]
if nparticles == 2*ndims + 1:
#pose_angle = tf.transformations.euler_from_quaternion(
# self.vehicle.pose_orientation)
sc_process_noise = self.slam_worker.trans_matrices(np.zeros(3), 1.0)[1] + self.slam_worker.trans_matrices(np.zeros(3), 0.01)[1]
mean_state = np.array([[req.north, req.east, 0, 0, 0, 0]])
self.slam_worker.states = sigma_pts(mean_state, sc_process_noise, _alpha=UKF_ALPHA, _beta=UKF_BETA, _kappa=UKF_KAPPA)[0]
else:
self.slam_worker.states[:,0] = req.north
self.slam_worker.states[:,1] = req.east
self.slam_worker.states[:,2] = self.vehicle.pose_position[2]
ret = SetNEResponse()
ret.success = True
return ret
