Ejemplo n.º 1
0
 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
Ejemplo n.º 2
0
 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