C++ (Cpp) rmhc_position_search Examples

Example #1

File: algorithms.cpp Project: awesomeleo/RPLidar-SLAMbot

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;
}

Example #2

File: jnibreezyslam_algorithms.c Project: 3660628/BreezySLAM

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;
} 

Example #3

File: pybreezyslam.c Project: awesomeleo/RPLidar-SLAMbot

// 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;
    
}