Position RMHC_SLAM::getNewPosition(Position & start_pos) { // Search for a new position if indicated Position likeliest_position = start_pos; if (this->randomizer) { // Use C to find likeliest position position_t start_pos_c; Position2position_t(start_pos, &start_pos_c); position_t c_likeliest_position = rmhc_position_search( start_pos_c, this->map->map, this->scan_for_distance->scan, *this->laser->laser, this->sigma_xy_mm, this->sigma_theta_degrees, this->max_search_iter, this->randomizer); // Convert back to C++ object likeliest_position = Position( c_likeliest_position.x_mm, c_likeliest_position.y_mm, c_likeliest_position.theta_degrees); } return likeliest_position; }
JNIEXPORT jobject JNICALL Java_edu_wlu_cs_levy_breezyslam_algorithms_RMHCSLAM_positionSearch (JNIEnv *env, jobject thisobject, jobject startpos_object, jobject map_object, jobject scan_object, jdouble sigma_xy_mm, jdouble sigma_theta_degrees, jint max_search_iter) { position_t startpos; startpos.x_mm = get_double_field(env, startpos_object, "x_mm"); startpos.y_mm = get_double_field(env, startpos_object, "y_mm"); startpos.theta_degrees = get_double_field(env, startpos_object, "theta_degrees"); void * random = ptr_from_obj(env, thisobject); position_t newpos = rmhc_position_search( startpos, cmap_from_jmap(env, map_object), cscan_from_jscan(env, scan_object), sigma_xy_mm, sigma_theta_degrees, max_search_iter, random); jclass cls = (*env)->FindClass(env, "edu/wlu/cs/levy/breezyslam/components/Position"); jmethodID constructor = (*env)->GetMethodID(env, cls, "<init>", "(DDD)V"); jobject newpos_object = (*env)->NewObject(env, cls, constructor, newpos.x_mm, newpos.y_mm, newpos.theta_degrees); return newpos_object; }
// Called internally, so minimal type-checking on arguments static PyObject * rmhcPositionSearch(PyObject *self, PyObject *args) { Position * py_start_pos = NULL; Map * py_map = NULL; Scan * py_scan = NULL; Laser * py_laser = NULL; double sigma_xy_mm = 0; double sigma_theta_degrees = 0; int max_search_iter = 0; Randomizer * py_randomizer = NULL; // Extract Python objects for map, scan, and position if (!PyArg_ParseTuple(args, "OOOOddiO", &py_start_pos, &py_map, &py_scan, &py_laser, &sigma_xy_mm, &sigma_theta_degrees, &max_search_iter, &py_randomizer)) { return null_on_raise_argument_exception("breezyslam.algorithms", "rmhcPositionSearch"); } // Convert Python objects to C structures position_t start_pos = pypos2cpos(py_start_pos); // Convert Python laser object to C struct laser_t laser = pylaser2claser(py_laser); position_t likeliest_position = rmhc_position_search( start_pos, &py_map->map, &py_scan->scan, laser, sigma_xy_mm, sigma_theta_degrees, max_search_iter, py_randomizer->randomizer); // Convert C position back to Python object PyObject * argList = Py_BuildValue("ddd", likeliest_position.x_mm, likeliest_position.y_mm, likeliest_position.theta_degrees); PyObject * py_likeliest_position = PyObject_CallObject((PyObject *) &pybreezyslam_PositionType, argList); Py_DECREF(argList); return py_likeliest_position; }