YMCL_EXPORT bool_t sample_motion_particles_thrun(particle_pool_t* particle_pool, motion_data_t* motion, const motion_param_t *param) {
int i;
real_t dx = motion->pose1.x - motion->pose0.x;
real_t dy = motion->pose1.y - motion->pose0.y;
pose_t pose_sum;
pose_sum.x = pose_sum.y = pose_sum.th = 0;
motion->delta_rot1 = normalize_angle(atan2(dy, dx) - motion->pose0.th);
if (motion->delta_rot1 < -M_PI / 2 || motion->delta_rot1 > M_PI / 2) { /// backward motion
motion->delta_trans = -sqrt(dx*dx + dy*dy);
motion->delta_rot1 = normalize_angle(motion->delta_rot1 + M_PI);
}
else {
motion->delta_trans = sqrt(dx*dx + dy*dy);
}
motion->delta_rot2 = normalize_angle(motion->pose1.th - motion->pose0.th - motion->delta_rot1);
if (fabs(motion->delta_trans) > param->update_distance || fabs(motion->delta_rot2) > param->update_heading) {
for (i = 0; i < particle_pool->num_particles; i++) {
sample_motion_data_thrun(particle_pool->particles + i, motion, param);
pose_sum.x += particle_pool->particles[i].pose.x;
pose_sum.y += particle_pool->particles[i].pose.y;
pose_sum.th += particle_pool->particles[i].pose.th;
}
particle_pool->mean_pose.x = pose_sum.x / particle_pool->num_particles;
particle_pool->mean_pose.y = pose_sum.y / particle_pool->num_particles;
particle_pool->mean_pose.th = pose_sum.th / particle_pool->num_particles;
return TRUE;
}
return FALSE;
}
int LightSensor :: update(const Posture& pos,
const boost::shared_ptr<Map>& map)
{
const std::vector<Map::ill_sw_t>& isv = map->get_illuminated_switches();
_activated = false;
for (size_t i = 0; i < isv.size(); ++i)
if (isv[i]->get_color() == get_color() && isv[i]->get_on())
{
float angle =
normalize_angle(atan2(isv[i]->get_y() - pos.y(),
isv[i]->get_x() - pos.x())
- pos.theta());
float x_res = 0, y_res = 0;// ignored
if (angle > normalize_angle(_angle - _range / 2.0f)
&& angle < normalize_angle(_angle + _range / 2.0f)
&& !map->check_inter_real(isv[i]->get_x(), isv[i]->get_y(),
pos.x(), pos.y(),
x_res, y_res))
{
_activated = true;
_num=i;
}
}
return _activated;
}
/** Read the next point in the asol fits file and set variable Aspsol to it.
* There is no randomn access, this will always read exactly the next line
* in the fits file.
* Aspsol is a module level variable that is defined when the FILE dither
* model is in use.
*/
static int get_single_aspsol_point (void)
{
double buf[7];
JDMVector_Type p;
Aspsol.t0 = Aspsol.t1;
Aspsol.ra0 = Aspsol.ra1;
Aspsol.dec0 = Aspsol.dec1;
Aspsol.roll0 = Aspsol.roll1;
Aspsol.dy0 = Aspsol.dy1;
Aspsol.dz0 = Aspsol.dz1;
Aspsol.dtheta0 = Aspsol.dtheta1;
if (-1 == jdfits_simple_d_read_btable (Aspsol.bt, buf))
{
close_aspsol ();
return -1;
}
Aspsol.t1 = buf[0];
Aspsol.ra1 = buf[1] * (PI/180.0);
Aspsol.dec1 = buf[2] * (PI/180.0);
Aspsol.roll1 = buf[3] * (PI/180.0);
Aspsol.dy1 = buf[4];
Aspsol.dz1 = buf[5];
Aspsol.dtheta1 = buf[6] * (PI/180.0);
/* Keep cumulative sum, because we eventually need the mean of those values*/
Aspsol.num_steps++;
Aspsol.sum_dy += Aspsol.dy1;
Aspsol.sum_dz += Aspsol.dz1;
Aspsol.sum_dtheta += Aspsol.dtheta1;
/* We need to convert the ra/dec information to the unrolled values
* since in the unrolled frame they are equivalent to yaw/pitch. Note
* that ra/dec system differs from the spherical system in the definition
* of the polar angle.
*/
p = JDMv_spherical_to_vector (1.0, (PI/2.0) - Aspsol.dec1, Aspsol.ra1);
p = JDMv_rotate_unit_vector (p, Nominal_Pointing, -Aspsol.roll1);
JDMv_unit_vector_to_spherical (p, &Aspsol.dec1, &Aspsol.ra1);
Aspsol.dec1 = (PI/2.0) - Aspsol.dec1;
normalize_angle (&Aspsol.ra1);
normalize_angle (&Aspsol.dec1);
normalize_angle (&Aspsol.roll1);
normalize_angle (&Aspsol.dtheta1);
marx_compute_ra_dec_offsets (Nominal_Ra, Nominal_Dec,
Aspsol.ra1, Aspsol.dec1,
&Aspsol.ra1, &Aspsol.dec1);
/* Aspsol.roll1 -= Nominal_Roll; */
Aspsol.t1 -= Start_Time;
return 0;
}
/// returns the shortest angle between two vectors with the given angles
inline float angle_diff(float first_angle, float second_angle)
{
float diff = std::abs(normalize_angle(first_angle) - normalize_angle(second_angle));
if (diff > M_PI)
{
diff = diff - M_PI;
}
return diff;
}
double Neuroseg_Theta_Offset(const Neuroseg *seg1, const Neuroseg *seg2)
{
double diff = fabs(normalize_angle(seg1->theta) -
normalize_angle(seg2->theta));
if (diff > TZ_PI) {
diff = TZ_2PI - diff;
}
return diff;
}
Pose Pose::operator* (const Pose& p) {
Pose result(*this);
result._t += _R*p._t;
result._R.angle() += p._R.angle();
result._R.angle() = normalize_angle(result._R.angle());
return result;
}
Posture operator+(const Posture& o) const
{
Posture p;
p._x = _x + o._x;
p._y = _y + o._y;
p._theta = normalize_angle(_theta + o._theta);
return p;
}
Pose Pose::inverse() {
Pose ret;
ret.setRotation(_R.inverse());
ret.setAngle(normalize_angle(ret._R.angle()));
//#ifdef _MSC_VER
// ret._t=ret._R*(Vector2d(_t*-1.));
//#else
ret.setTranslation(ret._R*(_t*-1.));
//#endif
return ret;
}
static inline double shortest_angular_distance(double from, double to)
{
double result = normalize_angle_positive(normalize_angle_positive(to) - normalize_angle_positive(from));
if (result > M_PI)
// If the result > 180,
// It's shorter the other way.
result = -(2.0*M_PI - result);
return normalize_angle(result);
}
static Boolean logs_sum_to_zero(
Complex summand0, Orientation eo0,
Complex summand1, Orientation eo1)
{
Complex sum;
if (eo0 != eo1)
summand1.real = - summand1.real;
sum = complex_plus(summand0, summand1);
normalize_angle(&sum.imag);
return (complex_modulus(sum) < CANCELLATION_EPSILON);
}
text_upright_e placement_finder::simplify_upright(text_upright_e upright, double angle) const
{
if (upright == UPRIGHT_AUTO)
{
return (std::fabs(normalize_angle(angle)) > 0.5*M_PI) ? UPRIGHT_LEFT : UPRIGHT_RIGHT;
}
if (upright == UPRIGHT_LEFT_ONLY)
{
return UPRIGHT_LEFT;
}
if (upright == UPRIGHT_RIGHT_ONLY)
{
return UPRIGHT_RIGHT;
}
return upright;
}
int _marx_init_dither (Param_File_Type *pf, int dither_flags, double *yoff, double *zoff)
{
_Marx_Dither_Mode = _MARX_DITHER_MODE_NONE;
Get_Dither_Function = NULL;
Get_Dither_Par_Means = NULL;
if (-1 == pf_get_parameters (pf, Dither_Parm_Table))
{
marx_error ("error getting dither parameters");
return -1;
}
Aspect_Blur *= PI/(180.0*3600.0);
if (Aspect_Blur < 0)
Aspect_Blur = 0.0;
Pointing_Offset_Y *= PI/(180.0*3600.0);
Pointing_Offset_Z *= PI/(180.0*3600.0);
*yoff = Pointing_Offset_Y;
*zoff = Pointing_Offset_Z;
Nominal_Ra *= PI/180.0;
Nominal_Dec *= PI/180.0;
Nominal_Roll *= PI/180.0;
normalize_angle (&Nominal_Roll);
if ((dither_flags & _MARX_DITHER_UNSUPPORTED)
|| (0 == strcmp (Dither_Model, "NONE")))
return init_no_dither (dither_flags);
if (0 == strcmp (Dither_Model, "INTERNAL"))
return init_internal_dither (dither_flags);
if (0 == strcmp (Dither_Model, "FILE"))
return init_aspsol_dither (dither_flags);
marx_error ("DitherModel = %s is not supported", Dither_Model);
return -1;
}
static void add_complex_with_log(
ComplexWithLog *cwl0, Orientation eo0,
ComplexWithLog *cwl1, Orientation eo1,
ComplexWithLog *cwl2, Orientation eo2)
{
/*
* First compute the sum of the logs, then recover
* the rectangular form.
*
* We do all computations relative to the Orientation
* of the EdgeClass. So if a particular edge is seen
* as left_handed by the EdgeClass, we must negate the
* real part of the log of its complex angle. (Recall
* that all all TetShapes are stored relative to the
* right_handed Orientation of the Tetrahedron.)
*/
Complex summand0,
summand1,
sum;
summand0 = cwl0->log;
if (eo0 == left_handed)
summand0.real = - summand0.real;
summand1 = cwl1->log;
if (eo1 == left_handed)
summand1.real = - summand1.real;
sum = complex_plus(summand0, summand1);
if (eo2 == left_handed)
sum.real = - sum.real;
normalize_angle(&sum.imag);
cwl2->log = sum;
cwl2->rect = complex_exp(sum);
}
const Posture& move(float d_l, float d_r,
float wheels_dist)
{
Posture old_pos = *this;
float alpha = (d_r - d_l) / wheels_dist;
Posture p;
if (fabs(alpha) > 1e-10)
{
float r = (d_l / alpha) + wheels_dist / 2;
float d_x = (cos(alpha) - 1) * r;
float d_y = sin(alpha) * r;
p = Posture(d_x, d_y, alpha);
p.rotate(old_pos.theta() - M_PI / 2);
p.set_theta(normalize_angle(alpha));
}
else
p = Posture(d_l * cos(old_pos.theta()),
d_l * sin(old_pos.theta()),
0);
*this = p + old_pos;
return *this;
}
/* ----------------------------------------------------------------------------
* Spews out the converted seeds.
*/
void converter::spew() {
size_t total_to_spit = amount_in_buffer * con_type->pikmin_per_conversion;
for(size_t s = 0; s < total_to_spit; ++s) {
if(pikmin_list.size() == max_pikmin_in_field) break;
float horizontal_strength =
CONVERTER_SPEW_H_SPEED +
randomf(
-CONVERTER_SPEW_H_SPEED_DEVIATION,
CONVERTER_SPEW_H_SPEED_DEVIATION
);
spew_pikmin_seed(
pos, z + CONVERTER_NEW_SEED_Z_OFFSET, current_type,
next_spew_angle, horizontal_strength, CONVERTER_SPEW_V_SPEED
);
next_spew_angle += CONVERTER_SPEW_ANGLE_SHIFT;
next_spew_angle = normalize_angle(next_spew_angle);
}
amount_in_buffer = 0;
}
bool placement_finder::single_line_placement(vertex_cache &pp, text_upright_e orientation)
{
//
// IMPORTANT NOTE: See note about coordinate systems in find_point_placement()!
//
vertex_cache::scoped_state begin(pp);
text_upright_e real_orientation = simplify_upright(orientation, pp.angle());
glyph_positions_ptr glyphs = std::make_shared<glyph_positions>();
std::vector<box2d<double> > bboxes_all;
glyphs->reserve(layouts_.glyphs_count());
bboxes_all.reserve(layouts_.glyphs_count());
unsigned upside_down_glyph_count = 0;
marker_placement_finder_ptr marker_placement_ptr;
for (auto const& layout_ptr : layouts_)
{
text_layout const& layout = *layout_ptr;
if(has_marker_ && layout.shield_layout())
{
marker_placement_ptr = std::make_shared<marker_placement_finder>(layout_ptr);
}
pixel_position align_offset = layout.alignment_offset();
pixel_position const& layout_displacement = layout.displacement();
double sign = (real_orientation == UPRIGHT_LEFT) ? -1 : 1;
double offset = layout_displacement.y + 0.5 * sign * layout.height();
double adjust_character_spacing = .0;
double layout_width = layout.width();
bool adjust = layout.horizontal_alignment() == H_ADJUST;
if (adjust)
{
text_layout::const_iterator longest_line = layout.longest_line();
if (longest_line != layout.end())
{
adjust_character_spacing = (pp.length() - longest_line->glyphs_width()) / longest_line->space_count();
layout_width = longest_line->glyphs_width() + longest_line->space_count() * adjust_character_spacing;
}
}
//find the proper center line
int line_count = layout.num_lines() % 2 == 0 ? layout.num_lines() : layout.num_lines() - 1;
int centre_line_index = line_count/2;
int line_index = -1;
for (auto const& line : layout)
{
line_index++;
if(has_marker_ && marker_placement_ptr &&
(line_index == centre_line_index))
{
marker_placement_ptr->line_size(line.size());
}
// Only subtract half the line height here and half at the end because text is automatically
// centered on the line
if (layout.num_lines() == 1 || (layout.num_lines() > 1 && line_index != 0))
{
offset -= sign * line.height()/2;
}
vertex_cache & off_pp = pp.get_offseted(offset, sign * layout_width);
vertex_cache::scoped_state off_state(off_pp); // TODO: Remove this when a clean implementation in vertex_cache::get_offseted is done
double line_width = adjust ? (line.glyphs_width() + line.space_count() * adjust_character_spacing) : line.width();
if (!off_pp.move(sign * layout.jalign_offset(line_width) - align_offset.x)) return false;
double last_cluster_angle = 999;
int current_cluster = -1;
pixel_position cluster_offset;
double angle = 0.0;
rotation rot;
double last_glyph_spacing = 0.;
for (auto const& glyph : line)
{
if (current_cluster != static_cast<int>(glyph.char_index))
{
if (adjust)
{
if (!off_pp.move(sign * (layout.cluster_width(current_cluster) + last_glyph_spacing)))
return false;
last_glyph_spacing = adjust_character_spacing;
}
else
{
if (!off_pp.move_to_distance(sign * (layout.cluster_width(current_cluster) + last_glyph_spacing)))
return false;
last_glyph_spacing = glyph.format->character_spacing * scale_factor_;
}
current_cluster = glyph.char_index;
// Only calculate new angle at the start of each cluster!
angle = normalize_angle(off_pp.angle(sign * layout.cluster_width(current_cluster)));
rot.init(angle);
if ((text_props_->max_char_angle_delta > 0) && (last_cluster_angle != 999) &&
std::fabs(normalize_angle(angle-last_cluster_angle)) > text_props_->max_char_angle_delta)
{
return false;
}
cluster_offset.clear();
last_cluster_angle = angle;
}
if (std::abs(angle) > M_PI/2) ++upside_down_glyph_count;
pixel_position pos = off_pp.current_position() + cluster_offset;
// Center the text on the line
double char_height = line.max_char_height();
pos.y = -pos.y - char_height/2.0*rot.cos;
pos.x = pos.x + char_height/2.0*rot.sin;
cluster_offset.x += rot.cos * glyph.advance();
cluster_offset.y -= rot.sin * glyph.advance();
box2d<double> bbox = get_bbox(layout, glyph, pos, rot);
if (collision(bbox, layouts_.text(), true)) return false;
//Whitespace glyphs have 0 height - sadly, there is no other easily
// accessible data in a glyph to indicate that it is whitespace.
bool glyph_is_empty = (0 == glyph.height());
//Collect all glyphs and angles for the centre line, in the case a
// marker position needs to be calculated later
if(has_marker_ && marker_placement_ptr
&& layout.shield_layout()
&& (line_index == centre_line_index))
{
marker_placement_ptr->add_angle(angle);
marker_placement_ptr->add_bbox(bbox);
}
//If the glyph is empty, don't draw it, and don't include it in the
// overall bounding box. This means that leading and trailing
// whitespace characters don't take up extra space where other
// symbols could be placed on a map.
if(!glyph_is_empty)
{
bboxes_all.push_back(std::move(bbox));
glyphs->emplace_back(glyph, pos, rot);
}
}
// See comment above
offset -= sign * line.height()/2;
}
if(marker_placement_ptr)
{
marker_placement_ptr->align_offset(align_offset);
}
}
if (upside_down_glyph_count > static_cast<unsigned>(layouts_.text().length() / 2))
{
if (orientation == UPRIGHT_AUTO)
{
// Try again with opposite orientation
begin.restore();
return single_line_placement(pp, real_orientation == UPRIGHT_RIGHT ? UPRIGHT_LEFT : UPRIGHT_RIGHT);
}
// upright==left_only or right_only and more than 50% of characters upside down => no placement
else if (orientation == UPRIGHT_LEFT_ONLY || orientation == UPRIGHT_RIGHT_ONLY)
{
return false;
}
}
//NOTE: Because of the glyph_is_empty check above, whitespace characters
// don't factor in to the bounding box. This way, whitespace used for
// layout adjustment purposes don't artificially increase the size of
// the bounding box and reduce the density of possible mapnik symbols.
box2d<double> overall_bbox;
bool overall_bbox_initialized = false;
for(box2d<double> const& bbox : bboxes_all)
{
detector_.insert(bbox, layouts_.text());
if(overall_bbox_initialized)
{
overall_bbox.expand_to_include(bbox);
}
else
{
overall_bbox = bbox;
}
}
//WI: loop over marker_placements and place the markers for each layout
if(has_marker_ && marker_placement_ptr )
{
pixel_position marker_pos;
double marker_cw_angle = 0.0;
marker_placement_ptr->find_marker_placement(scale_factor_, marker_pos, marker_cw_angle);
agg::trans_affine placement_tr;
placement_tr.rotate( marker_cw_angle );
add_marker(glyphs, marker_pos, true, placement_tr);
detector_.insert(overall_bbox, layouts_.text());
}
placements_.push_back(glyphs);
return true;
}
bool placement_finder::single_line_placement(vertex_cache &pp, text_upright_e orientation)
{
//
// IMPORTANT NOTE: See note about coordinate systems in find_point_placement()!
//
vertex_cache::scoped_state begin(pp);
text_upright_e real_orientation = simplify_upright(orientation, pp.angle());
glyph_positions_ptr glyphs = std::make_shared<glyph_positions>();
std::vector<box2d<double> > bboxes;
glyphs->reserve(layouts_.glyphs_count());
bboxes.reserve(layouts_.glyphs_count());
unsigned upside_down_glyph_count = 0;
for (auto const& layout_ptr : layouts_)
{
text_layout const& layout = *layout_ptr;
pixel_position align_offset = layout.alignment_offset();
pixel_position const& layout_displacement = layout.displacement();
double sign = (real_orientation == UPRIGHT_LEFT) ? -1 : 1;
double offset = layout_displacement.y + 0.5 * sign * layout.height();
for (auto const& line : layout)
{
// Only subtract half the line height here and half at the end because text is automatically
// centered on the line
offset -= sign * line.height()/2;
vertex_cache & off_pp = pp.get_offseted(offset, sign*layout.width());
vertex_cache::scoped_state off_state(off_pp); // TODO: Remove this when a clean implementation in vertex_cache::get_offseted is done
if (!off_pp.move(sign * layout.jalign_offset(line.width()) - align_offset.x)) return false;
double last_cluster_angle = 999;
int current_cluster = -1;
pixel_position cluster_offset;
double angle;
rotation rot;
double last_glyph_spacing = 0.;
for (auto const& glyph : line)
{
if (current_cluster != static_cast<int>(glyph.char_index))
{
if (!off_pp.move_to_distance(sign * (layout.cluster_width(current_cluster) + last_glyph_spacing)))
return false;
current_cluster = glyph.char_index;
last_glyph_spacing = glyph.format->character_spacing * scale_factor_;
// Only calculate new angle at the start of each cluster!
angle = normalize_angle(off_pp.angle(sign * layout.cluster_width(current_cluster)));
rot.init(angle);
if ((text_props_->max_char_angle_delta > 0) && (last_cluster_angle != 999) &&
std::fabs(normalize_angle(angle-last_cluster_angle)) > text_props_->max_char_angle_delta)
{
return false;
}
cluster_offset.clear();
last_cluster_angle = angle;
}
if (std::abs(angle) > M_PI/2) ++upside_down_glyph_count;
pixel_position pos = off_pp.current_position() + cluster_offset;
// Center the text on the line
double char_height = line.max_char_height();
pos.y = -pos.y - char_height/2.0*rot.cos;
pos.x = pos.x + char_height/2.0*rot.sin;
cluster_offset.x += rot.cos * glyph.advance();
cluster_offset.y -= rot.sin * glyph.advance();
box2d<double> bbox = get_bbox(layout, glyph, pos, rot);
if (collision(bbox, layouts_.text(), true)) return false;
bboxes.push_back(std::move(bbox));
glyphs->emplace_back(glyph, pos, rot);
}
// See comment above
offset -= sign * line.height()/2;
}
}
if (upside_down_glyph_count > static_cast<unsigned>(layouts_.text().length() / 2))
{
if (orientation == UPRIGHT_AUTO)
{
// Try again with opposite orientation
begin.restore();
return single_line_placement(pp, real_orientation == UPRIGHT_RIGHT ? UPRIGHT_LEFT : UPRIGHT_RIGHT);
}
// upright==left_only or right_only and more than 50% of characters upside down => no placement
else if (orientation == UPRIGHT_LEFT_ONLY || orientation == UPRIGHT_RIGHT_ONLY)
{
return false;
}
}
for (box2d<double> const& box : bboxes)
{
detector_.insert(box, layouts_.text());
}
placements_.push_back(glyphs);
return true;
}
/* ----------------------------------------------------------------------------
* Sets the widget's angle to a value (in radians), updating both
* the textbox and the circle's pointer.
*/
void angle_picker::set_angle_rads(float a) {
a = normalize_angle(a);
angle = a;
((textbox*) widgets["txt_angle"])->text = angle_to_str(a);
}
Pose& Pose::operator*= (const Pose& tr2) {
_t += _R*tr2._t;
_R.angle()+=tr2._R.angle();
_R.angle()=normalize_angle(_R.angle());
return *this;
}
void calculate_filteredSensorReadings(){
int i;
sensor_filteredSensorReadings.positionSensor.x = 0;
sensor_filteredSensorReadings.positionSensor.y = 0;
sensor_filteredSensorReadings.positionSensor.t = 0;
double positionSensor_t_X = 0;
double positionSensor_t_Y = 0;
sensor_filteredSensorReadings.positionSensor.compass_direction = 0;
double positionSensor_compass_direction_X = 0;
double positionSensor_compass_direction_Y = 0;
sensor_filteredSensorReadings.groundSensor = 0;
int sum_k = 0;
double obstacleSensor_front[READINGS_BUFFER_SIZE];
double obstacleSensor_left[READINGS_BUFFER_SIZE];
double obstacleSensor_right[READINGS_BUFFER_SIZE];
for(i = 0 ;i<READINGS_BUFFER_SIZE;i++){
int k = (i+1)/2;
k = (k==0) ? 1 : k;
sum_k += k;
//store for median value
obstacleSensor_front[i] = internal_values.sensor_sensorReadings_buffer[i].obstacleSensor.front;
obstacleSensor_left[i] = internal_values.sensor_sensorReadings_buffer[i].obstacleSensor.left;
obstacleSensor_right[i] = internal_values.sensor_sensorReadings_buffer[i].obstacleSensor.right;
//calculate sum for weighed avrage value
sensor_filteredSensorReadings.positionSensor.x += (double)k * internal_values.sensor_sensorReadings_buffer[i].positionSensor.x;
sensor_filteredSensorReadings.positionSensor.y += (double)k * internal_values.sensor_sensorReadings_buffer[i].positionSensor.y;
double val_t = internal_values.sensor_sensorReadings_buffer[i].positionSensor.t;
positionSensor_t_X += (double)k * cos(val_t);
positionSensor_t_Y += (double)k * sin(val_t);
//~ normalize_angle_to_positive(&val_t);
//~ sensor_filteredSensorReadings.positionSensor.t += (double)k * val_t;
double val_compass_direction = internal_values.sensor_sensorReadings_buffer[i].positionSensor.compass_direction;
positionSensor_compass_direction_X += (double)k * cos(val_compass_direction);
positionSensor_compass_direction_Y += (double)k * sin(val_compass_direction);
//~ normalize_angle_to_positive(&val_compass_direction);
//~ sensor_filteredSensorReadings.positionSensor.compass_direction += (double)k * val_compass_direction;
sensor_filteredSensorReadings.groundSensor += k * internal_values.sensor_sensorReadings_buffer[i].groundSensor;
}
//order for median value
order_double_array(obstacleSensor_front, READINGS_BUFFER_SIZE);
order_double_array(obstacleSensor_left, READINGS_BUFFER_SIZE);
order_double_array(obstacleSensor_right, READINGS_BUFFER_SIZE);
//get median value
if(READINGS_BUFFER_SIZE%2==0)
{
//avrage of 2 middle values
int index = READINGS_BUFFER_SIZE/2;
sensor_filteredSensorReadings.obstacleSensor.front = (obstacleSensor_front[index-1]+obstacleSensor_front[index])/2;
sensor_filteredSensorReadings.obstacleSensor.left = (obstacleSensor_left[index-1]+obstacleSensor_left[index])/2;
sensor_filteredSensorReadings.obstacleSensor.right = (obstacleSensor_right[index-1]+obstacleSensor_right[index])/2;
}
else
{
//just take the middle value
int index = (READINGS_BUFFER_SIZE-1)/2;
sensor_filteredSensorReadings.obstacleSensor.front = obstacleSensor_front[index];
sensor_filteredSensorReadings.obstacleSensor.left = obstacleSensor_left[index];
sensor_filteredSensorReadings.obstacleSensor.right = obstacleSensor_right[index];
}
//calculate the end avrage value
sensor_filteredSensorReadings.positionSensor.x = sensor_filteredSensorReadings.positionSensor.x / (double)sum_k;
sensor_filteredSensorReadings.positionSensor.y = sensor_filteredSensorReadings.positionSensor.y / (double)sum_k;
sensor_filteredSensorReadings.positionSensor.t = atan2(positionSensor_t_Y, positionSensor_t_X);
normalize_angle(&sensor_filteredSensorReadings.positionSensor.t);
sensor_filteredSensorReadings.positionSensor.compass_direction = atan2(positionSensor_compass_direction_Y, positionSensor_compass_direction_X);
normalize_angle(&sensor_filteredSensorReadings.positionSensor.compass_direction);
sensor_filteredSensorReadings.groundSensor = sensor_filteredSensorReadings.groundSensor / sum_k;
}
void kld_resample_particle(particle_pool_t* particle_pool, sampler_t* sampler, const map_t* map, const likelihood_field_t* lf, const ranger_data_t* ranger, const laser_param_t* laser_param, const sampler_param_t* param) {
particle_t* new_particles = particle_pool->old_particles;// new particle_t[particle_pool->max_particles];
real_t max_weight = -1;
int max_index = -1;
real_t min_weight = 1000000;
int min_index = -1;
real_t sum_weight = 0;
int _M = 0;
double _Mx = 10000;
int selected = 0;
int k = 0;
pose_t sum_pose;
sum_pose.x = sum_pose.y = sum_pose.th = 0;
memset(sampler->kld_bin, 0, sizeof(uint32_t) * sampler->kld_numBins);
calc_all_paritcles_weight(particle_pool, map, lf, ranger, laser_param, param);
if (param->sampling_method == SAMPLING_SYSTEMATIC) {
systemaitc_selection_init(particle_pool);
}
while (TRUE) {
draw_particle_with_random(particle_pool, map, param, new_particles + _M);
sum_weight += new_particles[_M].weight;
sum_pose.x += new_particles[_M].pose.x;
sum_pose.y += new_particles[_M].pose.y;
sum_pose.th += new_particles[_M].pose.th;
if (new_particles[_M].weight > max_weight) {
max_weight = new_particles[_M].weight;
max_index = _M;
}
if (new_particles[_M].weight < min_weight) {
min_weight = new_particles[_M].weight;
min_index = _M;
}
if (is_fall_into_empty_bin(sampler, map, new_particles + _M, param)) {
k++;
if (k > 1) {
_Mx = calc_Mx(param->kld_epsilon, param->kld_delta, k);
}
}
_M++;
if (((_M >= _Mx) && (_M > param->kld_min_particles)) || (_M >= param->kld_max_particles)) {
break;
}
}
particle_t* buf = particle_pool->particles;
particle_pool->particles = new_particles;
particle_pool->max_weight = max_weight;
particle_pool->max_index = max_index;
particle_pool->min_weight = min_weight;
particle_pool->min_index = min_index;
particle_pool->sum_weight = sum_weight;
particle_pool->old_particles = buf;
particle_pool->num_particles = _M;
particle_pool->mean_pose.x = sum_pose.x / _M;
particle_pool->mean_pose.y = sum_pose.y / _M;
particle_pool->mean_pose.th = normalize_angle(sum_pose.th / _M);
normalize_particle_weight(particle_pool);
}
void CellGrid::draw_active_water(Coord here) {
//Draw the water located at 'here' to water_surface
//...fancy!
CellBox cell = CellBox(*this, here, AIR, ROCK, INACTIVE_WATER, INACTIVE_WATER);
int exposed_count = cell.count(EXPOSED_WATER);
int inactive_count = cell.count(INACTIVE_WATER);
int air_count = cell.count(AIR);
//We draw either a bubble, or a wave.
bool draw_bubble = true;
//These are directions, for drawing waves.
//a1 and a2 indicate what the lines will be drawn through.
int a1 = -1, a2 = -1;
//'under' indicates what part is under water
int under = -1;
//Determine how to proceed
if (air_count == 3 && exposed_count == 0 && inactive_count == 1) {
//tower of water?
int d = cell.find(INACTIVE_WATER);
a1 = rotate_cc(d);
a2 = rotate_co(d);
under = d;
draw_bubble = false;
}
else if (air_count == 2 && exposed_count == 1 && inactive_count == 1) {
int e = cell.find(EXPOSED_WATER), i = cell.find(INACTIVE_WATER);
int ei_angle = normalize_angle(angle(e, i));
if (ei_angle == 4) {
//opposite
a1 = rotate_cc(i);
a2 = rotate_co(i);
under = i;
}
else if (ei_angle == 2 || ei_angle == 6) {
//adjacent
a1 = e;
under = i;
//a2 is i rotated away from a1
if (ei_angle == 6) {
a2 = rotate_cc(i);
}
else {
a2 = rotate_co(i);
}
}
else {
throw ei_angle;
}
draw_bubble = false;
}
else if (air_count == 1 && exposed_count == 2 && inactive_count == 1) {
int e1 = cell.find(EXPOSED_WATER, 0), e2 = cell.find(EXPOSED_WATER, 1);
if (normalize_angle(angle(e1, e2)) == 4) {
//We've got two exposeds opposite, with an inactive on one side
//put the line between the two inactives
a1 = e1;
a2 = e2;
draw_bubble = false;
under = cell.find(INACTIVE_WATER);
}
}
else if (air_count == 1 && exposed_count == 1 && inactive_count == 2) {
//similiar to above, except we want two inactives adjacent
int i1 = cell.find(INACTIVE_WATER, 0), i2 = cell.find(INACTIVE_WATER, 1);
int n = normalize_angle(angle(i1, i2));
if (n == 2 || n == 6) {
draw_bubble = false;
int e = cell.find(EXPOSED_WATER);
int between = opposite(cell.find_air());
a1 = e;
a2 = opposite(e);
under = between;
int direction = normalize_angle(angle(between, a2));
if (direction == 6) {
a2 = rotate_cc(a2);
}
else {
a2 = rotate_co(a2);
}
}
}
//Now do the drawing
int seed = (here.x << here.y) + (ticks/15); //used for RNG
const Uint32 surface_color = CellData::color(EXPOSED_WATER);
const Uint32 under_water = CellData::color(INACTIVE_WATER);
SDL_FillRect(water_surface, NULL, CellData::color(AIR));
if (draw_bubble) {
if (air_count == 4) {
//draw drop of water instead
seed = (here.x * 191) >> 3;
const int offset = (block_pixel_size/2) - 1;
const int radius = (block_pixel_size/3)-(seed % 5);
filledCircleColor(water_surface, offset, offset, radius, under_water);
circleColor(water_surface, offset, offset, radius, surface_color);
}
else {
//A few random foamy bubbles
if (inactive_count == 4) {
extern int32_t eo_absCalibratedEncoder_Acquire(EOabsCalibratedEncoder* o, int32_t position, uint8_t error_mask)
{
static const int16_t MAX_ENC_CHANGE = 7*ENCODER_QUANTIZATION;
if (!o->sign) return 0;
if (!error_mask)
{
position -= o->offset;
if (position < 0)
{
position += TICKS_PER_REVOLUTION;
}
else if (position >= TICKS_PER_REVOLUTION)
{
position -= TICKS_PER_REVOLUTION;
}
o->invalid_fault_cnt = 0;
o->timeout_fault_cnt = 0;
}
else
{
if (error_mask & 0x01)
{
if (o->invalid_fault_cnt > 50)
{
SET_BITS(o->state_mask, SM_INVALID_FAULT);
}
else
{
++o->invalid_fault_cnt;
}
}
else
{
o->invalid_fault_cnt = 0;
}
if (error_mask & 0x02)
{
if (o->timeout_fault_cnt > 50)
{
SET_BITS(o->state_mask, SM_TIMEOUT_FAULT);
}
else
{
++o->timeout_fault_cnt;
}
}
else
{
o->timeout_fault_cnt = 0;
}
}
if (o->state_mask & SM_NOT_INITIALIZED)
{
encoder_init(o, position, error_mask);
#ifndef USE_2FOC_FAST_ENCODER
o->velocity = 0;
#endif
return o->sign*o->distance;
}
if (!error_mask)
{
int32_t check = normalize_angle(position - o->position_last);
o->position_last = position;
//make it less sensible! (expecially because incremental encoders has very long steps after the transformation in ICUB degrees)
if (-MAX_ENC_CHANGE <= check && check <= MAX_ENC_CHANGE)
{
int32_t delta = normalize_angle(position - o->position_sure);
//if (delta || o->delta)
if (delta)
{
o->position_sure = position;
//int32_t inc = (o->delta + delta) >> 1;
o->delta = delta;
o->distance += delta;
//if (inc)
//{
// o->distance += inc;
//}
#ifndef USE_2FOC_FAST_ENCODER
//o->velocity = (7*o->velocity + o->sign*EMS_FREQUENCY_INT32*inc) >> 3;
o->velocity = (7*o->velocity + o->sign*EMS_FREQUENCY_INT32*delta) >> 3;
#endif
}
#ifndef USE_2FOC_FAST_ENCODER
else
{
short new_projectile(
world_point3d *origin,
short polygon_index,
world_point3d *_vector,
angle delta_theta, /* ±¶theta is added (in a circle) to the angle before firing */
short type,
short owner_index,
short owner_type,
short intended_target_index, /* can be NONE */
_fixed damage_scale)
{
struct projectile_definition *definition;
struct projectile_data *projectile;
short projectile_index;
type= adjust_projectile_type(origin, polygon_index, type, owner_index, owner_type, intended_target_index, damage_scale);
definition= get_projectile_definition(type);
for (projectile_index= 0, projectile= projectiles; projectile_index<MAXIMUM_PROJECTILES_PER_MAP;
++projectile_index, ++projectile)
{
if (SLOT_IS_FREE(projectile))
{
angle facing, elevation;
short object_index;
struct object_data *object;
facing= arctangent(_vector->x, _vector->y);
elevation= arctangent(isqrt(_vector->x*_vector->x+_vector->y*_vector->y), _vector->z);
if (delta_theta)
{
if (!(definition->flags&_no_horizontal_error)) facing= normalize_angle(facing+global_random()%(2*delta_theta)-delta_theta);
if (!(definition->flags&_no_vertical_error)) elevation= (definition->flags&_positive_vertical_error) ? normalize_angle(elevation+global_random()%delta_theta) :
normalize_angle(elevation+global_random()%(2*delta_theta)-delta_theta);
}
object_index= new_map_object3d(origin, polygon_index, definition->collection==NONE ? NONE : BUILD_DESCRIPTOR(definition->collection, definition->shape), facing);
if (object_index!=NONE)
{
object= get_object_data(object_index);
projectile->type= (definition->flags&_alien_projectile) ?
(alien_projectile_override==NONE ? type : alien_projectile_override) :
(human_projectile_override==NONE ? type : human_projectile_override);
projectile->object_index= object_index;
projectile->owner_index= owner_index;
projectile->target_index= intended_target_index;
projectile->owner_type= owner_type;
projectile->flags= 0;
projectile->gravity= 0;
projectile->ticks_since_last_contrail= projectile->contrail_count= 0;
projectile->elevation= elevation;
projectile->distance_travelled= 0;
projectile->damage_scale= damage_scale;
MARK_SLOT_AS_USED(projectile);
SET_OBJECT_OWNER(object, _object_is_projectile);
object->sound_pitch= definition->sound_pitch;
L_Call_Projectile_Created(projectile_index);
}
else
{
projectile_index= NONE;
}
break;
}
}
if (projectile_index==MAXIMUM_PROJECTILES_PER_MAP) projectile_index= NONE;
return projectile_index;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node");
ros::NodeHandle nh;
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
ros::Subscriber tag_sub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>
("filtered_pose", 10, tag_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
ros::Publisher set_vel_pub = nh.advertise<geometry_msgs::TwistStamped>
("mavros/setpoint_velocity/cmd_vel", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
("mavros/local_position/local", 10, position_cb);
//the setpoint publishing rate MUST be faster than 2Hz
float rate_hz = 100.0;
ros::Rate rate(rate_hz);
int publish_skip = int(rate_hz / 2); //Publish only every 2 seconds
int publish_idx = 0;
float avg_april_pose_x = 0.0;
float avg_april_pose_y = 0.0;
// wait for FCU connection
while(ros::ok() && current_state.connected){
ros::spinOnce();
rate.sleep();
}
geometry_msgs::TwistStamped twist_zero;
twist_zero.header.stamp = ros::Time::now();
twist_zero.header.frame_id = "fcu";
twist_zero.twist.linear.x = 0.0;
twist_zero.twist.linear.y = 0.0;
twist_zero.twist.linear.z = 0.0;
twist_zero.twist.angular.x = 0.0;
twist_zero.twist.angular.y = 0.0;
twist_zero.twist.angular.z = 0.0;
geometry_msgs::TwistStamped twist_pub = twist_zero;
//send a few setpoints before starting
while(current_state.mode != "OFFBOARD" && ros::ok()){
set_vel_pub.publish(twist_zero);
ros::spinOnce();
rate.sleep();
}
//Initial values for last error, derr
float last_error_x = 0.0;
float last_error_y = 0.0;
float last_error_z = 0.0;
float last_error_yaw = 0.0;
float derr_x = 0.0;
float derr_y = 0.0;
float derr_z = 0.0;
float derr_yaw = 0.0;
float curr_yaw = 0.0;
// Set reference / desired positions to current position ONCE when offboard enabled
geometry_msgs::PoseStamped des_position = current_position;
geometry_msgs::PoseStamped initial_position = current_position;
float des_yaw = getYaw(current_position.pose);
float initial_yaw = des_yaw;
ros::param::set("/x_init", -initial_position.pose.position.x);
ros::param::set("/y_init", -initial_position.pose.position.y);
ros::param::set("/z_init", initial_position.pose.position.z);
ros::param::set("/yaw_init", initial_yaw);
ROS_INFO("Offboard mode enabled! ");
ros::param::set("/offboard", 1); //
ros::Time time_begin = ros::Time::now();
while(ros::ok()){
// Handle control time
float time = (ros::Time::now()-time_begin).toSec();
// Load gains and flight path parameters
float kp, kd, kp_yaw, kd_yaw;
ros::param::getCached("/control_gains/p", kp);
ros::param::getCached("/control_gains/d", kd);
ros::param::getCached("/control_gains/p_yaw", kp_yaw);
ros::param::getCached("/control_gains/d_yaw", kd_yaw);
float max_xy_vel;
ros::param::getCached("/max_xy_vel", max_xy_vel);
float x_rel_setpoint, y_rel_setpoint, z_rel_setpoint, yaw_rel_setpoint;
ros::param::get("/x_rel_setpoint", x_rel_setpoint);
ros::param::get("/y_rel_setpoint", y_rel_setpoint);
ros::param::get("/z_rel_setpoint", z_rel_setpoint);
ros::param::getCached("/yaw_rel_setpoint", yaw_rel_setpoint);
//Convert from X=Right, Y=Forwards to X=Left, Y=Backwards (quad ros frame)
x_rel_setpoint = -x_rel_setpoint;
y_rel_setpoint = -y_rel_setpoint;
int land_now, zero_vel;
ros::param::getCached("/land_now", land_now);
ros::param::getCached("/zero_vel", zero_vel);
des_position.pose.position.x = initial_position.pose.position.x + x_rel_setpoint;
des_position.pose.position.y = initial_position.pose.position.y + y_rel_setpoint;
des_position.pose.position.z = initial_position.pose.position.z + z_rel_setpoint;
des_yaw = normalize_angle(normalize_angle(initial_yaw) + normalize_angle(yaw_rel_setpoint));
curr_yaw = getYaw(current_position.pose);
//P velocity controller to des_position setpoint
twist_pub = twist_zero;
float error_x = des_position.pose.position.x - current_position.pose.position.x;
float error_y = des_position.pose.position.y - current_position.pose.position.y;
float error_z = des_position.pose.position.z - current_position.pose.position.z;
float error_yaw = des_yaw - curr_yaw;
//low-pass filter derr_x over 5 steps
derr_x = ((4.0*derr_x + (error_x - last_error_x)*rate_hz) / 5.0);
derr_y = ((4.0*derr_y + (error_y - last_error_y)*rate_hz) / 5.0);
derr_z = ((4.0*derr_z + (error_z - last_error_z)*rate_hz) / 5.0);
derr_yaw = ((4.0*derr_yaw + (error_yaw - last_error_yaw)*rate_hz) / 5.0);
//Clamp x and y to +/- max_xy_vel
twist_pub.twist.linear.x = std::max(std::min(kp * error_x + kd * derr_x, max_xy_vel), -max_xy_vel);
twist_pub.twist.linear.y = std::max(std::min(kp * error_y + kd * derr_y, max_xy_vel), -max_xy_vel);
twist_pub.twist.linear.z = 0.5*(kp * error_z + kd * derr_z);
twist_pub.twist.angular.z = kp_yaw * error_yaw + kd_yaw * derr_yaw;
last_error_x = error_x;
last_error_y = error_y;
last_error_z = error_z;
last_error_yaw = error_yaw;
// Overwrite Z velocity if time to land or stop control
if(zero_vel > 0){twist_pub.twist.linear.z = 0.0;} // twist_zero;}
if(land_now > 0){
//twist_pub.twist.linear.x = 0.0;
//twist_pub.twist.linear.y = 0.0;
float land_z_vel;
ros::param::getCached("/land_z_vel", land_z_vel);
twist_pub.twist.linear.z = -land_z_vel;
}
// Actually publish velocity commands
set_vel_pub.publish(twist_pub);
//Print info at 2 hz
publish_idx++;
if(publish_idx % publish_skip == 0){
ROS_INFO("Time (s): %f", time);
//ROS_INFO("Cmd vel: vx:%f vy:%f vz:%f wz:%f", twist_pub.twist.linear.x, twist_pub.twist.linear.y, twist_pub.twist.linear.z, twist_pub.twist.angular.z);
ROS_INFO("Current Pos: x:%f y:%f z:%f yaw:%f", current_position.pose.position.x, current_position.pose.position.y, current_position.pose.position.z, curr_yaw);
ROS_INFO("Desired Pos: x:%f y:%f z:%f yaw:%f", des_position.pose.position.x, des_position.pose.position.y, des_position.pose.position.z, des_yaw);
}
ros::spinOnce();
rate.sleep();
}
return 0;
}
