int EFILE::domine2PBest(Particle&_a) {
std::vector<double> _box(nobjectives),_box2(nobjectives),_box3(nobjectives);
bool _flag=true;
//calculate the box of both particles
for (int _i = 0; _i < nobjectives; _i++){
if( EPS[_i]!=0) {
_box[_i] = (int) floor ((fabs(tlb[_i]-_a.fx[_i]) / EPS[_i]));
_box2[_i] = (int)floor ((fabs(tlb[_i]-_a.fxpbest[_i]) / EPS[_i]));
} else {
_box[_i]=0;
_box2[_i]=0;
}
//_box[_i] = (int) floor ((_a[_i] / EPS[_i]));
//_box2[_i] = (int)floor ((_b[_i] / EPS[_i]));
//_box3[_i] = (_box[_i]<_box2[_i])?_box[_i]*EPS[_i]:_box2[_i]*EPS[_i];//
_box3[_i] = (int)_box[_i]*EPS[_i];
//if they are in the same box
if(_box[_i]!=_box2[_i])_flag=false;
}
if(_flag==true){//check for dominance
int anterior = 0, mejor;
for(int _i=0;_i<nobjectives;_i++){
if(_a.fx[_i] <_a.fxpbest[_i]) mejor = 1;
else if(_a.fxpbest[_i]<_a.fx[_i])mejor = -1;
else mejor = 0;
if(mejor!=anterior&&anterior!=0&&mejor!=0){
if(euclideanDistance(_a.fx,_box3)<euclideanDistance(_a.fxpbest,_box3))
return 1;
else return -1; }
if(mejor!=0) anterior = mejor;
}
// if(anterior==1) return true;
//else return false;
return(anterior);
}
int anterior = 0, mejor;
for(int _i=0;_i<nobjectives;_i++){
if(_box[_i] <_box2[_i]) mejor = 1;
else if(_box2[_i]<_box[_i])mejor = -1;
else mejor = 0;
if(mejor!=anterior&&anterior!=0&&mejor!=0)return 11;
if(mejor!=0) anterior = mejor;
}
return(anterior);
}
template <typename PointSource, typename PointTarget, typename FeatureT> void
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::selectSamples (
const PointCloudSource &cloud, int nr_samples, float min_sample_distance,
std::vector<int> &sample_indices)
{
if (nr_samples > (int) cloud.points.size ())
{
PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%d)!\n",
nr_samples, (int) cloud.points.size ());
return;
}
// Iteratively draw random samples until nr_samples is reached
int iterations_without_a_sample = 0;
int max_iterations_without_a_sample = (int) (3 * cloud.points.size ());
sample_indices.clear ();
while ((int) sample_indices.size () < nr_samples)
{
// Choose a sample at random
int sample_index = getRandomIndex ((int) cloud.points.size ());
// Check to see if the sample is 1) unique and 2) far away from the other samples
bool valid_sample = true;
for (size_t i = 0; i < sample_indices.size (); ++i)
{
float distance_between_samples = euclideanDistance (cloud.points[sample_index], cloud.points[sample_indices[i]]);
if (sample_index == sample_indices[i] || distance_between_samples < min_sample_distance)
{
valid_sample = false;
break;
}
}
// If the sample is valid, add it to the output
if (valid_sample)
{
sample_indices.push_back (sample_index);
iterations_without_a_sample = 0;
}
else
{
++iterations_without_a_sample;
}
// If no valid samples can be found, relax the inter-sample distance requirements
if (iterations_without_a_sample >= max_iterations_without_a_sample)
{
PCL_WARN ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
PCL_WARN ("No valid sample found after %d iterations. Relaxing min_sample_distance_ to %f\n",
iterations_without_a_sample, 0.5*min_sample_distance);
min_sample_distance_ *= 0.5;
min_sample_distance = min_sample_distance_;
iterations_without_a_sample = 0;
}
}
}
static void updateErrors(Pose *referencePose, Pose *actualPose) {
error = euclideanDistance(referencePose, actualPose);
xError = fabs(referencePose->x - actualPose->x);
yError = fabs(referencePose->y - actualPose->y);
zError = fabs(referencePose->z - actualPose->z);
yawError = fabs(referencePose->yaw - actualPose->yaw);
}
void validationFase(int type, int num){
int i, *values = NULL;
if (!(values = (int*)malloc(2 * sizeof(int)))){
printf ("Erro ao alocar memória\n");
exit(1);
}
for(i = 0; i < num; i++){
euclideanDistance(i, values, g_data);
if (type == SEA_BASS){
if (g_outputMap[values[0]][values[1]] == '-' || g_outputMap[values[0]][values[1]] == 'R' )
g_outputMap[values[0]][values[1]] = 'R';
else
g_outputMap[values[0]][values[1]] = '*';
}else if (type == SALMON){
if (g_outputMap[values[0]][values[1]] == '-' || g_outputMap[values[0]][values[1]] == 'S')
g_outputMap[values[0]][values[1]] = 'S';
else
g_outputMap[values[0]][values[1]] = '*';
}
}
free(values);
for (i = 0; i < num; i++){
free(g_data[i]);
}
free(g_data);
}
void decideAcres(acre **acreArray, voronoiPointLinked *duchies)
{
voronoiPointLinked *tempPoint;
int favouredOwner, favouredDistance, newDistance ,i, j;
for(i = 0; i < acreArray[0]->totalSize; i++)
{
tempPoint = duchies;
favouredDistance = BIG_DISTANCE;
favouredOwner = 0;
j = 0;
while(tempPoint)
{
newDistance = euclideanDistance(acreArray[i]->middleX, acreArray[i]->middleY, tempPoint->x, tempPoint->y);
if(newDistance <= favouredDistance)//basically search for the closest by euclidean distance
{
favouredOwner = j;
favouredDistance = newDistance;
}
j++;
tempPoint = tempPoint->after;
}
acreArray[i]->duchyID = favouredOwner;
}
}
int numTowersInRange(double latitude, double longitude, double range){
int count = 0;
for (int i = 0 ; i < numTowers*2 ; i+=2){
if (euclideanDistance(latitude, longitude, towers[i], towers[i+1]) <= range)
count++;
}
return count;
}
//A method for collecting the points within a bounding circle.
void QTree::getPointsCircle(const BoundingBox bb, std::vector<Point>& points, std::vector<Point*>& res)
{
for(auto it = points.begin(); it != points.end(); it++)
{
if(euclideanDistance(bb.center, (*it)) <= bb.halfDim.x)
res.push_back(&(*it));
}
return;
}
void updateWorldModel() {
//TODO: current assumes all regions are the same size and resolution...this should be more flexible.
//TODO: this also ignores timestamps, assuming that we're just going to get a lot of data...
//
// First: construct a region wherein everything is drivable except the obstacles
std::cout << "Updating" << laneRegion.width << std::endl;
core_msgs::WorldRegion obsregion;
core_msgs::WorldRegion& baseRegion = gateRegion; //laneRegion;
if (baseRegion.width == 0) {
return;
}
std::cout << "I'm in" << std::endl;
obsregion.width = baseRegion.width;
obsregion.height = baseRegion.height;
obsregion.resolution = 100; // baseRegion.resolution;
int xres = 23;
int yres = 23;
obsregion.labels.assign(obsregion.width * obsregion.height, 1); //assume everything is drivable
for (std::vector<core_msgs::Obstacle>::iterator it = obstacles.begin();
it != obstacles.end();
it++) {
int centerX = (it->center.x * xres);
int centerY = -(it->center.y * yres);
int radius = it->radius * xres;
//std::cout << centerX << ", " << centerY << ", " << radius << std::endl;
for (uint row = 0; row < obsregion.height; ++row) {
for (uint col = 0; col < obsregion.width; ++col) {
int position = row * obsregion.width + col;
int x = obsregion.height - row;
int y = col - (obsregion.width / 2);
if (euclideanDistance(centerX, centerY, x, y) < radius) {
obsregion.labels[position] = 0;
}
}
}
}
// Next: merge all 3 regions together
core_msgs::WorldRegion merged;
merged.width = gateRegion.width;
merged.height = gateRegion.height;
merged.resolution = gateRegion.resolution;
merged.labels.assign(merged.height * merged.width, 0);
for (unsigned int inx = 0; inx < merged.width * merged.height; inx++) {
if (gateRegion.labels[inx] == 1
&& ((ros::Time::now() - lastReceivedLane) > ros::Duration(1.0) || laneRegion.labels[inx] == 1) //only consider lanes if received in the last second
&& ((ros::Time::now() - lastReceivedOther) > ros::Duration(1.0) || otherRegion.labels[inx] == 1) //only consider other region if received in the last second
&& obsregion.labels[inx] == 1) {
merged.labels[inx] = 1;
}
}
worldModel_pub.publish(merged);
}
/**
*@ classify unclassification sample
*/
string CTest_Knn::Classify(struct dataVector Sample,int k)
{
double dist=0;
int maxid=0,i,tmpfreq=1; //freq[k] store the number of each class apperaed in k-nearest nighbour
vector<int> freq;
distanceStruct gND;
string curClassLable = " ";
//memset(&freq,1,sizeof(freq)); //initialized freq[k] to 1
for(i=0; i<k; ++i)
{
freq.push_back(1);
}
//step.1---initalize the distance to MAX_VALUE
for(i=0;i<k;i++)
{
gND.distance = MAX_VALUE;
gNearestDistance.push_back(gND);
}
//step.2---calculate K-nearest neighbour distance
for(i=0;i<curTrainingSetSize;i++)
{
//step.2.1---calculate the distance between unclassification sample and each training sample
dist=euclideanDistance(gTrainingSet[i],Sample);
//step.2.2---get the max distance in gNearestDistance
maxid=GetMaxDistance(k);
//step.2.3---if the dist less than the maxdistance in gNearestDistance£¬it will be one of k-nearest neighbour
if(dist<gNearestDistance[maxid].distance)
{
gNearestDistance[maxid].ID=gTrainingSet[i].ID;
gNearestDistance[maxid].distance=dist;
gNearestDistance[maxid].classLabel=gTrainingSet[i].classLabel;
}
}
//step.3---statistics the number of each class appeared
for(i=0;i<k;i++)
{
for(int j=0;j<k;j++)
{
if((i!=j)&&(gNearestDistance[i].classLabel==gNearestDistance[j].classLabel))
{
freq[i]+=1;
}
}
}
//step.4---chose the class label with maximum frequency
for(i=0;i<k;i++)
{
if(freq[i]>tmpfreq)
{
tmpfreq=freq[i];
curClassLable=gNearestDistance[i].classLabel;
}
}
return curClassLable;
}
void printGraphOfPoints(point *points, int n, char *output) {
FILE *f = fopen(output, "w");
fprintf(f, "%d %d\n", n, n * (n - 1) / 2);
int i, j;
for (i = 0; i < n; ++i)
for (j = i + 1; j < n; ++j)
fprintf(f, "%d %d %lf\n", i, j, euclideanDistance(points[i], points[j]));
fclose(f);
}
static bool reached(Pose *referencePose, Pose *actualPose) {
if(euclideanDistance(referencePose, actualPose) < radius) {
if(yawEnabled) {
if(yawDistance(referencePose->yaw, actualPose->yaw) < maxYaw) {
return true;
}
return false;
}
return true;
}
return false;
}
bool upo_RRT::StateSpace::isSimpleGoalToleranceSatisfied(State* st, float &dist)
{
//float dx = goal->getX() - st->getX();
//float dy = goal->getY() - st->getY();
//dist = sqrt(pow(dx,2) + pow(dy,2));
//printf("StateSpace. px: %.2f, py: %.2f, gx: %.2f, gy:%.2f\n", st->getX(), st->getY(), goal_->getX(), goal_->getY());
dist = euclideanDistance(goal_, st);
if(dist <= goal_xy_tolerance_)
return true;
return false;
}
RayTraceIterRange rayTrace (const nm::MapMetaData& info, const gm::Point& p1, const gm::Point& p2,
bool project_onto_grid, bool project_source_onto_grid,
float max_range)
{
gm::Point np2 = p2;
if (max_range > 0) {
double distance = euclideanDistance(p1,p2);
if (distance > max_range) {
np2.x = ((p2.x - p1.x) * max_range/distance) + p1.x;
np2.y = ((p2.y - p1.y) * max_range/distance) + p1.y;
}
}
Cell c1 = pointCell(info, p1);
Cell c2 = pointCell(info, np2);
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Ray tracing between " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
const RayTraceIterator done(c1, c1, true);
const RayTraceIterRange empty_range(done, done);
if (!withinBounds(info, c1)) {
if (project_source_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c2, c1);
if (c)
c1 = *c;
else
return empty_range;
}
else
throw PointOutOfBoundsException(p1);
}
if (!withinBounds(info, np2)) {
if (project_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c1, c2);
if (c)
c2 = *c;
else
return empty_range;
}
else {
throw PointOutOfBoundsException(np2);
}
}
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Projected ray trace endpoints to " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
return rayTrace(c1, c2);
}
bool upo_RRT::StateSpace::isGoalToleranceSatisfied(State* st, float &dist)
{
dist = euclideanDistance(goal_, st);
if(dimensions_ > 2) {
float phi = st->getYaw() - goal_->getYaw();
//Normalize phi
phi = normalizeAngle(phi, -M_PI, M_PI);
if(dist <= goal_xy_tolerance_ && fabs(phi) <= goal_th_tolerance_)
return true;
}else {
if(dist <= goal_xy_tolerance_)
return true;
}
return false;
}
void colaborativeFase(){
int m, i, *values = NULL;
if (!(values = (int*)malloc(2 * sizeof(int)))){
printf ("Erro ao alocar memória\n");
exit(1);
}
for (m = 0; m < g_iteractions; m++){
for(i = 0; i < g_inputs; i++){
euclideanDistance(i, values, g_input);
updateNeighbors(values[0], values[1], i);
}
g_learning = g_startLearning * (1 - (m / g_iteractions));
}
free(values);
}
void EFILE::createProximityMatrix(){
int _i(0),_j(0);
for(int _k(0);_k<nsolutions-1;_k++,_i++){
proximitymatrix[_i][_i]=0;
_j=_i+1;
for(int _l(_k+1);_l<nsolutions;_l++,_j++){
proximitymatrix[_i][_j]=euclideanDistance(solutions[_k].nx,solutions[_l].nx);
}
pmsize=_i+1;
for(int _i(0);_i<pmsize;_i++){
proximitymatrixindex[_i][0]=_i;
for(int _j(1);_j<pmsize;_j++){
proximitymatrixindex[_i][_j]=-1;
}
}
}
}
//return the distance of each sample
double computeDistance(double* shapelet, int shapelet_len, double* data, int data_len){
//z-normal
double* shapelet_normalized;
shapelet_normalized = zNormal(shapelet, shapelet_len);
double min_dist = INFINITY;
for (int j=0; j<(data_len-shapelet_len+1); j++) {
//normalize the target comparing array
double* comp_shapelet = (double*) malloc(sizeof(double)*shapelet_len);
//create the subarray
for (int k=0; k<shapelet_len; k++) {
comp_shapelet[k] = data[j+k];
}
//znormal the subarray
double* norm_shapelet = zNormal(comp_shapelet, shapelet_len);
free(comp_shapelet);
//calcuate the euclidean distance
double dist = euclideanDistance(shapelet_normalized, norm_shapelet, shapelet_len);
if(CompareDoubles2(dist,min_dist)<0){
min_dist = dist;
}
free(norm_shapelet);
}
free(shapelet_normalized);
return (min_dist/(double)shapelet_len);
}
/**
* @function cropFace
* @brief Scales and rotates image, so that a image of the upright head of the size dstSize is returned.
*/
inline void cropFace(const cv::Mat& src, cv::Mat& dst, cv::Point& leftEye, cv::Point& rightEye, float offsetXPct, float offsetYPct, cv::Size& dstSize)
{
float offsetX = std::floor(offsetXPct * dstSize.width);
float offsetY = std::floor(offsetYPct * dstSize.height);
float rotation = std::atan2(float(rightEye.y - leftEye.y), float(rightEye.x - leftEye.x));
float rotationDeg = rotation * 180.0 / M_PI;
float dist = euclideanDistance(leftEye, rightEye);
float refDist = float(dstSize.width) - 2.0 * offsetX;
float scale = dist / refDist;
cv::Mat rotMat = cv::getRotationMatrix2D(leftEye, rotationDeg, 1.0);
cv::Mat dst2 = cv::Mat::zeros(src.rows, src.cols, src.type());
cv::warpAffine(src, dst2, rotMat, dst2.size());
float cropX = leftEye.x - scale * offsetX;
float cropY = leftEye.y - scale * offsetY;
float cropSizeWidth = float(dstSize.width) * scale;
float cropSizeHeight = float(dstSize.height) * scale;
cv::Rect roi(cropX, cropY, cropSizeWidth, cropSizeHeight);
if (roi.x + roi.width >= dst2.cols)
roi.width = dst2.cols - roi.x - 1;
if (roi.y + roi.height >= dst2.rows)
roi.height = dst2.rows - roi.y - 1;
if (roi.x < 0)
roi.x = 0;
if (roi.y < 0)
roi.y = 0;
cv::Rect insideROI = roi & cv::Rect(0, 0, dst2.cols, dst2.rows);
if(insideROI.area() > 0)
{
cv::Mat cropped = dst2(insideROI);
cv::resize(cropped, dst, dstSize);
}
else
{
cv::resize(dst2, dst, dstSize);
}
};
void Optics::getNeighbors(vector<Point>* pointsList, int pointIndex)
{
vector<Point*> * neighbors = new vector<Point*>;
Point * tempPoint;
double distCalc;
double coreDistance = EPS + 1;
// for every point
for(int i = 0; i < (int)pointsList->size(); i++)
{
if(pointIndex != i)
{
distCalc = euclideanDistance(pointsList->at(pointIndex).x, pointsList->at(pointIndex).y, pointsList->at(pointIndex).z, pointsList->at(i).x, pointsList->at(i).y, pointsList->at(i).z);
//cout << "distance: " << distCalc << endl;
if(distCalc <= EPS)
{
tempPoint = &(pointsList->at(i));
tempPoint->distanceToP = distCalc;
neighbors->push_back(tempPoint);
if(tempPoint->distanceToP < coreDistance)
{
coreDistance = tempPoint->distanceToP;
}
}
}
}
if((int)neighbors->size() >= MIN_PTS)
{
pointsList->at(pointIndex).coreDistance = coreDistance;
//cout << "core distance: " << coreDistance << endl;
}
pointsList->at(pointIndex).neighbors = neighbors;
}
// ACCESSORS
bool closeToEntries( const vec_type& v , precision cutoff )
{
// Go through each dimension and look for the one that
// satisfies the cutoff
for ( typename vec_type::size_type ii=0; ii<v.size(); ++ii ) {
DimensionEntries& dentries = table[ii];
typename DimensionEntries::iterator theend = dentries.end();
EDE lower;
lower.value = v[ii] - cutoff;
typename DimensionEntries::iterator begin = dentries.lower_bound( lower );
EDE upper;
upper.value = v[ii] + cutoff;
typename DimensionEntries::iterator end = dentries.upper_bound( upper );
if ( begin != theend && end != theend ) {
for(; begin!=end; ++begin) {
precision d = euclideanDistance( begin->entry.location().begin() ,
begin->entry.location().end() ,
v.begin() );
if ( d < cutoff + begin->entry.radius() ) { return true; }
}
}
}
return false;
}
typename Particle::precision
euclideanDistance( const Particle& p1 , const Particle& p2 )
{
return euclideanDistance( p1.X().begin() , p1.X().end() , p2.X().begin() );
}
void CutterSteering::steer()
{
if(!switchesgood_){
ROS_WARN("Steering is currently disabled. Make sure switch A is set to \"ITX cmd_vel\" and driving is enabled.");
publishVW(0,0);
return;
}
if (!got_state_)
{
ROS_WARN("Steering tried to steer, but hasn't got a state");
publishVW(0,0);
return;
}
if (!getFirstWayPoint())
{
ROS_WARN("Steering tried to steer, but has no more waypoints");
publishVW(0,0);
return;
}
//local declarations
double v, w;
double robotTheta, targetTheta, thetaDesired, thetaError;
//check distance to WayPt.
double targetDistance = euclideanDistance(map_pose_.position, target_waypoint_.pose.position);
double xDistance = target_waypoint_.pose.position.x - map_pose_.position.x;
double yDistance = target_waypoint_.pose.position.y - map_pose_.position.y;
// double brakingDistance = (last_cmd_vel_.linear.x/a_max_)*(last_v_/2.0); // (m/s)/(m/s/s)*m/s/2) = m/2
ROS_INFO("Robot (%.2f,%2f), Intitial position of robot was (%.2f,%.2f), distance to next waypoint (%.2f,%.2f): %f", map_pose_.position.x, map_pose_.position.y, initialX, initialY, target_waypoint_.pose.position.x, target_waypoint_.pose.position.y, targetDistance);
/*if (false)//TODO: Tom has no idea what this thing is. The velocity will be set to 5 if it is triggered, causing the robot to spiral out of control. (Originally the if statement contained "yDistance == 0 || xDistance ==0"
{
ROS_WARN("Starting up? xDistance or yDistance from robot to waypoint is 0!!");
targetTheta = 0;
}
else
{*/
targetTheta = atan2(yDistance * (target_waypoint_.direction - 2.0), xDistance * (target_waypoint_.direction - 2.0));
//}
robotTheta = tf::getYaw(map_pose_.orientation);
ROS_INFO("Angle to target: %f \t\t Angle of robot: %f", targetTheta, robotTheta);
//time check: older than .5s? then don't go so fast!
if(state_.header.stamp - ros::Time::now() > ros::Duration(.5)) //half a second
{
ROS_WARN("State is out of date!!");
w = 0;
v = 0;
}
else
{
//control:
//Are we there (x,y)?
if (targetDistance < target_waypoint_.distanceTol)
{
ROS_INFO("We made it to the waypoint.");
//don't move until a new waypoint is issued.
v = 0;
w = 0;
//call for new waypoint from path planner
waypoint_srv_.request.increment = true;
if(waypoint_client_.call(waypoint_srv_))
{
if (waypoint_srv_.response.pointsLeft > 0)
{
got_waypoint_ = true;
ROS_INFO("Got next waypoint");
last_waypoint_ = target_waypoint_;
target_waypoint_ = waypoint_srv_.response.currWayPoint;
next_waypoint_ = waypoint_srv_.response.nextWayPoint;
ROS_INFO("Updating initials");
initialX = last_waypoint_.pose.position.x;
initialY = last_waypoint_.pose.position.y;
initial_align = true;
}
else
{
ROS_INFO("No more waypoints");
got_waypoint_ = false;
}
}
else
{
ROS_WARN("Couldn't contact Path Planner's waypoint service");
got_waypoint_ = false;
}
}
else
{
//Are we not there yet?
//find desired heading given Pose and TargetWayPt
//find difference in Pose-heading and desired-heading
thetaDesired = targetTheta; // for now just want to aim at target (good for intial align
thetaError = angles::shortest_angular_distance(thetaDesired, robotTheta); // ros tf angles
ROS_INFO("found theta diff: %f", thetaError);
if(initial_align){//If the robot has not begun its jorney to the target, we want it to turn and face said target.
ROS_INFO("In initial rotational alignment stage.");
v = 0.0;
w = -K_wth_proportional_*thetaError;
ROS_INFO("Set w: %f", w);
initial_align = (fabs(thetaError) > 0.05);
}
else
{
double c = CutterSteering::distToLine(initialX, initialY, target_waypoint_.pose.position, map_pose_.position);
ROS_INFO("c = %f", c);
v = targetDistance*K_v_proportional_;
w = K_wc_proportional_ * c - K_wth_proportional_ * thetaError;
ROS_INFO("Progressing to target.");
if(slipped_){
ROS_WARN("Stalled!!!!!");
pointofstall_ = ros::Time::now().toSec();
ROS_INFO("Time of stall was %f.", pointofstall_);
slipped_ = false;
sliprecover_ = true;
}
if(sliprecover_){
w = 0.0;
v = -1.0 * v;
if(ros::Time::now().toSec() > pointofstall_ + stall_time_){
sliprecover_ = false;
}
}
}
}
//stay within limits
w = profile(w, last_cmd_vel_.angular.z, w_max_, w_a_max_);
v = profile(v * (target_waypoint_.direction - 2.0), last_cmd_vel_.linear.x, v_max_, v_a_max_);
ROS_INFO("profiled v: %f", v);
}
publishVW(v,w);
return;
}
bool obsDistanceCompare(const core_msgs::Obstacle& ob1, const core_msgs::Obstacle& ob2) {
return euclideanDistance(ob1.center) < euclideanDistance(ob2.center);
}
std::vector<Vec2> TMXPathFinding::getPath(Vec2 startPos, Vec2 endPos, std::vector<int> walkableGIDs, std::vector<int> obstacleGIDs) {
open.clear();
close.clear();
// inizialize a goal node
goalNode = new TileNode();
goalNode->setLocX(endPos.x);
goalNode->setLocY(endPos.y);
// inizialize a start node
startNode = new TileNode();
startNode->setLocX(startPos.x);
startNode->setLocY(startPos.y);
startNode->setCostFromStart(0);
startNode->setParent(nullptr);
int cost = euclideanDistance(startNode, goalNode);
startNode->setCostToGoal(cost);
startNode->setTotalCost();
open.emplace(startNode, startNode->getTotalCost());
while (open.size() != 0) {
// Fix a Cost to check the values
min = 32767; // std::numeric_limits<int>::max()
TileNode *minNode;
// Find minNode from open QUEUE
for (auto kv : open) {
extractNode = kv.first;
iCost = kv.second;
if (iCost < min) {
min = iCost; // Change min to the New Cost got from the open QUEUE
minNode = extractNode;
}
}
extractNode = minNode;
open.erase(minNode); // pop node from open
// if it's a goal, we're done
if (extractNode->getLocation() == goalNode->getLocation()) {
// 1- retrieve all extractNode's parents
// 2- save into Vec2 vector
// 3- reverse Vec2 vector
// 4- return Vec2 vector
std::vector<Vec2> points;
points.push_back(extractNode->getLocation());
int size = extractNode->getCostFromStart();
for (int i = 0; i < size; i++) {
points.push_back(Vec2(extractNode->getParent()->getLocation().x, extractNode->getParent()->getLocation().y));
extractNode = extractNode->getParent();
}
std::reverse(points.begin(), points.end());
return points;
}
else {
for (int i = 0; i < dir; i++) {
costToOpen = 0;
costToClose = 0;
inOpen = false;
inClose = false;
newNode = new TileNode();
newNode->setLocX(extractNode->getLocation().x);
newNode->setLocY(extractNode->getLocation().y);
switch (i) {
case 0: // left
newNode->setLocX(-1);
newNode->setLocY(0);
break;
case 1: // right
newNode->setLocX(1);
newNode->setLocY(0);
break;
case 2: // up
newNode->setLocX(0);
newNode->setLocY(1);
break;
case 3: // down
newNode->setLocX(0);
newNode->setLocY(-1);
break;
case 4: // top-left
newNode->setLocX(-1);
newNode->setLocY(1);
break;
case 5: // bottom-left
newNode->setLocX(-1);
newNode->setLocY(-1);
break;
case 6: // bottom-right
newNode->setLocX(1);
newNode->setLocY(-1);
break;
case 7: // top-right
newNode->setLocX(1);
newNode->setLocY(1);
break;
}
if (newNode->getLocation() != goalNode->getLocation()) {
if (newNode->getLocation().x < 0 || newNode->getLocation().y < 0 ||
newNode->getLocation().x >= tileMap->getMapSize().width ||
newNode->getLocation().y >= tileMap->getMapSize().height) {
// newNode is invalid, outside tileMap so ignore
continue;
}
bool isNeedToContinue = false;
// check newNode in given/all layers
for (auto layer : tileLayers) {
for (int gid : obstacleGIDs) {
if (layer->getTileGIDAt(newNode->getLocation()) == gid) {
// newNode is obstacle so ignore
isNeedToContinue = true;
break;
}
}
if (isNeedToContinue)
break;
if (walkableGIDs.size() > 0) {
isNeedToContinue = true;
for (int gid : walkableGIDs) {
if (layer->getTileGIDAt(newNode->getLocation()) == gid) {
// newNode is walkable
isNeedToContinue = false;
break;
}
}
if (isNeedToContinue) // newNode is not walkable so ignore
break;
}
}
if (isNeedToContinue)
continue;
}
cost = euclideanDistance(newNode, goalNode);
newNode->setCostFromStart(extractNode->getCostFromStart() + 1);
newNode->setCostToGoal(cost);
newNode->setParent(extractNode);
newNode->setTotalCost();
inOpen = false;
inClose = false;
for (auto kv : open) {
TileNode *node = kv.first;
if (newNode->getLocation() == node->getLocation()) {
costToOpen = node->getTotalCost();
inOpen = true;
break;
}
}
for (auto kv : close) {
TileNode *node = kv.first;
if (newNode->getLocation() == node->getLocation()) {
costToClose = node->getTotalCost();
inClose = true;
break;
}
}
if ((inOpen && (newNode->getTotalCost() >= costToOpen)) || (inClose && (newNode->getTotalCost() >= costToClose))) {
// newNode is already in open or close QUEUE with lower cost so ignore
continue;
}
else {
if (inClose) {
close.erase(newNode);
// reinsert newNode in open
open.emplace(newNode, newNode->getTotalCost());
}
if (inOpen) {
// adjust newNode's location in Open
iCost = costToOpen;
modifiedNode = newNode;
// remove from open
open.erase(newNode);
modifiedNode->setTotalCost(newNode->getTotalCost());
// updated node reinsert in open
open.emplace(modifiedNode, modifiedNode->getTotalCost());
}
else {
// if its neither in OPEN nor in CLOSE insert it in OPEN
open.emplace(newNode, newNode->getTotalCost());
}
}
}
}
close.emplace(extractNode, extractNode->getTotalCost());
}
std::vector<Vec2> dummy;
return dummy;
}
void ejecutarTrayectoria2()
{
float x, y, xFinal, yFinal, theta, thetaFinal;
float errorTheta = 0.008;
float errorDist = 0.01;
// turn 90 degrees on the robot
float v = 0;
float w = 1;
setSpeed(v,w);
// condicion de parada
theta = 0;
thetaFinal = (PI)/2;
while(abs(theta - thetaFinal) > errorTheta) {
AcquireMutex(semaphore_odometry);
x = robot_odometry.x;
y = robot_odometry.y;
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
nxtDisplayBigTextLine(2, "Tramo 1");
// generate 1st part of trayectory
v = 0.2;
w = 1.33333;
setSpeed(v,-w);
xFinal = 0.1061;
yFinal = 0.1434;
thetaFinal = degToRad(17);
while( euclideanDistance(x,xFinal,y,yFinal) > errorDist &&
abs(theta - thetaFinal) > errorTheta) {
AcquireMutex(semaphore_odometry);
x = robot_odometry.x;
y = robot_odometry.y;
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
nxtDisplayBigTextLine(2, "Tramo 2");
// generate 2nd part of trayectory
v = 0.2;
w = 0;
setSpeed(v,w);
errorDist = 0.05;
xFinal = 0.8711;
yFinal = 0.3773;
thetaFinal = degToRad(17);
while( euclideanDistance(x,xFinal,y,yFinal) > errorDist) {
nxtDisplayTextLine(3, "dist %2.2f", euclideanDistance(x,xFinal,y,yFinal));
nxtDisplayTextLine(4, "x,y: %2.2f %2.2f", x,y);
AcquireMutex(semaphore_odometry);
x = robot_odometry.x;
y = robot_odometry.y;
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
nxtDisplayBigTextLine(2, "Tramo 3");
// generate 3rd part of trayectory
v = 0.2;
w = 0.5;
setSpeed(v,-w);
errorDist = 0.01;
xFinal = 0.8711;
yFinal = -0.3877;
thetaFinal = normTheta(degToRad(-197));
while( euclideanDistance(x,xFinal,y,yFinal) > errorDist &&
abs(theta - thetaFinal) > errorTheta) {
AcquireMutex(semaphore_odometry);
x = robot_odometry.x;
y = robot_odometry.y;
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
nxtDisplayBigTextLine(2, "Tramo 4");
// generate 4th part of trayectory
v = 0.2;
w = 0;
setSpeed(v,w);
errorDist = 0.05;
xFinal = 0.1061;
yFinal = -0.1538;
thetaFinal = normTheta(degToRad(-197));
while( euclideanDistance(x,xFinal,y,yFinal) > errorDist) {
AcquireMutex(semaphore_odometry);
x = robot_odometry.x;
y = robot_odometry.y;
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
nxtDisplayBigTextLine(2, "Tramo 5");
// generate 5th part of trayectory
v = 0.2;
w = 1.33333;
setSpeed(v,-w);
errorDist = 0.01;
xFinal = 0;
yFinal = 0;
thetaFinal = (PI)/2;
while( euclideanDistance(x,xFinal,y,yFinal) > errorDist &&
abs(theta - thetaFinal) > errorTheta) {
AcquireMutex(semaphore_odometry);
x = robot_odometry.x;
y = robot_odometry.y;
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
nxtDisplayBigTextLine(2, "FIN");
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
double nan = std::numeric_limits<double>::quiet_NaN();
double inf = std::numeric_limits<double>::infinity();
if (nrhs == 0) {
mexPrintf("Lucas-Kanade\n");
return;
}
switch ((int) *mxGetPr(prhs[0])) {
// initialize or clean up
case 0: {
if (IMG!=0 && PYR!=0) {
for (int i = 0; i < MAX_IMG; i++) {
cvReleaseImage(&(IMG[i])); IMG[i] = 0;
cvReleaseImage(&(PYR[i])); PYR[i] = 0;
}
free(IMG); IMG = 0;
free(PYR); PYR = 0;
//mexPrintf("LK: deallocated\n");
}
IMG = (IplImage**) calloc(MAX_IMG,sizeof(IplImage*));
PYR = (IplImage**) calloc(MAX_IMG,sizeof(IplImage*));
//mexPrintf("LK: initialized\n");
return;
}
// tracking
case 2: {
if (IMG == 0 || (nrhs != 5 && nrhs != 6)) {
mexPrintf("lk(2,imgI,imgJ,ptsI,ptsJ,Level)\n");
// 0 1 2 3 4
return;
}
int Level;
if (nrhs == 6) {
Level = (int) *mxGetPr(prhs[5]);
} else {
Level = 5;
}
int I = 0;
int J = 1;
int Winsize = 10;
// Images
if (IMG[I] != 0) {
loadImageFromMatlab(prhs[1],IMG[I]);
} else {
CvSize imageSize = cvSize(mxGetN(prhs[1]),mxGetM(prhs[1]));
IMG[I] = cvCreateImage( imageSize, 8, 1 );
PYR[I] = cvCreateImage( imageSize, 8, 1 );
loadImageFromMatlab(prhs[1],IMG[I]);
}
if (IMG[J] != 0) {
loadImageFromMatlab(prhs[2],IMG[J]);
} else {
CvSize imageSize = cvSize(mxGetN(prhs[2]),mxGetM(prhs[2]));
IMG[J] = cvCreateImage( imageSize, 8, 1 );
PYR[J] = cvCreateImage( imageSize, 8, 1 );
loadImageFromMatlab(prhs[2],IMG[J]);
}
// Points
double *ptsI = mxGetPr(prhs[3]); int nPts = mxGetN(prhs[3]);
double *ptsJ = mxGetPr(prhs[4]);
if (nPts != mxGetN(prhs[4])) {
mexPrintf("Inconsistent input!\n");
return;
}
points[0] = (CvPoint2D32f*)cvAlloc(nPts*sizeof(CvPoint2D32f)); // template
points[1] = (CvPoint2D32f*)cvAlloc(nPts*sizeof(CvPoint2D32f)); // target
points[2] = (CvPoint2D32f*)cvAlloc(nPts*sizeof(CvPoint2D32f)); // forward-backward
for (int i = 0; i < nPts; i++) {
points[0][i].x = ptsI[2*i]; points[0][i].y = ptsI[2*i+1];
points[1][i].x = ptsJ[2*i]; points[1][i].y = ptsJ[2*i+1];
points[2][i].x = ptsI[2*i]; points[2][i].y = ptsI[2*i+1];
}
float *ncc = (float*) cvAlloc(nPts*sizeof(float));
float *ssd = (float*) cvAlloc(nPts*sizeof(float));
float *fb = (float*) cvAlloc(nPts*sizeof(float));
char *status = (char*) cvAlloc(nPts);
cvCalcOpticalFlowPyrLK( IMG[I], IMG[J], PYR[I], PYR[J], points[0], points[1], nPts, cvSize(win_size,win_size), Level, status, 0, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), CV_LKFLOW_INITIAL_GUESSES);
cvCalcOpticalFlowPyrLK( IMG[J], IMG[I], PYR[J], PYR[I], points[1], points[2], nPts, cvSize(win_size,win_size), Level, 0 , 0, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), CV_LKFLOW_INITIAL_GUESSES | CV_LKFLOW_PYR_A_READY | CV_LKFLOW_PYR_B_READY );
normCrossCorrelation(IMG[I],IMG[J],points[0],points[1],nPts, status, ncc, Winsize,CV_TM_CCOEFF_NORMED);
//normCrossCorrelation(IMG[I],IMG[J],points[0],points[1],nPts, status, ssd, Winsize,CV_TM_SQDIFF);
euclideanDistance( points[0],points[2],fb,nPts);
// Output
int M = 4;
plhs[0] = mxCreateDoubleMatrix(M, nPts, mxREAL);
double *output = mxGetPr(plhs[0]);
for (int i = 0; i < nPts; i++) {
if (status[i] == 1) {
output[M*i] = (double) points[1][i].x;
output[M*i+1] = (double) points[1][i].y;
output[M*i+2] = (double) fb[i];
output[M*i+3] = (double) ncc[i];
//output[M*i+4] = (double) ssd[i];
} else {
output[M*i] = nan;
output[M*i+1] = nan;
output[M*i+2] = nan;
output[M*i+3] = nan;
//output[M*i+4] = nan;
}
}
return;
}
}
}
template <typename PointInT, typename PointNT, typename PointOutT> float
pcl::GSS3DEstimation<PointInT, PointNT, PointOutT>::computeGeodesicDistance (size_t x0, size_t y0,
size_t x1, size_t y1)
{
// Based on the Bresenham's algorithm: http://en.wikipedia.org/wiki/Bresenham's_line_algorithm
bool steep = abs (static_cast<long> (y1 - y0)) > abs (static_cast<long> (x1 - x0));
size_t aux;
if (steep)
{
aux = x0;
x0 = y0;
y0 = aux;
aux = x1;
x1 = y1;
y1 = aux;
}
if (x0 > x1)
{
aux = x0;
x0 = x1;
x1 = aux;
aux = y0;
y0 = y1;
y1 = aux;
}
size_t delta_x = x1 - x0,
delta_y = abs (static_cast<long> (y1 - y0));
float error = 0.0f;
float delta_error = float (delta_y) / delta_x;
int ystep;
size_t y = y0;
if (y0 < y1)
ystep = 1;
else
ystep = -1;
PointInT previous_point, current_point;
if (steep)
current_point = surface_->at (y0, x0);
else
current_point = surface_->at (x0, y0);
float distance,
total_distance = 0.0f;
for (size_t x = x0; x <= x1; ++x)
{
if (steep)
{
previous_point = current_point;
current_point = surface_->at (y, x);
}
else
{
previous_point = current_point;
current_point = surface_->at (x, y);
}
distance = euclideanDistance (previous_point, current_point);
if (pcl_isnan (distance)) return (-1.0f);
total_distance += distance;
error += delta_error;
if (error >= 0.5f)
{
y += ystep;
error -= 1.0f;
}
}
return (total_distance);
}
// Lucas-Kanade
Eigen::Matrix<double, 4, 150> lk2(IplImage* imgI, IplImage* imgJ, Eigen::Matrix<double, 2,
150> const & pointsI, Eigen::Matrix<double, 2, 150> const & pointsJ,
unsigned int sizeI, unsigned int sizeJ, unsigned int level) {
double nan = std::numeric_limits<double>::quiet_NaN();
int Level;
if (level != 0) {
Level = (int) level;
} else {
Level = 5;
}
int I = 0;
int J = 1;
int Winsize = 10;
// Images
if (IMG[I] != 0) {
IMG[I] = imgI;
} else {
CvSize imageSize = cvGetSize(imgI);
IMG[I] = cvCreateImage(imageSize, 8, 1);
PYR[I] = cvCreateImage(imageSize, 8, 1);
IMG[I] = imgI;
}
if (IMG[J] != 0) {
IMG[J] = imgJ;
} else {
CvSize imageSize = cvGetSize(imgJ);
IMG[J] = cvCreateImage(imageSize, 8, 1);
PYR[J] = cvCreateImage(imageSize, 8, 1);
IMG[J] = imgJ;
}
// Points
int nPts = sizeI;
if (nPts != sizeJ) {
std::cout << "Inconsistent input!" << std::endl;
return Eigen::MatrixXd::Zero(1, 1);
}
points[0] = (CvPoint2D32f*) cvAlloc(nPts * sizeof(CvPoint2D32f)); // template
points[1] = (CvPoint2D32f*) cvAlloc(nPts * sizeof(CvPoint2D32f)); // target
points[2] = (CvPoint2D32f*) cvAlloc(nPts * sizeof(CvPoint2D32f)); // forward-backward
for (int i = 0; i < nPts; i++) {
points[0][i].x = pointsI(0, i);
points[0][i].y = pointsI(1, i);
points[1][i].x = pointsJ(0, i);
points[1][i].y = pointsJ(1, i);
points[2][i].x = pointsI(0, i);
points[2][i].y = pointsI(1, i);
}
float *ncc = (float*) cvAlloc(nPts * sizeof(float));
float *fb = (float*) cvAlloc(nPts * sizeof(float));
char *status = (char*) cvAlloc(nPts);
cvCalcOpticalFlowPyrLK(IMG[I], IMG[J], PYR[I], PYR[J], points[0],
points[1], nPts, cvSize(win_size, win_size), Level, status, 0,
cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03),
CV_LKFLOW_INITIAL_GUESSES);
cvCalcOpticalFlowPyrLK(IMG[J], IMG[I], PYR[J], PYR[I], points[1],
points[2], nPts, cvSize(win_size, win_size), Level, 0, 0,
cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03),
CV_LKFLOW_INITIAL_GUESSES | CV_LKFLOW_PYR_A_READY
| CV_LKFLOW_PYR_B_READY );
normCrossCorrelation(IMG[I], IMG[J], points[0], points[1], nPts, status,
ncc, Winsize, CV_TM_CCOEFF_NORMED);
euclideanDistance(points[0], points[2], fb, nPts);
// Output
int M = 4;
Eigen::MatrixXd output(M, 150);
for (int i = 0; i < nPts; i++) {
if (status[i] == 1) {
output(0, i) = (double) points[1][i].x;
output(1, i) = (double) points[1][i].y;
output(2, i) = (double) fb[i];
output(3, i) = (double) ncc[i];
} else {
output(0, i) = nan;
output(1, i) = nan;
output(2, i) = nan;
output(3, i) = nan;
}
}
return output;
}
/**
* Tracks Points from 1.Image to 2.Image.
* Need initImgs before start and at the end of the program for cleanup.
*
* @param imgI previous Image source. (isn't changed)
* @param imgJ actual Image target. (isn't changed)
* @param ptsI points to track from first Image.
* Format [0] = x1, [1] = y1, [2] = x2 ...
* @param nPtsI number of Points to track from first Image
* @param ptsJ container for calculated points of second Image.
* Must have the length of nPtsI.
* @param nPtsJ number of Points
* @param level Pyramidlevel, default 5
* @param fb forward-backward confidence value.
* (corresponds to euclidean distance between).
* Must have the length of nPtsI: nPtsI * sizeof(float).
* @param ncc normCrossCorrelation values. needs as inputlength nPtsI * sizeof(float)
* @param status Indicates positive tracks. 1 = PosTrack 0 = NegTrack
* needs as inputlength nPtsI * sizeof(char)
*
*
* Based Matlab function:
* lk(2,imgI,imgJ,ptsI,ptsJ,Level) (Level is optional)
*/
int trackLK(IplImage *imgI, IplImage *imgJ, float ptsI[], int nPtsI,
float ptsJ[], int nPtsJ, int level, float * fb, float*ncc, char*status)
{
//TODO: watch NaN cases
//double nan = std::numeric_limits<double>::quiet_NaN();
//double inf = std::numeric_limits<double>::infinity();
// tracking
int I, J, winsize_ncc;
CvSize pyr_sz;
int i;
//if unused std 5
if (level == -1)
{
level = 5;
}
I = 0;
J = 1;
winsize_ncc = 10;
//NOTE: initImgs() must be used correctly or memleak will follow.
pyr_sz = cvSize(imgI->width + 8, imgI->height / 3);
PYR[I] = cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1);
PYR[J] = cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1);
// Points
if (nPtsJ != nPtsI)
{
printf("Inconsistent input!\n");
return 0;
}
points[0] = (CvPoint2D32f*) malloc(nPtsI * sizeof(CvPoint2D32f)); // template
points[1] = (CvPoint2D32f*) malloc(nPtsI * sizeof(CvPoint2D32f)); // target
points[2] = (CvPoint2D32f*) malloc(nPtsI * sizeof(CvPoint2D32f)); // forward-backward
//TODO:Free
char* statusBacktrack = (char*) malloc(nPtsI);
for (i = 0; i < nPtsI; i++)
{
points[0][i].x = ptsI[2 * i];
points[0][i].y = ptsI[2 * i + 1];
points[1][i].x = ptsJ[2 * i];
points[1][i].y = ptsJ[2 * i + 1];
points[2][i].x = ptsI[2 * i];
points[2][i].y = ptsI[2 * i + 1];
}
//lucas kanade track
cvCalcOpticalFlowPyrLK(imgI, imgJ, PYR[I], PYR[J], points[0], points[1],
nPtsI, cvSize(win_size_lk, win_size_lk), level, status, 0, cvTermCriteria(
CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03),
CV_LKFLOW_INITIAL_GUESSES);
//backtrack
cvCalcOpticalFlowPyrLK(imgJ, imgI, PYR[J], PYR[I], points[1], points[2],
nPtsI, cvSize(win_size_lk, win_size_lk), level, statusBacktrack, 0, cvTermCriteria(
CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03),
CV_LKFLOW_INITIAL_GUESSES | CV_LKFLOW_PYR_A_READY | CV_LKFLOW_PYR_B_READY);
for (i = 0; i < nPtsI; i++)
{
if (status[i] && statusBacktrack[i])
{
status[i] = 1;
}else{
status[i] = 0;
}
}
normCrossCorrelation(imgI, imgJ, points[0], points[1], nPtsI, status, ncc,
winsize_ncc, CV_TM_CCOEFF_NORMED);
euclideanDistance(points[0], points[2], fb, nPtsI);
for (i = 0; i < nPtsI; i++)
{
if (status[i] == 1)
{
ptsJ[2 * i] = points[1][i].x;
ptsJ[2 * i + 1] = points[1][i].y;
}
else //flow for the corresponding feature hasn't been found
{
//Todo: shell realy write N_A_N in it?
ptsJ[2 * i] = N_A_N;
ptsJ[2 * i + 1] = N_A_N;
fb[i] = N_A_N;
ncc[i] = N_A_N;
}
}
for (i = 0; i < 3; i++)
{
free(points[i]);
points[i] = 0;
}
free(statusBacktrack);
return 1;
}
void ejecutarTrayectoria1()
{
float x, y, xFinal, yFinal, theta, thetaFinal;
float errorTheta = 0.005;
float errorDist = 0.005;
// turn 90 degrees on the robot
float v = 0;
float w = 1;
setSpeed(v,-w);
// condicion de parada
theta = (PI)/2;
thetaFinal = 0;
while(abs(theta - thetaFinal) > errorTheta) {
//nxtDisplayTextLine(3, "dist %2.2f", euclideanDistance(x,xFinal,y,yFinal));
//nxtDisplayTextLine(4, "Theta: %2.2f", abs(theta - thetaFinal));
AcquireMutex(semaphore_odometry);
x = robot_odometry.x;
y = robot_odometry.y;
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
nxtDisplayBigTextLine(2, "Tramo 1");
// generate 1st part of trayectory
v = 0.2;
w = 0.5;
setSpeed(v,w);
xFinal = 0;
yFinal = 0.8;
thetaFinal = (PI);
while( euclideanDistance(x,xFinal,y,yFinal) > errorDist &&
abs(theta - thetaFinal) > errorTheta) {
//nxtDisplayTextLine(3, "dist %2.2f", euclideanDistance(x,xFinal,y,yFinal));
//nxtDisplayTextLine(4, "Theta: %2.2f", abs(theta - thetaFinal));
AcquireMutex(semaphore_odometry);
x = robot_odometry.x;
y = robot_odometry.y;
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
nxtDisplayBigTextLine(2, "Tramo 2");
// generate 2nd part of trayectory
setSpeed(v,-w);
xFinal = 0;
yFinal = 1.6;
thetaFinal = 0;
while( euclideanDistance(x,xFinal,y,yFinal) > errorDist &&
abs(theta - thetaFinal) > errorTheta) {
//nxtDisplayTextLine(3, "dist %2.2f", euclideanDistance(x,xFinal,y,yFinal));
//nxtDisplayTextLine(4, "Theta: %2.2f", abs(theta - thetaFinal));
AcquireMutex(semaphore_odometry);
x = robot_odometry.x;
y = robot_odometry.y;
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
nxtDisplayBigTextLine(2, "Tramo 3");
// generate 3rd part of trayectory
setSpeed(v,-w);
xFinal = 0;
yFinal = 0.8;
thetaFinal = -(PI);
while( euclideanDistance(x,xFinal,y,yFinal) > errorDist &&
abs(theta - thetaFinal) > errorTheta) {
//nxtDisplayTextLine(3, "dist %2.2f", euclideanDistance(x,xFinal,y,yFinal));
//nxtDisplayTextLine(4, "Theta: %2.2f", abs(theta - thetaFinal));
AcquireMutex(semaphore_odometry);
x = robot_odometry.x;
y = robot_odometry.y;
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
nxtDisplayBigTextLine(2, "Tramo 4");
// generate 4th part of trayectory
setSpeed(v,w);
xFinal = 0;
yFinal = 0;
thetaFinal = 0;
while( euclideanDistance(x,xFinal,y,yFinal) > errorDist &&
abs(theta - thetaFinal) > errorTheta) {
//nxtDisplayTextLine(3, "dist %2.2f", euclideanDistance(x,xFinal,y,yFinal));
//nxtDisplayTextLine(4, "Theta: %2.2f", abs(theta - thetaFinal));
AcquireMutex(semaphore_odometry);
x = robot_odometry.x;
y = robot_odometry.y;
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
nxtDisplayBigTextLine(2, "FIN");
}
