int IMU::updateIMU(){
int sampleCount = 0;
int sampleRate = 0;
if (imu->IMURead()) {
RTIMU_DATA imuData = imu->getIMUData();
while (imu->IMURead()) {
imuData = imu->getIMUData();
}
sampleCount++;
roll.insert(to_degrees(imuData.fusionPose.x()) + rollComp);
pitch.insert(to_degrees(imuData.fusionPose.y()) + pitchComp);
yaw.insert(to_degrees(imuData.fusionPose.z()));
mX.insert(imuData.accel.x()+ mXComp);
mY.insert(imuData.accel.y() + mYComp);
rollRate.insert(imuData.gyro.x());
pitchRate.insert(imuData.gyro.y());
//rotation(1,imuData.fusionPose.x(),imuData.fusionPose.y(),imuData.fusionPose.z());
//printf("Test one piece: Roll = %f\n",to_degrees(imuData.fusionPose.data(0)));
//printf("Sample rate %d: %s\r", sampleRate, RTMath::displayDegrees("", imuData.fusionPose));
return 1;
}else{
return 0;
}
}
static void invert_dist_azimuth(double lon1, double lat1, double dist,
double azi, double * out_lon2,
double * out_lat2)
/*
Return the location loc2=(longitude, latitude) such that
compute_distance(loc1, loc2) == dist and compute_azimuth(loc1, loc2) = azi.
Or, in other words if we move along the great-circle from 'loc1' along the
azimuth 'azi' for a distance of 'dist' degrees then we will reach loc2.
Example:
invert_dist_azimuth(10, 0, 10.049369393181079, 95.740074136412659, ..)
--> 20, 0
*/
{
/* convert everything to radians */
lon1 = to_radians(lon1);
lat1 = to_radians(lat1);
dist = to_radians(dist);
azi = to_radians(azi);
double lat2 = asin(sin(lat1) * cos(dist) + cos(lat1) * sin(dist) * cos(azi));
double lon2 = lon1 + atan2(sin(azi) * sin(dist) * cos(lat1),
cos(dist) - sin(lat1) * sin(lat2));
*out_lon2 = to_degrees(lon2);
*out_lat2 = to_degrees(lat2);
}
void horizontal_to_equatorial(double az_deg, double el_deg, time_t lst_s, double lat_deg,
double* ra_hours, double* dec_deg)
{
double ha = 0.0;
double dec = 0.0;
double sa, ca, se, ce, sp, cp, x, y, z, r;
sincos(from_degrees(az_deg), &sa, &ca);
sincos(from_degrees(el_deg), &se, &ce);
sincos(from_degrees(lat_deg), &sp, &cp);
/* HA,Dec as x,y,z */
x = -ca * ce * sp + se * cp;
y = -sa * ce;
z = ca * ce * cp + se * sp;
/* To spherical */
r = sqrt(x * x + y * y);
ha = (r == 0.0) ? 0.0 : atan2(y, x);
dec = atan2(z, r);
*ra_hours = to_hours(from_seconds(lst_s) - ha);
*ra_hours = wrap_to(*ra_hours, 24.0);
*dec_deg = to_degrees(dec);
}
static double compute_distance(double lon1, double lat1, double lon2,
double lat2)
/*
Compute the great circle distance between two point on the earth's surface
in degrees. Both the input and the output values are in degrees.
Returns a number between 0 and 180
*/
{
lon1 = to_radians(lon1);
lat1 = to_radians(lat1);
lon2 = to_radians(lon2);
lat2 = to_radians(lat2);
double sin_lat1 = sin(lat1);
double cos_lat1 = cos(lat1);
double sin_lat2 = sin(lat2);
double cos_lat2 = cos(lat2);
double delta_lon = lon2 - lon1;
double cos_delta_lon = cos(delta_lon);
double sin_delta_lon = sin(delta_lon);
double angle = atan2(sqrt(square(cos_lat2 * sin_delta_lon) +
square(cos_lat1 * sin_lat2 -
sin_lat1 * cos_lat2 * cos_delta_lon)),
sin_lat1 * sin_lat2
+ cos_lat1 * cos_lat2 * cos_delta_lon);
return to_degrees(angle);
}
void AttitudeController::useCurrentYaw(const nav_msgs::OdometryConstPtr& latest_state)
{
double current_yaw, current_pitch, current_roll;
odom_msg_to_ypr(latest_state, current_yaw, current_pitch, current_roll);
setYawCmd(to_degrees(current_yaw));
}
int L_tan(lua_State * env)
{
float x = lua_tointeger(env, -1);
float result = to_degrees(tan(to_radians(x)));
lua_pushnumber(env, result);
return 1;
}
void MorganSCARASolution::cartesian_to_actuator(const float cartesian_mm[], float actuator_mm[] )
{
float SCARA_pos[2],
SCARA_C2,
SCARA_S2,
SCARA_K1,
SCARA_K2,
SCARA_theta,
SCARA_psi;
SCARA_pos[X_AXIS] = (cartesian_mm[X_AXIS] - this->morgan_offset_x) * this->morgan_scaling_x; //Translate cartesian to tower centric SCARA X Y AND apply scaling factor from this offset.
SCARA_pos[Y_AXIS] = (cartesian_mm[Y_AXIS] * this->morgan_scaling_y - this->morgan_offset_y); // morgan_offset not to be confused with home offset. This makes the SCARA math work.
// Y has to be scaled before subtracting offset to ensure position on bed.
if (this->arm1_length == this->arm2_length)
SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-2.0f*SQ(this->arm1_length)) / (2.0f * SQ(this->arm1_length));
else
SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-SQ(this->arm1_length)-SQ(this->arm2_length)) / (2.0f * SQ(this->arm1_length));
// SCARA position is undefined if abs(SCARA_C2) >=1
// In reality abs(SCARA_C2) >0.95 can be problematic.
if (SCARA_C2 > this->morgan_undefined_max)
SCARA_C2 = this->morgan_undefined_max;
else if (SCARA_C2 < -this->morgan_undefined_min)
SCARA_C2 = -this->morgan_undefined_min;
SCARA_S2 = sqrtf(1.0f-SQ(SCARA_C2));
SCARA_K1 = this->arm1_length+this->arm2_length*SCARA_C2;
SCARA_K2 = this->arm2_length*SCARA_S2;
SCARA_theta = (atan2f(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2f(SCARA_K1, SCARA_K2))*-1.0f; // Morgan Thomas turns Theta in oposite direction
SCARA_psi = atan2f(SCARA_S2,SCARA_C2);
actuator_mm[ALPHA_STEPPER] = to_degrees(SCARA_theta); // Multiply by 180/Pi - theta is support arm angle
actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_theta + SCARA_psi); // Morgan kinematics (dual arm)
//actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_psi); // real scara
actuator_mm[GAMMA_STEPPER] = cartesian_mm[Z_AXIS]; // No inverse kinematics on Z - Position to add bed offset?
}
void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float actuator_mm[] )
{
float SCARA_pos[2],
SCARA_C2,
SCARA_S2,
SCARA_K1,
SCARA_K2,
SCARA_theta,
SCARA_psi;
SCARA_pos[X_AXIS] = cartesian_mm[X_AXIS] - this->morgan_offset_x; //Translate cartesian to tower centric SCARA X Y
SCARA_pos[Y_AXIS] = cartesian_mm[Y_AXIS] - this->morgan_offset_y; // morgan_offset not to be confused with home offset. Makes the SCARA math work.
if (this->arm1_length == this->arm2_length)
SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-2.0f*SQ(this->arm1_length)) / (2.0f * SQ(this->arm1_length));
else
SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-SQ(this->arm1_length)-SQ(this->arm2_length)) / (2.0f * SQ(this->arm1_length));
// SCARA position is undefined if abs(SCARA_C2) >=1
// In reality abs(SCARA_C2) >0.95 is problematic.
if (SCARA_C2 > 0.95f)
SCARA_C2 = 0.95f;
else if (SCARA_C2 < -0.95f)
SCARA_C2 = -0.95f;
SCARA_S2 = sqrtf(1.0f-SQ(SCARA_C2));
SCARA_K1 = this->arm1_length+this->arm2_length*SCARA_C2;
SCARA_K2 = this->arm2_length*SCARA_S2;
SCARA_theta = (atan2f(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2f(SCARA_K1, SCARA_K2))*-1.0f; // Morgan Thomas turns Theta in oposite direction
SCARA_psi = atan2f(SCARA_S2,SCARA_C2);
actuator_mm[ALPHA_STEPPER] = to_degrees(SCARA_theta); // Multiply by 180/Pi - theta is support arm angle
actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_theta + SCARA_psi); // Morgan kinematics (dual arm)
//actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_psi); // real scara
actuator_mm[GAMMA_STEPPER] = cartesian_mm[Z_AXIS]; // No inverse kinematics on Z - Position to add bed offset?
}
void wrl::constraint::calc_rot(double& a, double& d, const vec3& p,
const vec3& v) const
{
double t = (wvector(T) * zvector(T) - p * zvector(T)) / (v * zvector(T));
vec3 q = p + v * t - wvector(T);
double aa = to_degrees(atan2(q * yvector(T), q * xvector(T)));
double dd = length(q);
a = snap(aa, double(grid_a));
d = snap(dd, (grid_d));
}
// Sets g-code parser position in mm. Input in steps. Called by the system abort and hard
// limit pull-off routines.
/// 8c1
void gc_set_current_position(int32_t x, int32_t y, int32_t z, int32_t t)
{
gc.position[X_AXIS] = x/settings.steps_per_mm[X_AXIS];
gc.position[Y_AXIS] = y/settings.steps_per_mm[Y_AXIS];
gc.position[Z_AXIS] = z/settings.steps_per_mm[Z_AXIS];
/// 8c1
#if (AXIS_T_TYPE == ROTARY)
gc.position[T_AXIS] = t/to_degrees(settings.steps_per_mm[T_AXIS]); /// steps_per_degrees
#elif (AXIS_T_TYPE == LINEAR)
gc.position[T_AXIS] = t/settings.steps_per_mm[T_AXIS];
#endif
}
void equatorial_to_horizontal(double ra_hours, double dec_deg, time_t lst_s, double lat_deg,
double* az_deg, double* el_deg)
{
double ha = from_seconds(lst_s) - from_hours(ra_hours);
double sh, ch, sd, cd, sp, cp, x, y, z, r, a;
sincos(ha, &sh, &ch);
sincos(from_degrees(dec_deg), &sd, &cd);
sincos(from_degrees(lat_deg), &sp, &cp);
/* Az,El as x,y,z */
x = -ch * cd * sp + sd * cp;
y = -sh * cd;
z = ch * cd * cp + sd * sp;
/* To spherical */
r = sqrt(x * x + y * y);
a = (fabs(r) < DBL_MIN) ? 0.0 : atan2(y, x);
*az_deg = to_degrees((a < 0.0) ? a + (2.0 * M_PI) : a);
*el_deg = to_degrees(atan2(z, r));
}
float8 colors_delta_e_cmc(float8 l1, float8 a1, float8 b1, float8 l2, float8 a2, float8 b2, float8 pl, float8 pc)
{
float8 C_1,
C_2,
delta_L,
delta_C,
H_1,
F,
T,
S_L,
S_C,
S_H,
delta_H_sq,
delta_H;
C_1 = sqrt(pow(a1, 2) + pow(b1, 2));
C_2 = sqrt(pow(a2, 2) + pow(b2, 2));
delta_L = l1 - l2;
delta_C = C_1 - C_2;
H_1 = to_degrees(atan2(b1, a1));
H_1 += (H_1 < 0 ? 360 : 0);
F = sqrt(pow(C_1, 4) / (pow(C_1, 4) + 1900.0));
if (164 <= H_1 && H_1 <= 345) {
T = 0.56 + fabs(0.2 * cos(to_radians(H_1 + 168)));
} else {
T = 0.36 + fabs(0.4 * cos(to_radians(H_1 + 35)));
}
if (l1 < 16) {
S_L = 0.511;
} else {
S_L = (0.040975 * l1) / (1 + 0.01765 * l1);
}
S_C = ((0.0638 * C_1) / (1 + 0.0131 * C_1)) + 0.638;
S_H = S_C * (F * T + 1 - F);
delta_H_sq = -pow(delta_C, 2) + pow(a1 - a2, 2) + pow(b1 - b2, 2);
delta_H = sqrt((delta_H_sq > 0 ? delta_H_sq : 0));
return sqrt(pow(delta_L / (pl * S_L), 2) + pow(delta_C / (pc * S_C), 2) + pow(delta_H / S_H, 2));
}
static void draw_goal (RendererGoal *self, lcmtypes_goal_t * goal)
{
double carpos[3] = { 0, 0, 0 };
ctrans_local_pos (self->ctrans, carpos);
glPushMatrix();
glTranslated(goal->pos[0], goal->pos[1], carpos[2]);
glRotated(to_degrees(goal->theta), 0, 0, 1);
glEnable(GL_BLEND);
glBegin(GL_QUADS);
glVertex2d( - goal->size[0]/2, - goal->size[1]/2);
glVertex2d( - goal->size[0]/2, + goal->size[1]/2);
glVertex2d( + goal->size[0]/2, + goal->size[1]/2);
glVertex2d( + goal->size[0]/2, - goal->size[1]/2);
glEnd();
if (goal->use_theta) {
glBegin(GL_LINES);
glColor3f(.8, 1, .8);
glVertex2d( - goal->size[0]/2, 0);
glVertex2d( + goal->size[0]/2, 0);
glVertex2d( 0, goal->size[1]/2);
glVertex2d( + goal->size[0]/2, 0);
glVertex2d( 0, -goal->size[1]/2);
glVertex2d( + goal->size[0]/2, 0);
glEnd();
} else {
glBegin(GL_LINES);
glColor3f(.8, 1, .8);
glVertex2d( - goal->size[0]/2, - goal->size[1]/2);
glVertex2d( goal->size[0]/2, goal->size[1]/2);
glVertex2d( - goal->size[0]/2, goal->size[1]/2);
glVertex2d( goal->size[0]/2, - goal->size[1]/2);
glEnd();
}
glPopMatrix();
}
static double compute_azimuth(double lon1, double lat1, double lon2,
double lat2)
/*
Angle in degrees measured clockwise from north starting at
loc1 towards loc2. loc1 and loc2 are (longitude, latitude) in degrees.
See https://en.wikipedia.org/wiki/Great-circle_navigation.
However, we want north = 0 degrees,
east = 90 degrees,
south = 180 degrees, and
west = 270 degrees.
Return an angle in 0 to 360 degrees.
*/
{
lon1 = to_radians(lon1);
lat1 = to_radians(lat1);
lon2 = to_radians(lon2);
lat2 = to_radians(lat2);
double delta_lon = lon2 - lon1;
double y = sin(delta_lon);
double x = cos(lat1) * tan(lat2) - sin(lat1) * cos(delta_lon);
double azi = to_degrees(atan2(y, x));
/*
azi is now in range -180/180
the following trick brings it in the 0 - 360 range
*/
return fmod(azi + 360 , 360);
}
bool wrl::constraint::point(const vec3& p, const vec3& v, mat4& A)
{
if (to_degrees(acos(mouse_v * v)) > 1.0)
{
if (mode)
{
double a;
double d;
calc_rot(a, d, p, v);
if (fabs(a - mouse_a) > 0.0 || fabs(d - mouse_d) > 0.0)
{
A = translation( wvector(T))
* rotation( zvector(T), to_radians(a - mouse_a))
* translation(-wvector(T));
return true;
}
}
else
{
double x;
double y;
calc_pos(x, y, p, v);
if (fabs(x - mouse_x) > 0.0 || fabs(y - mouse_y) > 0.0)
{
A = translation(xvector(T) * (x - mouse_x)
+ yvector(T) * (y - mouse_y));
return true;
}
}
}
return false;
}
static inline void
bot_fasttrig_atan2_test()
{
double max_err = 0;
int64_t start = _timestamp_now();
for (int i = 0; i < 10000000; i++) {
double x = signed_randf();
double y = signed_randf();
double a = atan2(y,x);
double b = bot_fasttrig_atan2(y,x);
double err = fabs(a-b);
if (err > max_err) {
printf("max err: %15f deg (%15f %15f %15f %15f)\n", to_degrees(err), x, y, a, b);
max_err = err;
}
}
int64_t stop = _timestamp_now();
printf("dt: %15f\n", (stop-start)/1000000.0);
}
int
geocode_dem (projection_type_t projection_type, // What we are projection to.
project_parameters_t *pp, // Parameters we project to.
datum_type_t datum, // Datum we project to.
// Pixel size of output image, in output projection units
// (meters or possibly degrees, if we decide to support
// projecting to pseudoprojected form).
double pixel_size,
resample_method_t resample_method, // How to resample pixels.
const char *input_image, // Base name of input image.
const meta_parameters *imd, // Input DEM image metadata.
const char *output_image // Base name of output image.
)
{
int return_code; // Holds return codes from functions.
// Function to use to project or unproject between latlon and input
// or output coordinates.
projector_t project_input; // latlon => input image map projection
projector_t unproject_input; // input image_map_projection => latlon
projector_t project_output; // latlon => output image map projection
projector_t unproject_output; // output image map projection => latlon
// Like the above, but act on arrays.
array_projector_t array_project_input, array_unproject_input;
array_projector_t array_project_output, array_unproject_output;
// We only deal with reprojection map projected DEMs.
g_assert (imd->projection != NULL);
// FIXME: what to do with background value is something that still
// needs to be determined (probably in consultation with the guys
// working on terrain correction).
const float background_value = 0.0;
// Geocoding to pseudoprojected form presents issues, for example
// with the meaning of the pixel_size argument, which is taken as a
// distance in map projection coordinates for all other projections
// (deciding how to interpret it when projecting to pseudoprojected
// form is tough), and since there probably isn't much need, we
// don't allow it.
g_assert (projection_type != LAT_LONG_PSEUDO_PROJECTION);
// Get the functions we want to use for projecting and unprojecting.
set_projection_functions (imd->projection->type, &project_input,
&unproject_input, &array_project_input,
&array_unproject_input);
set_projection_functions (projection_type, &project_output,
&unproject_output, &array_project_output,
&array_unproject_output);
// Input image dimensions in pixels in x and y directions.
size_t ii_size_x = imd->general->sample_count;
size_t ii_size_y = imd->general->line_count;
// Convenience aliases.
meta_projection *ipb = imd->projection;
project_parameters_t *ipp = &imd->projection->param;
// First we march around the entire outside of the image and compute
// projection coordinates for every pixel, keeping track of the
// minimum and maximum projection coordinates in each dimension.
// This lets us determine the exact extent of the DEM in
// output projection coordinates.
asfPrintStatus ("Determining input image extent in projection coordinate "
"space... ");
double min_x = DBL_MAX;
double max_x = -DBL_MAX;
double min_y = DBL_MAX;
double max_y = -DBL_MAX;
// In going around the edge, we are just trying to determine the
// extent of the image in the horizontal, so we don't care about
// height yet.
{ // Scoping block.
// Number of pixels in the edge of the image.
size_t edge_point_count = 2 * ii_size_x + 2 * ii_size_y - 4;
double *lats = g_new0 (double, edge_point_count);
double *lons = g_new0 (double, edge_point_count);
size_t current_edge_point = 0;
size_t ii = 0, jj = 0;
for ( ; ii < ii_size_x - 1 ; ii++ ) {
return_code = get_pixel_lat_long (imd, unproject_input, ii, jj,
&(lats[current_edge_point]),
&(lons[current_edge_point]));
g_assert (return_code);
current_edge_point++;
}
for ( ; jj < ii_size_y - 1 ; jj++ ) {
return_code = get_pixel_lat_long (imd, unproject_input, ii, jj,
&(lats[current_edge_point]),
&(lons[current_edge_point]));
g_assert (return_code);
current_edge_point++;
}
for ( ; ii > 0 ; ii-- ) {
return_code = get_pixel_lat_long (imd, unproject_input, ii, jj,
&(lats[current_edge_point]),
&(lons[current_edge_point]));
g_assert (return_code);
current_edge_point++;
}
for ( ; jj > 0 ; jj-- ) {
return_code = get_pixel_lat_long (imd, unproject_input, ii, jj,
&(lats[current_edge_point]),
&(lons[current_edge_point]));
g_assert (return_code);
current_edge_point++;
}
g_assert (current_edge_point == edge_point_count);
// Pointers to arrays of projected coordinates to be filled in.
// The projection function will allocate this memory itself.
double *x = NULL, *y = NULL;
// Project all the edge pixels.
return_code = array_project_output (pp, lats, lons, NULL, &x, &y, NULL,
edge_point_count, datum);
g_assert (return_code == TRUE);
// Find the extents of the image in projection coordinates.
for ( ii = 0 ; ii < edge_point_count ; ii++ ) {
if ( x[ii] < min_x ) { min_x = x[ii]; }
if ( x[ii] > max_x ) { max_x = x[ii]; }
if ( y[ii] < min_y ) { min_y = y[ii]; }
if ( y[ii] > max_y ) { max_y = y[ii]; }
}
free (y);
free (x);
g_free (lons);
g_free (lats);
}
asfPrintStatus ("done.\n\n");
// Issue a warning when the chosen pixel size is smaller than the
// input pixel size. FIXME: this condition will really never fire
// for pseudoprojected image, since the pixels size of the input is
// tiny (degrees per pixel) and the pixel_size has already been
// computed in asf_geocode function itself as an arc length on the
// ground.
if ( GSL_MIN(imd->general->x_pixel_size,
imd->general->y_pixel_size) > pixel_size ) {
asfPrintWarning
("Requested pixel size %f is smaller then the input image resolution "
"(%le meters).\n", pixel_size,
GSL_MIN (imd->general->x_pixel_size, imd->general->y_pixel_size));
}
// The pixel size requested by the user better not oversample by the
// factor of 2. Specifying --force will skip this check. FIXME:
// same essential problem as the above condition, but in this case
// it always goes off.
// if (!force_flag && GSL_MIN(imd->general->x_pixel_size,
// imd->general->y_pixel_size) > (2*pixel_size) ) {
// report_func
// ("Requested pixel size %f is smaller then the minimum implied by half \n"
// "the input image resolution (%le meters), this is not supported.\n",
// pixel_size, GSL_MIN (imd->general->x_pixel_size,
// imd->general->y_pixel_size));
// }
asfPrintStatus ("Opening input DEM image... ");
char *input_data_file = (char *) MALLOC(sizeof(char)*(strlen(input_image)+5));
sprintf(input_data_file, "%s.img", input_image);
FloatImage *iim
= float_image_new_from_file (ii_size_x, ii_size_y, input_data_file, 0,
FLOAT_IMAGE_BYTE_ORDER_BIG_ENDIAN);
FREE(input_data_file);
asfPrintStatus ("done.\n\n");
// Maximum pixel indicies in output image.
size_t oix_max = ceil ((max_x - min_x) / pixel_size);
size_t oiy_max = ceil ((max_y - min_y) / pixel_size);
// Output image dimensions.
size_t oi_size_x = oix_max + 1;
size_t oi_size_y = oiy_max + 1;
// Output image.
FloatImage *oim = float_image_new (oi_size_x, oi_size_y);
// Translate the command line notion of the resampling method into
// the lingo known by the float_image class. The compiler is
// reassured with a default.
float_image_sample_method_t float_image_sample_method
= FLOAT_IMAGE_SAMPLE_METHOD_BILINEAR;
switch ( resample_method ) {
case RESAMPLE_NEAREST_NEIGHBOR:
float_image_sample_method = FLOAT_IMAGE_SAMPLE_METHOD_NEAREST_NEIGHBOR;
break;
case RESAMPLE_BILINEAR:
float_image_sample_method = FLOAT_IMAGE_SAMPLE_METHOD_BILINEAR;
break;
case RESAMPLE_BICUBIC:
float_image_sample_method = FLOAT_IMAGE_SAMPLE_METHOD_BICUBIC;
break;
default:
g_assert_not_reached ();
}
// We need to find the z coordinates in the output projection of all
// the pixels in the input DEM. We store these values in their own
// FloatImage instance.
//FloatImage *x_coords = float_image_new (ii_size_x, ii_size_y);
//FloatImage *y_coords = float_image_new (ii_size_x, ii_size_y);
FloatImage *z_coords = float_image_new (ii_size_x, ii_size_y);
// We transform the points using the array transformation function
// for efficiency, but we don't want to do them all at once, since
// that would require huge gobs of memory.
const size_t max_transform_chunk_pixels = 5000000;
size_t rows_per_chunk = max_transform_chunk_pixels / ii_size_x;
size_t chunk_pixels = rows_per_chunk * ii_size_x;
double *chunk_x = g_new0 (double, chunk_pixels);
double *chunk_y = g_new0 (double, chunk_pixels);
double *chunk_z = g_new0 (double, chunk_pixels);
double *lat = g_new0 (double, chunk_pixels);
double *lon = g_new0 (double, chunk_pixels);
double *height = g_new0 (double, chunk_pixels);
asfPrintStatus ("Determining Z coordinates of input pixels in output "
"projection space... ");
// Transform all the chunks, storing results in the z coordinate image.
size_t ii, jj, kk; // Index variables.
for ( ii = 0 ; ii < ii_size_y ; ) {
size_t rows_remaining = ii_size_y - ii;
size_t rows_to_load
= rows_per_chunk < rows_remaining ? rows_per_chunk : rows_remaining;
for ( jj = 0 ; jj < rows_to_load ; jj++ ) {
size_t current_image_row = ii + jj;
for ( kk = 0 ; kk < ii_size_x ; kk++ ) {
size_t current_chunk_pixel = jj * ii_size_x + kk;
chunk_x[current_chunk_pixel] = ipb->startX + kk * ipb->perX;
chunk_y[current_chunk_pixel]
= ipb->startY + current_image_row * ipb->perY;
if ( imd->projection->type == LAT_LONG_PSEUDO_PROJECTION ) {
chunk_x[current_chunk_pixel] *= D2R;
chunk_y[current_chunk_pixel] *= D2R;
}
chunk_z[current_chunk_pixel]
= float_image_get_pixel (iim, kk, current_image_row);
}
}
long current_chunk_pixels = rows_to_load * ii_size_x;
array_unproject_input (ipp, chunk_x, chunk_y, chunk_z, &lat, &lon,
&height, current_chunk_pixels, ipb->datum);
array_project_output (pp, lat, lon, height, &chunk_x, &chunk_y, &chunk_z,
current_chunk_pixels, datum);
for ( jj = 0 ; jj < rows_to_load ; jj++ ) {
size_t current_image_row = ii + jj;
for ( kk = 0 ; kk < ii_size_x ; kk++ ) {
size_t current_chunk_pixel = jj * ii_size_x + kk;
// Current pixel x, y, z coordinates.
//float cp_x = (float) chunk_x[current_chunk_pixel];
//float cp_y = (float) chunk_y[current_chunk_pixel];
float cp_z = (float) chunk_z[current_chunk_pixel];
//float_image_set_pixel (x_coords, kk, current_image_row, cp_x);
//float_image_set_pixel (y_coords, kk, current_image_row, cp_y);
float_image_set_pixel (z_coords, kk, current_image_row, cp_z);
}
}
ii += rows_to_load;
}
asfPrintStatus ("done.\n\n");
#ifdef DEBUG_GEOCODE_DEM_Z_COORDS_IMAGE_AS_JPEG
// Take a look at the z_coordinate image (for debugging).
float_image_export_as_jpeg_with_mask_interval (z_coords, "z_coords.jpg",
GSL_MAX (z_coords->size_x,
z_coords->size_y),
-FLT_MAX, -100);
#endif
g_free (chunk_x);
g_free (chunk_y);
g_free (chunk_z);
g_free (lat);
g_free (lon);
g_free (height);
// Now we want to determine the pixel coordinates in the input which
// correspond to each of the output pixels. We can then sample the
// new height value already computed for that input pixel to
// determine the pixel value to use as output.
// We want to proceed in chunks as we did when going in the other
// direction.
rows_per_chunk = max_transform_chunk_pixels / oi_size_x;
chunk_pixels = rows_per_chunk * oi_size_x;
chunk_x = g_new0 (double, chunk_pixels);
chunk_y = g_new0 (double, chunk_pixels);
// We don't have height information in this direction, nor do we care.
chunk_z = NULL;
lat = g_new0 (double, chunk_pixels);
lon = g_new0 (double, chunk_pixels);
// We don't have height information in this direction, nor do we care.
height = NULL;
asfPrintStatus ("Sampling Z coordinates to form pixels in output projection "
"space... ");
// Transform all the chunks, using the results to form the output image.
for ( ii = 0 ; ii < oi_size_y ; ) {
size_t rows_remaining = oi_size_y - ii;
size_t rows_to_load
= rows_per_chunk < rows_remaining ? rows_per_chunk : rows_remaining;
for ( jj = 0 ; jj < rows_to_load ; jj++ ) {
size_t current_image_row = ii + jj;
for ( kk = 0 ; kk < oi_size_x ; kk++ ) {
size_t current_chunk_pixel = jj * oi_size_x + kk;
chunk_x[current_chunk_pixel] = min_x + kk * pixel_size;
chunk_y[current_chunk_pixel] = max_y - current_image_row * pixel_size;
}
}
long current_chunk_pixels = rows_to_load * oi_size_x;
array_unproject_output (pp, chunk_x, chunk_y, NULL, &lat, &lon, NULL,
current_chunk_pixels, datum);
array_project_input (ipp, lat, lon, NULL, &chunk_x, &chunk_y, NULL,
current_chunk_pixels, ipb->datum);
if ( imd->projection->type == LAT_LONG_PSEUDO_PROJECTION ) {
ssize_t ll; // For (semi)clarity we don't reuse index variable :)
for ( ll = 0 ; ll < current_chunk_pixels ; ll++ ) {
chunk_x[ll] *= R2D;
chunk_y[ll] *= R2D;
}
}
for ( jj = 0 ; jj < rows_to_load ; jj++ ) {
size_t current_image_row = ii + jj;
for ( kk = 0 ; kk < oi_size_x ; kk++ ) {
size_t current_chunk_pixel = jj * oi_size_x + kk;
// Compute pixel coordinates in input image.
ssize_t in_x
= (chunk_x[current_chunk_pixel] - ipb->startX) / ipb->perX;
ssize_t in_y
= (chunk_y[current_chunk_pixel] - ipb->startY) / ipb->perY;
if ( in_image (z_coords, in_x, in_y) ) {
// FIXME: something needs to be done somewhere about
// propogating no data values.
float_image_set_pixel (oim, kk, current_image_row,
float_image_sample (z_coords, in_x, in_y,
resample_method));
}
else {
float_image_set_pixel (oim, kk, current_image_row, background_value);
}
}
}
ii += rows_to_load;
}
asfPrintStatus ("done.\n\n");
g_free (chunk_x);
g_free (chunk_y);
g_free (lat);
g_free (lon);
#ifdef DEBUG_GEOCODE_DEM_OUTPUT_IMAGE_AS_JPEG
// Take a look at the output image (for debugging).
float_image_export_as_jpeg_with_mask_interval (oim, "oim.jpg",
GSL_MAX (oim->size_x,
oim->size_y),
-FLT_MAX, -100);
#endif
// Store the output image.
asfPrintStatus ("Storing output image... ");
char *output_data_file =
(char *) MALLOC(sizeof(char)*(strlen(output_image)+5));
sprintf(output_data_file, "%s.img", output_image);
return_code = float_image_store (oim, output_data_file,
FLOAT_IMAGE_BYTE_ORDER_BIG_ENDIAN);
g_assert (return_code == 0);
asfPrintStatus ("done.\n\n");
// Now we need some metadata for the output image. We will just
// start with the metadata from the input image and add the
// geocoding parameters.
char *input_meta_file = (char *) MALLOC(sizeof(char)*(strlen(input_image)+6));
sprintf(input_meta_file, "%s.meta", input_image);
char *output_meta_file =
(char *) MALLOC(sizeof(char)*(strlen(output_image)+6));
sprintf(output_meta_file, "%s.meta", output_image);
meta_parameters *omd = meta_read (input_meta_file);
// Adjust the metadata to correspond to the output image instead of
// the input image.
omd->general->x_pixel_size = pixel_size;
omd->general->y_pixel_size = pixel_size;
omd->general->line_count = oi_size_y;
omd->general->sample_count = oi_size_x;
// SAR block is not really appropriate for map projected images, but
// since it ended up with this value that can signify map
// projectedness in it somehow, we fill it in for safety.
omd->sar->image_type = 'P';
// Note that we have already verified that the input image is
// projected, and since we initialize the output metadata from there
// we know we will have a projection block.
omd->projection->type = projection_type;
omd->projection->startX = min_x;
omd->projection->startY = max_y;
omd->projection->perX = pixel_size;
omd->projection->perY = -pixel_size;
strcpy (omd->projection->units, "meters");
// Set the spheroid axes lengths as appropriate for the output datum.
spheroid_axes_lengths (datum_spheroid (datum), &(omd->projection->re_major),
&(omd->projection->re_minor));
// What the heck, might as well set the ones in the general block as
// well.
spheroid_axes_lengths (datum_spheroid (datum), &(omd->general->re_major),
&(omd->general->re_minor));
// Latitude and longitude at center of the output image. We will
// set these relative to the spheroid underlying the datum in use
// for the projected image. Yeah, that seems appropriate.
double lat_0, lon_0;
double center_x = omd->projection->startX + (omd->projection->perX
* omd->general->line_count / 2);
double center_y = (omd->projection->startY
+ (omd->projection->perY
* omd->general->sample_count / 2));
unproject_output (pp, center_x, center_y, ASF_PROJ_NO_HEIGHT, &lat_0, &lon_0,
NULL, datum);
omd->general->center_latitude = R2D * lat_0;
omd->general->center_longitude = R2D * lon_0;
// FIXME: We are ignoring the meta_location fields for now since I'm
// not sure whether they are supposed to refer to the corner pixels
// or the corners of the data itself.
if ( lat_0 > 0.0 ) {
omd->projection->hem = 'N';
}
else {
omd->projection->hem = 'S';
}
// Convert the projection parameter values back into degrees.
to_degrees (projection_type, pp);
omd->projection->param = *pp;
meta_write (omd, output_meta_file);
float_image_free (oim);
FREE(output_data_file);
meta_free (omd);
FREE(input_meta_file);
FREE(output_meta_file);
return 0;
}
bool yee_compare(CompareArgs &args)
{
if ((args.image_a_->get_width() != args.image_b_->get_width()) or
(args.image_a_->get_height() != args.image_b_->get_height()))
{
args.error_string_ = "Image dimensions do not match\n";
return false;
}
const auto w = args.image_a_->get_width();
const auto h = args.image_a_->get_height();
const auto dim = w * h;
auto identical = true;
for (auto i = 0u; i < dim; i++)
{
if (args.image_a_->get(i) != args.image_b_->get(i))
{
identical = false;
break;
}
}
if (identical)
{
args.error_string_ = "Images are binary identical\n";
return true;
}
// Assuming colorspaces are in Adobe RGB (1998) convert to XYZ.
std::vector<float> a_lum(dim);
std::vector<float> b_lum(dim);
std::vector<float> a_a(dim);
std::vector<float> b_a(dim);
std::vector<float> a_b(dim);
std::vector<float> b_b(dim);
if (args.verbose_)
{
std::cout << "Converting RGB to XYZ\n";
}
const auto gamma = args.gamma_;
const auto luminance = args.luminance_;
#pragma omp parallel for shared(args, a_lum, b_lum, a_a, a_b, b_a, b_b)
for (auto y = 0; y < static_cast<ptrdiff_t>(h); y++)
{
for (auto x = 0u; x < w; x++)
{
const auto i = x + y * w;
const auto a_color_r = powf(args.image_a_->get_red(i) / 255.0f,
gamma);
const auto a_color_g = powf(args.image_a_->get_green(i) / 255.0f,
gamma);
const auto a_color_b = powf(args.image_a_->get_blue(i) / 255.0f,
gamma);
float a_x;
float a_y;
float a_z;
adobe_rgb_to_xyz(a_color_r, a_color_g, a_color_b, a_x, a_y, a_z);
float l;
xyz_to_lab(a_x, a_y, a_z, l, a_a[i], a_b[i]);
const auto b_color_r = powf(args.image_b_->get_red(i) / 255.0f,
gamma);
const auto b_color_g = powf(args.image_b_->get_green(i) / 255.0f,
gamma);
const auto b_color_b = powf(args.image_b_->get_blue(i) / 255.0f,
gamma);
float b_x;
float b_y;
float b_z;
adobe_rgb_to_xyz(b_color_r, b_color_g, b_color_b, b_x, b_y, b_z);
xyz_to_lab(b_x, b_y, b_z, l, b_a[i], b_b[i]);
a_lum[i] = a_y * luminance;
b_lum[i] = b_y * luminance;
}
}
if (args.verbose_)
{
std::cout << "Constructing Laplacian Pyramids\n";
}
const LPyramid la(a_lum, w, h);
const LPyramid lb(b_lum, w, h);
const auto num_one_degree_pixels =
to_degrees(2 *
std::tan(args.field_of_view_ * to_radians(.5f)));
const auto pixels_per_degree = w / num_one_degree_pixels;
if (args.verbose_)
{
std::cout << "Performing test\n";
}
const auto adaptation_level = adaptation(num_one_degree_pixels);
float cpd[MAX_PYR_LEVELS];
cpd[0] = 0.5f * pixels_per_degree;
for (auto i = 1u; i < MAX_PYR_LEVELS; i++)
{
cpd[i] = 0.5f * cpd[i - 1];
}
const auto csf_max = csf(3.248f, 100.0f);
static_assert(MAX_PYR_LEVELS > 2,
"MAX_PYR_LEVELS must be greater than 2");
float f_freq[MAX_PYR_LEVELS - 2];
for (auto i = 0u; i < MAX_PYR_LEVELS - 2; i++)
{
f_freq[i] = csf_max / csf(cpd[i], 100.0f);
}
auto pixels_failed = 0u;
auto error_sum = 0.;
#pragma omp parallel for reduction(+ : pixels_failed, error_sum) \
shared(args, a_a, a_b, b_a, b_b, cpd, f_freq)
for (auto y = 0; y < static_cast<ptrdiff_t>(h); y++)
{
for (auto x = 0u; x < w; x++)
{
const auto index = y * w + x;
const auto adapt = std::max((la.get_value(x, y, adaptation_level) +
lb.get_value(x, y, adaptation_level)) * 0.5f,
1e-5f);
auto sum_contrast = 0.f;
auto factor = 0.f;
for (auto i = 0u; i < MAX_PYR_LEVELS - 2; i++)
{
const auto n1 =
fabsf(la.get_value(x, y, i) - la.get_value(x, y, i + 1));
const auto n2 =
fabsf(lb.get_value(x, y, i) - lb.get_value(x, y, i + 1));
const auto numerator = std::max(n1, n2);
const auto d1 = fabsf(la.get_value(x, y, i + 2));
const auto d2 = fabsf(lb.get_value(x, y, i + 2));
const auto denominator = std::max(std::max(d1, d2), 1e-5f);
const auto contrast = numerator / denominator;
const auto f_mask = mask(contrast * csf(cpd[i], adapt));
factor += contrast * f_freq[i] * f_mask;
sum_contrast += contrast;
}
sum_contrast = std::max(sum_contrast, 1e-5f);
factor /= sum_contrast;
factor = std::min(std::max(factor, 1.f), 10.f);
const auto delta =
fabsf(la.get_value(x, y, 0) - lb.get_value(x, y, 0));
error_sum += delta;
auto pass = true;
// pure luminance test
if (delta > factor * tvi(adapt))
{
pass = false;
}
if (not args.luminance_only_)
{
// CIE delta E test with modifications
auto color_scale = args.color_factor_;
// ramp down the color test in scotopic regions
if (adapt < 10.0f)
{
// Don't do color test at all.
color_scale = 0.0;
}
const auto da = a_a[index] - b_a[index];
const auto db = a_b[index] - b_b[index];
const auto delta_e = (da * da + db * db) * color_scale;
error_sum += delta_e;
if (delta_e > factor)
{
pass = false;
}
}
if (not pass)
{
pixels_failed++;
if (args.image_difference_)
{
args.image_difference_->set(255, 0, 0, 255, index);
}
}
else
{
if (args.image_difference_)
{
args.image_difference_->set(0, 0, 0, 255, index);
}
}
}
}
const auto error_sum_buff =
std::to_string(error_sum) + " error sum\n";
const auto different =
std::to_string(pixels_failed) + " pixels are different\n";
// Always output image difference if requested.
if (args.image_difference_)
{
args.image_difference_->write_to_file(args.image_difference_->get_name());
args.error_string_ += "Wrote difference image to ";
args.error_string_ += args.image_difference_->get_name();
args.error_string_ += "\n";
}
if (pixels_failed < args.threshold_pixels_)
{
args.error_string_ = "Images are perceptually indistinguishable\n";
args.error_string_ += different;
return true;
}
args.error_string_ = "Images are visibly different\n";
args.error_string_ += different;
if (args.sum_errors_)
{
args.error_string_ += error_sum_buff;
}
return false;
}
static void
car_draw (Viewer *viewer, Renderer *super)
{
RendererCar *self = (RendererCar*) super->user;
lcmtypes_pose_t pose;
if (ctrans_local_pose (self->ctrans, &pose) < 0)
return;
GtkuParamWidget *pw = self->pw;
int bling = self->chassis_model ?
gtku_param_widget_get_bool (pw, PARAM_NAME_BLING) : 0;
int wheels = self->wheel_model ?
gtku_param_widget_get_bool (pw, PARAM_NAME_WHEELS) : 0;
if ((bling || wheels) && !self->display_lists_ready) {
load_bling (self);
}
glColor4f(0,1,0,0.75);
glLineWidth (10);
glBegin(GL_LINE_STRIP);
glVertex3dv (self->last_pose.pos);
for (unsigned int i = 0;
i < MIN (gu_ptr_circular_size(self->path), self->max_draw_poses);
i++) {
glVertex3dv(gu_ptr_circular_index(self->path, i));
}
glEnd();
glPushMatrix();
// compute the rotation matrix to orient the vehicle in world
// coordinates
double body_quat_m[16];
rot_quat_pos_to_matrix(pose.orientation, pose.pos, body_quat_m);
// opengl expects column-major matrices
double body_quat_m_opengl[16];
matrix_transpose_4x4d (body_quat_m, body_quat_m_opengl);
// rotate and translate the vehicle
glMultMatrixd (body_quat_m_opengl);
glEnable (GL_DEPTH_TEST);
if (bling && self->display_lists_ready && self->chassis_dl)
draw_chassis_model (self);
else
draw_footprint (self);
if (wheels && self->display_lists_ready && self->wheel_dl)
draw_wheels (self);
glPopMatrix();
if (self->display_detail) {
char buf[256];
switch (self->display_detail)
{
case DETAIL_SPEED:
sprintf(buf, "%.2f m/s",
sqrt(sq(pose.vel[0]) + sq(pose.vel[1]) + sq(pose.vel[2])));
break;
case DETAIL_RPY:
{
double rpy[3];
rot_quat_to_roll_pitch_yaw(pose.orientation, rpy);
sprintf(buf, "r: %6.2f\np: %6.2f\ny: %6.2f", to_degrees(rpy[0]), to_degrees(rpy[1]), to_degrees(rpy[2]));
break;
}
case DETAIL_GPS:
{
double lle[3], q[4];
ctrans_gps_pose(self->ctrans, lle, q);
double rpy[3];
rot_quat_to_roll_pitch_yaw(q, rpy);
sprintf(buf, "%15.7f %15.7f\nelev: %10.3f\nhead: %6.2f\n", lle[0], lle[1], lle[2], to_degrees(rpy[2]));
break;
}
}
glColor3f(1,1,1);
glutil_draw_text(pose.pos, GLUT_BITMAP_HELVETICA_12, buf,
GLUTIL_DRAW_TEXT_DROP_SHADOW);
}
}
// Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram
// and the actual location of the CNC machine. Users may change the following function to their
// specific needs, but the desired real-time data report must be as short as possible. This is
// requires as it minimizes the computational overhead and allows grbl to keep running smoothly,
// especially during g-code programs with fast, short line segments and high frequency reports (5-20Hz).
void report_realtime_status() {
// **Under construction** Bare-bones status report. Provides real-time machine position relative to
// the system power on location (0,0,0) and work coordinate position (G54 and G92 applied). Eventually
// to be added are distance to go on block, processed block id, and feed rate. Also a settings bitmask
// for a user to select the desired real-time data.
uint8_t i;
/// 8c0
int32_t current_position[N_AXIS]; // Copy current state of the system position variable
memcpy (current_position, sys.position, sizeof (sys.position) );
float print_position[N_AXIS];
// Report current machine state
switch (sys.state) {
case STATE_IDLE:
printPgmString (PSTR ("<Idle") );
break;
// case STATE_INIT: printPgmString(PSTR("[Init")); break; // Never observed
case STATE_QUEUED:
printPgmString (PSTR ("<Queue") );
break;
case STATE_CYCLE:
printPgmString (PSTR ("<Run") );
break;
case STATE_HOLD:
printPgmString (PSTR ("<Hold") );
break;
case STATE_HOMING:
printPgmString (PSTR ("<Home") );
break;
case STATE_ALARM:
printPgmString (PSTR ("<Alarm") );
break;
case STATE_CHECK_MODE:
printPgmString (PSTR ("<Check") );
break;
}
// Report machine position
/// 8c1
// printPgmString (PSTR (",MPos:[") );
printPgmString (PSTR (",MPos:") );
/// 8c0
for (i = 0; i < N_AXIS; i++) {
/// 8c1
if (i == T_AXIS)
#if (AXIS_T_TYPE == ROTARY)
print_position[i] = current_position[i] / to_degrees(settings.steps_per_mm[i]);
#elif (AXIS_T_TYPE == LINEAR)
print_position[i] = current_position[i] / settings.steps_per_mm[i];
#endif
else
print_position[i] = current_position[i] / settings.steps_per_mm[i];
if (bit_istrue (settings.flags, BITFLAG_REPORT_INCHES) ) {
/// 8c1
if (i == T_AXIS) {
#if (AXIS_T_TYPE == LINEAR)
print_position[i] *= INCH_PER_MM;
#endif
}
else
print_position[i] *= INCH_PER_MM;
}
printFloat (print_position[i]);
/// 8c1
// if (i < N_AXIS-1)
printPgmString (PSTR (",") );
}
// Grbl global settings print out.
// NOTE: The numbering scheme here must correlate to storing in settings.c
void report_grbl_settings() {
printPgmString (PSTR ("$0=") );
printFloat (settings.steps_per_mm[X_AXIS]);
printPgmString (PSTR (" (x, step/mm)\r\n$1=") );
printFloat (settings.steps_per_mm[Y_AXIS]);
printPgmString (PSTR (" (y, step/mm)\r\n$2=") );
printFloat (settings.steps_per_mm[Z_AXIS]);
printPgmString (PSTR (" (z, step/mm)\r\n$3=") );
/// 8c1
#if (AXIS_T_TYPE == LINEAR)
printFloat (settings.steps_per_mm[T_AXIS]);
printPgmString (PSTR (" (t, step/mm.)\r\n$4=") );
#elif (AXIS_T_TYPE == ROTARY)
printFloat (to_degrees(settings.steps_per_mm[T_AXIS]));
printPgmString (PSTR (" (t, step/deg.)\r\n$4=") );
#endif
printInteger (settings.pulse_microseconds);
printPgmString (PSTR (" (step pulse, usec)\r\n$5=") );
printFloat (settings.default_feed_rate);
printPgmString (PSTR (" (default feed, mm/min)\r\n$6=") );
printFloat (settings.default_seek_rate);
printPgmString (PSTR (" (default seek, mm/min)\r\n$7=") );
printInteger (settings.invert_mask);
printPgmString (PSTR (" (step port invert mask, int:") );
print_uint8_base2 (settings.invert_mask);
printPgmString (PSTR (")\r\n$8=") );
printInteger (settings.stepper_idle_lock_time);
printPgmString (PSTR (" (step idle delay, msec)\r\n$9=" ));
// Convert from mm/min^2 for human readability
printFloat (settings.acceleration / (60 * 60) );
printPgmString (PSTR (" (acceleration, mm/sec^2)\r\n$10=") );
printFloat (settings.junction_deviation);
printPgmString (PSTR (" (junction deviation, mm)\r\n$11=") );
printFloat (settings.mm_per_arc_segment);
printPgmString (PSTR (" (arc, mm/segment)\r\n$12=") );
printInteger (settings.n_arc_correction);
printPgmString (PSTR (" (n-arc correction, int)\r\n$13=") );
printInteger (settings.decimal_places);
printPgmString (PSTR (" (n-decimals, int)\r\n$14=") );
printInteger (bit_istrue (settings.flags, BITFLAG_REPORT_INCHES) );
printPgmString (PSTR (" (report inches, bool)\r\n$15=") );
printInteger (bit_istrue (settings.flags, BITFLAG_AUTO_START) );
printPgmString (PSTR (" (auto start, bool)\r\n$16=") );
printInteger (bit_istrue (settings.flags, BITFLAG_INVERT_ST_ENABLE) );
printPgmString (PSTR (" (invert step enable, bool)\r\n$17=") );
printInteger (bit_istrue (settings.flags, BITFLAG_HARD_LIMIT_ENABLE) );
printPgmString (PSTR (" (hard limits, bool)\r\n$18=") );
printInteger (bit_istrue (settings.flags, BITFLAG_HOMING_ENABLE) );
printPgmString (PSTR (" (homing cycle, bool)\r\n$19=") );
printInteger (settings.homing_dir_mask);
printPgmString (PSTR (" (homing dir invert mask, int:") );
print_uint8_base2 (settings.homing_dir_mask);
printPgmString (PSTR (")\r\n$20=") );
printFloat (settings.homing_feed_rate);
printPgmString (PSTR (" (homing feed, mm/min)\r\n$21=") );
printFloat (settings.homing_seek_rate);
printPgmString (PSTR (" (homing seek, mm/min)\r\n$22=") );
printInteger (settings.homing_debounce_delay);
printPgmString (PSTR (" (homing debounce, msec)\r\n$23=") );
printFloat (settings.homing_pulloff);
printPgmString (PSTR (" (homing pull-off, mm)\r\n") );
}
int tracker_data_update (tracker2_data_t *tracks, navlcm_feature_list_t *fnext, int nsensors)
{
/* convert tracks to list of features */
navlcm_feature_list_t *fprev = (navlcm_feature_list_t*)malloc (sizeof(navlcm_feature_list_t));
tracks_head_features (tracks, fprev);
fprev->width = fnext->width;
fprev->height = fnext->height;
for (int i=0;i<fnext->num;i++)
fnext->el[i].index = i;
/* init tracks */
if (fprev->num == 0) {
tracker_data_init_from_data (tracks, fnext, nsensors);
return 0;
}
dbg (DBG_CLASS, "[tracker] matching %d vs %d features.", fprev->num, fnext->num);
/* match features */
navlcm_feature_match_set_t *matches = (navlcm_feature_match_set_t*)malloc(sizeof(navlcm_feature_match_set_t));
if (tracks->frontmat_training) {
find_feature_matches_fast_full (fprev, fnext, TRUE, TRUE, FALSE, .8, 30, to_degrees (40.0), matches, NULL, nsensors, NULL, NULL);
tracks_train_step (tracks, matches, fprev->width, fprev->height, nsensors);
} else {
find_feature_matches_fast_full (fprev, fnext, TRUE, TRUE, FALSE, .8, 30, to_degrees (40.0), matches, NULL, nsensors, NULL, NULL);
}
/* update tracks*/
int matched[fnext->num];
for (int i=0;i<fnext->num;i++)
matched[i] = 0;
for (int i=0;i<matches->num;i++) {
navlcm_feature_t *f1 = &matches->el[i].src;
navlcm_feature_t *f2 = matches->el[i].dst;
track2_t *t = (track2_t*)g_queue_peek_nth (tracks->data, f1->index);
track_insert_feature (t, f2);
matched[f2->index] = 1;
}
/* remove outdated tracks */
tracker_data_remove_outdated (tracks, fnext->utime, 2);
/* create new tracks */
int count=0;
for (int i=0;i<fnext->num;i++) {
if (!matched[i]) {
navlcm_feature_t *f = fnext->el + i;
tracker_data_add_track (tracks, f);
count++;
}
}
dbg (DBG_CLASS, "[tracker] added %d (%d) new tracks.", count, fprev->num);
/* free */
navlcm_feature_match_set_t_destroy (matches);
navlcm_feature_list_t_destroy (fprev);
return 0;
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed. All units and positions are converted and exported to grbl's
// internal functions in terms of (mm, mm/min) and absolute machine coordinates, respectively.
uint8_t gc_execute_line(char *line)
{
// If in alarm state, don't process. Immediately return with error.
// NOTE: Might not be right place for this, but also prevents $N storing during alarm.
if (sys.state == STATE_ALARM)
return(STATUS_ALARM_LOCK);
uint8_t char_counter = 0;
char letter;
float value;
int int_value;
uint16_t modal_group_words = 0; // Bitflag variable to track and check modal group words in block
uint8_t axis_words = 0; // Bitflag to track which XYZ(ABC) parameters exist in block
float inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
uint8_t absolute_override = false; // true(1) = absolute motion for this block only {G53}
uint8_t non_modal_action = NON_MODAL_NONE; // Tracks the actions of modal group 0 (non-modal)
/// 8c0
float target[N_AXIS], offset[N_AXIS];
clear_vector(target); // XYZ(ABC) axes parameters.
clear_vector(offset); // IJK Arc offsets are incremental. Value of zero indicates no change.
gc.status_code = STATUS_OK;
/* Pass 1: Commands and set all modes. Check for modal group violations.
NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */
uint8_t group_number = MODAL_GROUP_NONE;
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
// Set modal group values
switch(int_value) {
case 4: case 10: case 28: case 30: case 53: case 92: group_number = MODAL_GROUP_0; break;
case 0: case 1: case 2: case 3: case 80: group_number = MODAL_GROUP_1; break;
case 17: case 18: case 19: group_number = MODAL_GROUP_2; break;
case 90: case 91: group_number = MODAL_GROUP_3; break;
case 93: case 94: group_number = MODAL_GROUP_5; break;
case 20: case 21: group_number = MODAL_GROUP_6; break;
case 54: case 55: case 56: case 57: case 58: case 59: group_number = MODAL_GROUP_12; break;
}
// Set 'G' commands
switch(int_value) {
case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
case 4: non_modal_action = NON_MODAL_DWELL; break;
case 10: non_modal_action = NON_MODAL_SET_COORDINATE_DATA; break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(Z_AXIS, X_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30:
int_value = trunc(10*value); // Multiply by 10 to pick up Gxx.1
switch(int_value) {
case 280: non_modal_action = NON_MODAL_GO_HOME_0; break;
case 281: non_modal_action = NON_MODAL_SET_HOME_0; break;
case 300: non_modal_action = NON_MODAL_GO_HOME_1; break;
case 301: non_modal_action = NON_MODAL_SET_HOME_1; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 53: absolute_override = true; break;
case 54: case 55: case 56: case 57: case 58: case 59:
gc.coord_select = int_value-54;
break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 92:
int_value = trunc(10*value); // Multiply by 10 to pick up G92.1
switch(int_value) {
case 920: non_modal_action = NON_MODAL_SET_COORDINATE_OFFSET; break;
case 921: non_modal_action = NON_MODAL_RESET_COORDINATE_OFFSET; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
// Set modal group values
switch(int_value) {
case 0: case 1: case 2: case 30: group_number = MODAL_GROUP_4; break;
case 3: case 4: case 5: group_number = MODAL_GROUP_7; break;
}
// Set 'M' commands
switch(int_value) {
case 0: gc.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause
case 1: break; // Optional stop not supported. Ignore.
case 2: case 30: gc.program_flow = PROGRAM_FLOW_COMPLETED; break; // Program end and reset
case 3: gc.spindle_direction = 1; break;
case 4: gc.spindle_direction = -1; break;
case 5: gc.spindle_direction = 0; break;
#ifdef ENABLE_M7
case 7: gc.coolant_mode = COOLANT_MIST_ENABLE; break;
#endif
case 8: gc.coolant_mode = COOLANT_FLOOD_ENABLE; break;
case 9: gc.coolant_mode = COOLANT_DISABLE; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
}
// Check for modal group multiple command violations in the current block
if (group_number) {
if ( bit_istrue(modal_group_words,bit(group_number)) ) {
FAIL(STATUS_MODAL_GROUP_VIOLATION);
} else {
bit_true(modal_group_words,bit(group_number));
}
group_number = MODAL_GROUP_NONE; // Reset for next command.
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code)
return gc.status_code;
/* Pass 2: Parameters. All units converted according to current block commands. Position
parameters are converted and flagged to indicate a change. These can have multiple connotations
for different commands. Each will be converted to their proper value upon execution. */
float p = 0, r = 0;
uint8_t l = 0;
char_counter = 0;
while(next_statement(&letter, &value, line, &char_counter)) {
switch(letter) {
case 'G': case 'M': case 'N': break; // Ignore command statements and line numbers
case 'F':
if (value <= 0)
FAIL(STATUS_INVALID_STATEMENT); // Must be greater than zero
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = to_millimeters(value); // seconds per motion for this motion only
}
else {
gc.feed_rate = to_millimeters(value); // millimeters per minute
}
break;
case 'I': case 'J': case 'K':
offset[letter-'I'] = to_millimeters(value);
break;
case 'L': l =
trunc(value);
break;
case 'P':
p = value;
break;
case 'R':
r = to_millimeters(value);
break;
case 'S':
if (value < 0)
FAIL(STATUS_INVALID_STATEMENT); // Cannot be negative
// TBD: Spindle speed not supported due to PWM issues, but may come back once resolved.
// gc.spindle_speed = value;
break;
case 'T':
if (value < 0)
FAIL(STATUS_INVALID_STATEMENT); // Cannot be negative
gc.tool = trunc(value);
break;
case 'X':
target[X_AXIS] = to_millimeters(value);
bit_true(axis_words,bit(X_AXIS));
break;
case 'Y':
target[Y_AXIS] = to_millimeters(value);
bit_true(axis_words,bit(Y_AXIS));
break;
case 'Z':
target[Z_AXIS] = to_millimeters(value);
bit_true(axis_words,bit(Z_AXIS));
break;
/// 8c1
#if (AXIS_T_TYPE == LINEAR)
/// axis U, V, W choice
#if AXIS_T == AXIS_U
case 'U':
#elif AXIS_T == AXIS_V
case 'V' :
#elif AXIS_T == AXIS_W
case 'W' :
#endif
#if (AXIS_T == AXIS_U || AXIS_T == AXIS_V || AXIS_T == AXIS_W)
target[T_AXIS] = to_millimeters(value);
bit_true(axis_words,bit(T_AXIS));
break;
#else
#error
#endif
#elif (AXIS_T_TYPE == ROTARY)
/// axis A, B, C choice
#if AXIS_T == AXIS_A
case 'A':
#elif AXIS_T == AXIS_B
case 'B':
#elif AXIS_T == AXIS_C
case 'C':
#endif
#if (AXIS_T == AXIS_A|| AXIS_T == AXIS_B || AXIS_T == AXIS_C)
target[T_AXIS] = to_degrees(value);
bit_true(axis_words,bit(T_AXIS));
#else
#error
#endif
break;
#else
#error
#endif
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code)
return gc.status_code;
/* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41.
NOTE: Independent non-motion/settings parameters are set out of this order for code efficiency
and simplicity purposes, but this should not affect proper g-code execution. */
// ([F]: Set feed and seek rates.)
// TODO: Seek rates can change depending on the direction and maximum speeds of each axes. When
// max axis speed is installed, the calculation can be performed here, or maybe in the planner.
if (sys.state != STATE_CHECK_MODE) {
// ([M6]: Tool change should be executed here.)
// [M3,M4,M5]: Update spindle state
spindle_run(gc.spindle_direction);
// [*M7,M8,M9]: Update coolant state
coolant_run(gc.coolant_mode);
}
// [G54,G55,...,G59]: Coordinate system selection
if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_12)) ) { // Check if called in block
float coord_data[N_AXIS];
if (!(settings_read_coord_data(gc.coord_select,coord_data)))
return(STATUS_SETTING_READ_FAIL);
memcpy(gc.coord_system,coord_data,sizeof(coord_data));
}
// [G4,G10,G28,G30,G92,G92.1]: Perform dwell, set coordinate system data, homing, or set axis offsets.
// NOTE: These commands are in the same modal group, hence are mutually exclusive. G53 is in this
// modal group and do not effect these actions.
switch (non_modal_action) {
case NON_MODAL_DWELL:
if (p < 0) { // Time cannot be negative.
FAIL(STATUS_INVALID_STATEMENT);
}
else {
// Ignore dwell in check gcode modes
if (sys.state != STATE_CHECK_MODE)
mc_dwell(p);
}
break;
case NON_MODAL_SET_COORDINATE_DATA:
int_value = trunc(p); // Convert p value to int.
if ((l != 2 && l != 20) || (int_value < 0 || int_value > N_COORDINATE_SYSTEM)) { // L2 and L20. P1=G54, P2=G55, ...
FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
else
if (!axis_words && l==2) {// No axis words.
FAIL(STATUS_INVALID_STATEMENT);
}
else {
if (int_value > 0)
int_value--; // Adjust P1-P6 index to EEPROM coordinate data indexing.
else
int_value = gc.coord_select; // Index P0 as the active coordinate system
float coord_data[N_AXIS];
if (!settings_read_coord_data(int_value,coord_data))
return(STATUS_SETTING_READ_FAIL);
uint8_t i;
// Update axes defined only in block. Always in machine coordinates. Can change non-active system.
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
if (l == 20) {
coord_data[i] = gc.position[i]-target[i]; // L20: Update axis current position to target
}
else {
coord_data[i] = target[i]; // L2: Update coordinate system axis
}
}
}
settings_write_coord_data(int_value,coord_data);
// Update system coordinate system if currently active.
if (gc.coord_select == int_value)
memcpy(gc.coord_system,coord_data,sizeof(coord_data));
}
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_GO_HOME_0: case NON_MODAL_GO_HOME_1:
// Move to intermediate position before going home. Obeys current coordinate system and offsets
// and absolute and incremental modes.
if (axis_words) {
// Apply absolute mode coordinate offsets or incremental mode offsets.
uint8_t i;
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if ( bit_istrue(axis_words,bit(i)) ) {
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i];
}
else {
target[i] += gc.position[i];
}
}
else {
target[i] = gc.position[i];
}
}
/// 8c1 : line
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[T_AXIS], settings.default_seek_rate, false, C_LINE);
}
// Retreive G28/30 go-home position data (in machine coordinates) from EEPROM
float coord_data[N_AXIS];
if (non_modal_action == NON_MODAL_GO_HOME_1) {
if (!settings_read_coord_data(SETTING_INDEX_G30 ,coord_data))
return STATUS_SETTING_READ_FAIL;
}
else {
if (!settings_read_coord_data(SETTING_INDEX_G28 ,coord_data))
return(STATUS_SETTING_READ_FAIL);
}
///8c1 : line
mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], coord_data[T_AXIS], settings.default_seek_rate, false, C_LINE);
memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[];
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1:
if (non_modal_action == NON_MODAL_SET_HOME_1) {
settings_write_coord_data(SETTING_INDEX_G30,gc.position);
}
else {
settings_write_coord_data(SETTING_INDEX_G28,gc.position);
}
break;
case NON_MODAL_SET_COORDINATE_OFFSET:
if (!axis_words) { // No axis words
FAIL(STATUS_INVALID_STATEMENT);
}
else {
// Update axes defined only in block. Offsets current system to defined value. Does not update when
// active coordinate system is selected, but is still active unless G92.1 disables it.
uint8_t i;
/// 8c0 : 2 -> N_AXIS
for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
if (bit_istrue(axis_words,bit(i)) ) {
gc.coord_offset[i] = gc.position[i]-gc.coord_system[i]-target[i];
}
}
}
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_RESET_COORDINATE_OFFSET:
clear_vector(gc.coord_offset); // Disable G92 offsets by zeroing offset vector.
break;
}
// [G0,G1,G2,G3,G80]: Perform motion modes.
// NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes.
// Enter motion modes only if there are axis words or a motion mode command word in the block.
if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_1)) || axis_words ) {
// G1,G2,G3 require F word in inverse time mode.
if ( gc.inverse_feed_rate_mode ) {
if (inverse_feed_rate < 0 && gc.motion_mode != MOTION_MODE_CANCEL)
FAIL(STATUS_INVALID_STATEMENT);
}
// Absolute override G53 only valid with G0 and G1 active.
if ( absolute_override && !(gc.motion_mode == MOTION_MODE_SEEK || gc.motion_mode == MOTION_MODE_LINEAR))
FAIL(STATUS_INVALID_STATEMENT);
// Report any errors.
if (gc.status_code)
return(gc.status_code);
// Convert all target position data to machine coordinates for executing motion. Apply
// absolute mode coordinate offsets or incremental mode offsets.
// NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
uint8_t i;
/// 8c0
for (i=0; i< N_AXIS; i++) { // Axes indices are consistent, so loop may be used to save flash space.
if ( bit_istrue(axis_words,bit(i)) ) {
if (!absolute_override) { // Do not update target in absolute override mode
if (gc.absolute_mode) {
target[i] += gc.coord_system[i] + gc.coord_offset[i]; // Absolute mode
}
else {
target[i] += gc.position[i]; // Incremental mode
}
}
}
else {
target[i] = gc.position[i]; // No axis word in block. Keep same axis position.
}
}
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL:
if (axis_words)
FAIL(STATUS_INVALID_STATEMENT); // No axis words allowed while active.
break;
case MOTION_MODE_SEEK:
if (!axis_words) {
FAIL(STATUS_INVALID_STATEMENT);
}
else
/// 8c1 :line
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[T_AXIS],
settings.default_seek_rate, false, C_LINE);
break;
case MOTION_MODE_LINEAR:
// TODO: Inverse time requires F-word with each statement. Need to do a check. Also need
// to check for initial F-word upon startup. Maybe just set to zero upon initialization
// and after an inverse time move and then check for non-zero feed rate each time. This
// should be efficient and effective.
if (!axis_words) {
FAIL(STATUS_INVALID_STATEMENT);
}
else {
/// 8c1 :line
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[T_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, C_LINE);
}
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
// Check if at least one of the axes of the selected plane has been specified. If in center
// format arc mode, also check for at least one of the IJK axes of the selected plane was sent.
if ( !( bit_false(axis_words,bit(gc.plane_axis_2)) ) ||
( !r && !offset[gc.plane_axis_0] && !offset[gc.plane_axis_1] ) ) {
FAIL(STATUS_INVALID_STATEMENT);
} else {
if (r != 0) { // Arc Radius Mode
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
// First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller
// than d. If so, the sqrt of a negative number is complex and error out.
float h_x2_div_d = 4 * r*r - x*x - y*y;
if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); }
// Finish computing h_x2_div_d.
h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d)
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) {
h_x2_div_d = -h_x2_div_d;
r = -r; // Finished with r. Set to positive for mc_arc
}
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
} else { // Arc Center Format Offset Mode
r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
}
// Set clockwise/counter-clockwise sign for mc_arc computations
uint8_t isclockwise = false;
if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
// Trace the arc
mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
r, isclockwise);
}
break;
}
// Report any errors.
if (gc.status_code) { return(gc.status_code); }
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(target)); // gc.position[] = target[];
}
// M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may
// refill and can only be resumed by the cycle start run-time command.
if (gc.program_flow) {
plan_synchronize(); // Finish all remaining buffered motions. Program paused when complete.
sys.auto_start = false; // Disable auto cycle start. Forces pause until cycle start issued.
// If complete, reset to reload defaults (G92.2,G54,G17,G90,G94,M48,G40,M5,M9). Otherwise,
// re-enable program flow after pause complete, where cycle start will resume the program.
if (gc.program_flow == PROGRAM_FLOW_COMPLETED) { mc_reset(); }
else { gc.program_flow = PROGRAM_FLOW_RUNNING; }
}
return(gc.status_code);
}
void TOea_Planner::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal_msg)
{
if (!map_received_)
{
ROS_ERROR_NAMED(logger_name_, "No map received yet. Unable to compute path.");
return;
}
planner_state.data = hardware::BUSY; //PLANNING;
state_pub_.publish(planner_state);
ROS_DEBUG_NAMED(logger_name_, "New Goal received on topic");
// get world pose from msg
Astar_.goal_world_pose_.x = goal_msg->pose.position.x;
Astar_.goal_world_pose_.y = goal_msg->pose.position.y;
Astar_.goal_world_pose_.yaw = tf::getYaw(goal_msg->pose.orientation);
oea_msgs::Oea_path oea_path; // plan variable
// oea_path.path.poses.clear();
//check validity:
if (Astar_.goal_world_pose_.yaw!=Astar_.goal_world_pose_.yaw) //if nan
{
ROS_ERROR_STREAM_NAMED(logger_name_, "Invalig Goal: yaw is " << to_degrees(Astar_.goal_world_pose_.yaw));
//else just publish the blank path
oea_path_pub_.publish(oea_path); //publishing 0 poses will cause the controller to stop following the previous path
visual_path_pub_.publish(oea_path.path); // publish nav_msgs/Path to view path on rviz
planner_state.data = hardware::IDLE;
state_pub_.publish(planner_state);
return;
}
//convert it to grid coord
Astar_.ConvertWorlCoordToMatrix(Astar_.goal_world_pose_.x, Astar_.goal_world_pose_.y, Astar_.goal_world_pose_.yaw , Astar_.goal_grid_pose_.x, Astar_.goal_grid_pose_.y, Astar_.goal_grid_pose_.z);
std::string error_str;
// check if goal is valid
//error_str = "GoalCb: check grid pose val";
if (Astar_.is_valid_point(Astar_.goal_grid_pose_,error_str))
{
//get path from the Astar...
executeCycle(error_str, oea_path);
}
else
{
ROS_WARN_STREAM_NAMED(logger_name_, error_str);
// no need to clear Grid, because Astar was not called
// publish empty plan to stop the robot in case it's moving and received a new point...
oea_path_pub_.publish(oea_path);
visual_path_pub_.publish(oea_path.path);
}
planner_state.data = hardware::IDLE;
state_pub_.publish(planner_state);
}
float8 colors_delta_e_cie_2000(float8 l1, float8 a1, float8 b1, float8 l2, float8 a2, float8 b2, float8 Kl, float8 Kc, float8 Kh)
{
float8 avg_Lp,
C1,
C2,
avg_C1_C2,
G,
a1p,
a2p,
C1p,
C2p,
avg_C1p_C2p,
h1p,
h2p,
avg_Hp,
T,
diff_h2p_h1p,
delta_hp,
delta_Lp,
delta_Cp,
delta_Hp,
S_L,
S_C,
S_H,
delta_ro,
R_C,
R_T;
avg_Lp = (l1+l2)/2.0;
C1 = sqrt(pow(a1, 2.0) + pow(b1, 2.0));
C2 = sqrt(pow(a2, 2.0) + pow(b2, 2.0));
avg_C1_C2 = (C1 + C2) / 2.0;
G = 0.5 * (1 - sqrt(pow(avg_C1_C2, 7.0) / (pow(avg_C1_C2, 7.0) + pow(25.0, 7.0))));
a1p = (1.0 + G) * a1;
a2p = (1.0 + G) * a2;
C1p = sqrt(pow(a1p, 2.0) + pow(b1, 2.0));
C2p = sqrt(pow(a2p, 2.0) + pow(b2, 2.0));
avg_C1p_C2p = (C1p + C2p) / 2.0;
h1p = to_degrees(atan2(b1, a1p));
h1p += (h1p < 0 ? 360 : 0);
h2p = to_degrees(atan2(b2, a2p));
h2p += (h2p < 0 ? 360 : 0);
avg_Hp = ((fabs(h1p - h2p) > 180 ? 360 : 0) + h1p + h2p) / 2.0;
T = 1 - 0.17 * cos(to_radians(avg_Hp - 30)) +
0.24 * cos(to_radians(2 * avg_Hp)) +
0.32 * cos(to_radians(3 * avg_Hp + 6)) -
0.2 * cos(to_radians(4 * avg_Hp - 63));
diff_h2p_h1p = h2p - h1p;
delta_hp = diff_h2p_h1p + (fabs(diff_h2p_h1p) > 180 ? 360 : 0);
delta_hp -= (h2p > h1p ? 720 : 0);
delta_Lp = l2 - l1;
delta_Cp = C2p - C1p;
delta_Hp = 2 * sqrt(C2p * C1p) * sin(to_radians(delta_hp) / 2.0);
S_L = 1 + ((0.015 * pow(avg_Lp - 50, 2.0)) / sqrt(20 + pow(avg_Lp - 50, 2.0)));
S_C = 1 + 0.045 * avg_C1p_C2p;
S_H = 1 + 0.015 * avg_C1p_C2p * T;
delta_ro = 30 * exp(-(pow(((avg_Hp - 275) / 25), 2.0)));
R_C = sqrt((pow(avg_C1p_C2p, 7.0)) / (pow(avg_C1p_C2p, 7.0) + pow(25.0, 7.0)));
R_T = -2 * R_C * sin(2 * to_radians(delta_ro));
return sqrt(
pow(delta_Lp / (S_L * Kl), 2) +
pow(delta_Cp / (S_C * Kc), 2) +
pow(delta_Hp / (S_H * Kh), 2) +
R_T * (delta_Cp / (S_C * Kc)) * (delta_Hp / (S_H * Kh)));
}
static void
_draw_track (RendererSimTraffic *self, const lcmtypes_track_t *vd)
{
// generate a stable non-black, non-white color from object ID
srand(vd->id);
GLfloat rgb[4];
rand_color(rgb);
glColorMaterial (GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
glMaterialfv (GL_FRONT_AND_BACK, GL_AMBIENT, rgb);
glMaterialfv (GL_FRONT_AND_BACK, GL_DIFFUSE, rgb);
// assumes viewing, modeling matrices, etc. set up for local frame
// leaves these matrices as they were before invocation
glMatrixMode (GL_MODELVIEW);
glPushMatrix();
glTranslated (vd->pos[0], vd->pos[1], 0);
// rotate basis to body coords of vehicle (rotate by theta about Z axis)
glRotated (to_degrees(vd->theta), 0.0, 0.0, 1.0); // convert rad to deg
if (self->ehandler.hovering &&
self->selected_vehicle_id == vd->id) {
glColor4f (1, 1, 1, 0.6);
} else {
glColor4fv(rgb);
}
// int use_model = gtku_param_widget_get_bool(self->pw, PARAM_PRETTY);
//
// if (use_model) {
//
// glRotated(90, 0, 0, 1);
// glRotated(90, 1, 0, 0);
//
// double s = 4.5;
// glScalef(s,s,s);
//
// glTranslated(0,.115,0); // car height
// glTranslated(0,0,.1); // front-to-back offset
// glBegin(GL_TRIANGLES);
// for (int i = 0; i < numChevy_vanFaces; i++) {
// int *vs = Chevy_vanFaces[i];
// for (int j = 0; j < 3; j++) {
// glNormal3fv(Chevy_vanVertNorms[vs[j]]);
// glVertex3fv(Chevy_vanVerts[vs[j]]);
// }
//
// }
// glEnd();
// } else {
glTranslated ( 0, 0, VEHICLE_HEIGHT / 2);
glScalef (vd->size[0], vd->size[1], VEHICLE_HEIGHT);
glutil_draw_cube();
// }
// restore matrix state
glPopMatrix();
if (self->ehandler.picking && self->selected_vehicle_id == vd->id) {
glColor4f (1, 1, 1, 0.6);
glPushMatrix ();
glTranslatef (self->last_xy[0], self->last_xy[1], 0);
glRotated (to_degrees(vd->theta), 0.0, 0.0, 1.0); // convert rad to deg
glScalef (vd->size[0], vd->size[1], VEHICLE_HEIGHT);
glutil_draw_cube_frame ();
glPopMatrix ();
}
}
