Skip to content
Browse files

missing files

  • Loading branch information...
1 parent e60d240 commit b73e86d986ee7e98b2d535ff0772c7d0539dc086 @rtv committed Dec 29, 2011
Showing with 839 additions and 0 deletions.
  1. +683 −0 examples/ctrl/idw.cc
  2. +156 −0 examples/ctrl/wander_accel.cc
View
683 examples/ctrl/idw.cc
@@ -0,0 +1,683 @@
+const unsigned int VCOUNT = 32;
+const unsigned int WCOUNT = 32;
+
+class Vec;
+Vec operator*( double s, const Vec& v) ;
+
+class Vec : public std::pair<double, double>
+{
+public:
+ Vec( double a=0, double b=0 )
+ {
+ first = a;
+ second = b;
+ }
+
+ double Dot( const Vec& v) const
+ { return( first*v.first + second*v.second); }
+
+ Vec operator-(const Vec& v) const
+ { return Vec( first-v.first, second-v.second); }
+
+ Vec operator+(const Vec& v ) const
+ { return Vec( first+v.first, second+v.second);}
+
+ Vec operator/( const double s ) const
+ { return Vec( first/s, second/s ); }
+
+ Vec operator*( double s )
+ { return Vec( s*first, s*second); }
+
+ bool operator<( const Vec& v )
+ { return( LengthSqr() < v.LengthSqr() ); }
+
+ double LengthSqr() const
+ { return Dot(*this); }
+
+ double Length() const
+ { return sqrt(LengthSqr()); }
+
+ Vec Normalize() const
+ { return( ( 1.0 / Length()) * *this ); }
+
+ std::string Str(void)
+ {
+ char buf[1024];
+ snprintf( buf, sizeof(buf), "[%.2f,%.2f]", first, second );
+ return std::string( buf );
+ }
+};
+
+Vec operator*( double s, const Vec& v)
+{ return Vec( s*v.first, s*v.second); }
+
+
+class Circle
+{
+public:
+ Vec c;
+ double r;
+
+ Circle( const Vec& c, double r ) : c(c), r(r)
+ {}
+
+ Circle() : c(0,0), r(0)
+ {}
+
+ void Draw()
+ {
+ glBegin( GL_LINE_LOOP );
+ for( double a=0; a<2.0*M_PI; a+=M_PI/32.0 )
+ glVertex2f( c.first + r*cos(a), c.second + r*sin(a) );
+ glEnd();
+ }
+};
+
+ int circle_circle_intersection(double x0, double y0, double r0,
+ double x1, double y1, double r1,
+ double *xi, double *yi,
+ double *xi_prime, double *yi_prime)
+ {
+ //double a, dx, dy, d, h, rx, ry;
+ //double x2, y2;
+
+ /* dx and dy are the vertical and horizontal distances between
+ * the circle centers.
+ */
+ const double dx = x1 - x0;
+ const double dy = y1 - y0;
+
+ /* Determine the straight-line distance between the centers. */
+ const double d = hypot(dx,dy);
+
+ /* Check for solvability. */
+ if (d > (r0 + r1))
+ {
+ /* no solution. circles do not intersect. */
+ return 0;
+ }
+ if (d < fabs(r0 - r1))
+ {
+ /* no solution. one circle is contained in the other */
+ return 0;
+ }
+
+ /* 'point 2' is the point where the line through the circle
+ * intersection points crosses the line between the circle
+ * centers.
+ */
+
+ /* Determine the distance from point 0 to point 2. */
+ const double a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;
+
+ /* Determine the coordinates of point 2. */
+ const double x2 = x0 + (dx * a/d);
+ const double y2 = y0 + (dy * a/d);
+
+ /* Determine the distance from point 2 to either of the
+ * intersection points.
+ */
+ const double h = sqrt((r0*r0) - (a*a));
+
+ /* Now determine the offsets of the intersection points from
+ * point 2.
+ */
+ const double rx = -dy * (h/d);
+ const double ry = dx * (h/d);
+
+ /* Determine the absolute intersection points. */
+ *xi = x2 + rx;
+ *xi_prime = x2 - rx;
+
+ *yi = y2 + ry;
+ *yi_prime = y2 - ry;
+
+ return 1;
+ }
+
+
+// args:
+//
+// int polySides = how many corners the polygon has
+// float polyX[] = horizontal coordinates of corners
+// float polyY[] = vertical coordinates of corners
+// float x, y = point to be tested
+//
+// (Globals are used in this example for purposes of speed. Change as
+// desired.)
+//
+// The function will return YES if the point x,y is inside the polygon, or
+// NO if it is not. If the point is exactly on the edge of the polygon,
+// then the function may return YES or NO.
+//
+// Note that division by zero is avoided because the division is protected
+// by the "if" clause which surrounds it.
+
+bool PointInPolygon( int polySides, float polyX[], float polyY[], float x, float y )
+{
+ int i, j=polySides-1 ;
+ bool oddNodes=false;
+
+ for (i=0; i<polySides; i++) {
+ if (((polyY[i]< y && polyY[j]>=y)
+ || (polyY[j]< y && polyY[i]>=y))
+ && (polyX[i]<=x || polyX[j]<=x)) {
+ oddNodes^=(polyX[i]+(y-polyY[i])/(polyY[j]-polyY[i])*(polyX[j]-polyX[i])<x); }
+ j=i; }
+
+ return oddNodes;
+}
+
+
+/*
+ The I-DWA scores for speed and turn rate
+ args:
+ v: current forward speed
+ vmax: maximum speed
+ w: current turn rate
+ wmax: maximum turn rate
+ roh: distance to goal point (distance error)
+ alpha: angle to goal point (heading error)
+ dist: distance to nearest obstacle after one timestep if we drive at this speed
+*/
+std::pair<double,double> IDWAScores( double v, double vmax,
+ double w, double wmax,
+ double roh, double alpha )
+{
+ // gains must be positive - values from the paper
+ const double k_v = 1.0; // distance error weight
+ const double k_alpha = 0.59; // angular error weight
+ const double k_roh = 3.0; // slow on goal approach weight
+
+ // I-DWA Equation 13
+ // ideal forward speed to arrive at the goal
+ const double vi = k_v * vmax * cos(alpha) * tanh( roh / k_roh );
+
+ // I-DWA Equation 14
+ // ideal turn speed to arrive at the goal
+ const double wi = k_alpha * alpha + vi * sin(alpha) / roh;
+
+ // I-DWA Equation 17
+ // weighted comparison of current speeds with the
+ // ideal speeds taking into account the distance from the goal point
+ // and the distance to obstacles.
+
+ const double vs = 1.0 - fabs(v-vi) / (2.0*vmax );
+ double ws = 1.0 - fabs(w-wi) / (2.0*wmax );
+
+ // printf( "w %.2f wi %.2f fabs(w-wi) %.2f wmax %.6f ws %.2f\n",
+ // w, wi, fabs(w-wi), wmax, ws );
+
+ // ws is going slighty below zero for some reason. Force it not to.
+ if( ws < 0.0 )
+ ws = 0.0;
+
+ assert( vs >= 0.0 );
+ assert( vs <= 1.0 );
+ assert( ws >= 0.0 );
+ assert( ws <= 1.0 );
+
+ return std::pair<double,double>( vs, ws );
+}
+
+
+// return weighted sum of scores, gains chosen to normalize [0..1]
+double ObjectiveFunc( double so, double go,
+ double sv, double gv,
+ double sw, double gw )
+{
+ // scores must be normalized to the range [0..1]
+ assert( so >= 0.0 );
+ assert( so <= 1.0 );
+ assert( sv >= 0.0 );
+ assert( sv <= 1.0 );
+ assert( sw >= 0.0 );
+ assert( sw <= 1.0 );
+
+ // gains must be positive and sum to 1.0
+ assert( go >= 0.0 );
+ assert( go <= 1.0 );
+ assert( gv >= 0.0 );
+ assert( gv <= 1.0 );
+ assert( gw >= 0.0 );
+ assert( gw <= 1.0 );
+ assert( fabs( (gv + gw + go ) - 1.0) < 0.0001 );
+
+ //printf( "sv %.2f sw %.2f\n", sv, sw );
+ return( gv * sv + gw * sw + go * so );
+}
+
+class Vel;
+
+bool PointsInsideRangerScan( const ModelRanger::Sensor& sensor, Vel& v );
+
+
+class Vel
+{
+public:
+ double v, w;
+ double score;
+
+ double score_speed;
+ double score_heading;
+ double score_obstacle;
+
+ bool admissible;
+
+ bool inwindow;
+
+ // distance to nearest obstacle
+ double obstacle_dist;
+
+ // offset of new pose after moving with velocity (v,w) for 1 timestep
+ double dx, dy;
+
+ // loction of nearest obstacle
+ double ox, oy;
+
+ // all the points that this trajectory intersects with the obstacle cloud
+ std::vector<Vec> cloud_hits;
+
+ Vel( double v, double w )
+ : v(v),
+ w( fabs(w) == 0 ? 0.01 : w ), // disallow zero rotation
+ score(0),
+ admissible(true),
+ obstacle_dist(0),
+ dx( interval * v * cos(interval*w) ),
+ dy( interval * v * sin(interval*w) ),
+ ox(0),
+ oy(0),
+ cloud_hits()
+ {
+ }
+
+ Vel()
+ : v(0),
+ w(0),
+ score(0),
+ admissible(true),
+ obstacle_dist(0),
+ ox(0),
+ oy(0),
+ cloud_hits()
+ {
+ }
+
+
+ void Evaluate( double goal_dist,
+ double goal_angle,
+ double vmax,
+ double wmax,
+ const std::vector<Circle>& cloud )
+ {
+ double radius = v / w;
+
+
+ // compute where the trajectory collides with the obstacle cloud
+ cloud_hits.clear();
+
+ Vec circle( 0, radius ); // center of circle
+
+ Vec hit1, hit2;
+ FOR_EACH( it, cloud )
+ if( circle_circle_intersection( 0, radius, fabs(radius),
+ it->c.first, it->c.second, fabs(it->r),
+ &hit1.first, &hit1.second,
+ &hit2.first, &hit2.second ))
+ {
+ cloud_hits.push_back( hit1 );
+ cloud_hits.push_back( hit2 );
+ }
+
+ // default value will be overridden if this trajectory hits an obstacle
+ obstacle_dist = obstacle_dist_max;
+
+ // if( fabs(w) > 0.01 )
+ {
+
+ if( cloud_hits.size() )
+ {
+ // find the closest cloud hit
+ Vec closest = *min_element( cloud_hits.begin(), cloud_hits.end() );
+
+ ox = closest.first;
+ oy = closest.second;
+
+ // find the distance along the circle until we hit the obstacle
+ Vec a( 0, -radius );
+ Vec b( closest.first, closest.second - radius );
+ double g = acos( a.Dot(b) / (a.Length() * b.Length() ));
+
+ //printf( "g %.2f r %.2f\n", g, radius );
+
+ obstacle_dist = std::min( obstacle_dist, fabs( g * radius ) );
+ //it->obstacle_dist = fabs( g*radius );
+
+ assert( obstacle_dist > 0.0 );
+ assert( obstacle_dist <= obstacle_dist_max );
+
+ // printf( "v %.2f w %.2f hit (%.2f,%.2f) hypot %.2f dist %.2f\n",
+ // it->v, it->w, closest.first, closest.second,
+ // hypot(closest.first, fabs(closest.second)), it->obstacle_dist );
+ }
+ }
+
+ // normalize the score
+ score_obstacle = obstacle_dist / obstacle_dist_max;
+
+ // printf( "(%.2f,%2.f) obstacle_dist %.2f max %.2f score %.2f\n",
+ // v, w, obstacle_dist, obstacle_dist_max, score_obstacle );
+
+ assert( score_obstacle >=0 );
+ assert( score_obstacle <= 1.0 );
+
+ std::pair<double,double> scores = IDWAScores( v, vmax,
+ w, wmax,
+ goal_dist,
+ goal_angle );
+
+ score_speed = scores.first;
+ score_heading = scores.second;
+
+ // values from the I-DWA paper
+ double dist_gain = 7.0/13.0;
+ double speed_gain = 3.0/13.0;
+ double turn_gain = 3.0/13.0;
+
+
+ score = ObjectiveFunc( score_obstacle, dist_gain,
+ score_speed, speed_gain,
+ score_heading, turn_gain );
+ }
+
+
+
+ void Print()
+ {
+ printf( "[v %.3f w %.3f @(%.2f,%.2f) admit %s obstacle %.2f @(%.2f,%.3f) score %.3f]\n",
+ v, w,
+ dx, dy,
+ admissible ? "T" : "F",
+ obstacle_dist,
+ ox, oy,
+ score );
+ }
+};
+
+// convert a ranger scan into a vector of line segments
+std::vector<Circle> ObstacleCloud( const ModelRanger::Sensor& sensor,
+ const double safety_radius )
+{
+ std::vector<Circle> cloud( sensor.sample_count );
+
+ for( unsigned int i=0; i<sensor.sample_count; i++ )
+ {
+ double r = sensor.ranges[i];
+ double a = sensor.bearings[i];
+
+ cloud[i].c.first = r * cos(a);
+ cloud[i].c.second = r * sin(a);
+ cloud[i].r = std::max( safety_radius, 0.7 * M_PI/sensor.sample_count * r );
+ }
+
+ return cloud;
+}
+
+// convert a ranger scan into a vector of line segments
+std::vector<Circle> Cloud( std::vector<double> ranges,
+ std::vector<double> bearings,
+ const double safety_radius )
+{
+ assert( ranges.size() == bearings.size() );
+
+ unsigned int num = ranges.size();
+
+ std::vector<Circle> cloud( num );
+
+ for( unsigned int i=0; i<num; ++i )
+ {
+ double r = ranges[i];
+ double a = bearings[i];
+
+ cloud[i].c.first = r * cos(a);
+ cloud[i].c.second = r * sin(a);
+ cloud[i].r = std::max( safety_radius, 0.7 * M_PI/num * r );
+ }
+
+ return cloud;
+}
+
+
+class VelSpace
+{
+public:
+ std::vector<Vel> vels;
+ unsigned int vcount, wcount;
+ Bounds vbounds, wbounds;
+ Bounds dvbounds, dwbounds;
+ std::vector<Circle> cloud;
+
+ Vel& GetVel( unsigned int v, unsigned int w )
+ { return vels[ v + w*wcount ]; }
+
+ const Vel& GetVel( unsigned int v, unsigned int w ) const
+ { return vels[ v + w*wcount ]; }
+
+ VelSpace( const unsigned int vcount,
+ const unsigned int wcount,
+ const Bounds& vbounds,
+ const Bounds& wbounds,
+ const Bounds& dvbounds,
+ const Bounds& dwbounds
+ ) :
+ vcount(vcount),
+ wcount(wcount),
+ vbounds(vbounds),
+ wbounds(wbounds),
+ dvbounds(dvbounds),
+ dwbounds(dwbounds),
+ cloud()
+ {
+ vels.resize( vcount * wcount );
+
+ for( unsigned int vi=0; vi<vcount; ++vi )
+ for( unsigned int wi=0; wi<wcount; ++wi )
+ {
+ Vel& vel = GetVel( vi, wi );
+
+ const double vrange = vbounds.max - vbounds.min;
+ const double wrange = wbounds.max - wbounds.min;
+
+ const double vincr = vrange / (double)vcount;
+ const double wincr = wrange / (double)wcount;
+
+ vel.v = vbounds.min + vi * vincr;
+ vel.w = wbounds.min + wi * wincr;
+
+ // disallow zero w
+ if( fabs(vel.w) < 0.001 )
+ vel.w = 0.001;
+ }
+ }
+
+
+
+
+ Vel& Best( double x, double y, double a,
+ double v, double w,
+ double r, double b,
+ const std::vector<double>& ranges,
+ const std::vector<double>& bearings,
+ const double radius )
+ {
+ // range of achievable speeds at the nexttimestep is the dynamic window
+ const double v1max = std::min( v + dvbounds.max * interval, vbounds.max );
+ const double v1min = std::max( v + dvbounds.min * interval, vbounds.min );
+
+ const double w1max = std::min( w + dwbounds.max * interval, wbounds.max );
+ const double w1min = std::max( w + dwbounds.min * interval, wbounds.min );
+
+ // printf( "Reachable bounds (%.3f to %.3f) (%.3f %.3f)\n",
+ // v1min, v1max, w1min, w1max );
+
+ //
+ cloud = Cloud( ranges, bearings, radius );
+
+ unsigned int best_index = 0;
+ double best_score = 0.0;
+
+ // evaluate each candidate (v,w) pair
+ FOR_EACH( it, vels )
+ {
+ it->Evaluate( r, b, vbounds.max, wbounds.max, cloud);
+
+ if( it->v <= v1max &&
+ it->v >= v1min &&
+ it->w <= w1max &&
+ it->w >= w1min )
+ {
+ it->inwindow = true;
+ }
+ else
+ it->inwindow = false;
+
+ if( vels[i].inwindow && (vels[i].score > best_score) )
+ {
+ best_score = vels[i].score;
+ best_index = i;
+ }
+ }
+ return vels[best_index];
+ }
+
+
+
+ void Draw()
+ {
+ glPointSize( 8.0 );
+
+ glPushMatrix();
+
+ glLoadIdentity();
+ glScalef( 0.1, 0.1, 0 );
+
+ // glRotatef( -rtod(mod->GetGlobalPose().a), 0,0,1 );
+
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+
+ for( unsigned int v=0; v<vcount; ++v )
+ for( unsigned int w=0; w<vcount; ++w )
+ {
+ const Vel& vel = GetVel( v, w );
+
+ Color c = vel.inwindow ? Color( vel.score,0,0 ) : Color( 0,0,vel.score );
+ c.a = 0.5;
+ c.GLSet();
+ glRectf( v, w, v+1, w+1 );
+ }
+ glPopMatrix();
+
+ // draw the obstacle cloud
+ glColor3f( 0, 1, 0 );
+ FOR_EACH( it, cloud )
+ it->Draw();
+
+ // draw each trajectory as a faint circle
+ // glColor4f( 0,0,0,0.2 );
+ // const double steps = 100;
+ // FOR_EACH( it, vels )
+ // {
+ // glBegin( GL_LINE_LOOP );
+ // double radius = it->v / it->w;
+ // double a = (2.0 * M_PI) / steps;
+ // for( int i=0; i<steps; i++ )
+ // glVertex2f( fabs(radius) * cos( i*a ), radius + fabs(radius) * sin( i*a ) );
+ // glEnd();
+ // }
+
+ // draw the cloud hits
+ const double e=0.02;
+
+ glColor4f( 0,0,1, 0.5 );
+
+ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+ glBegin( GL_QUADS );
+ FOR_EACH( it, vels )
+ {
+ // printf( "cloud huts len %d\n", (int)it->cloud_hits.size() );
+ //if( it->inwindow )
+ FOR_EACH( it2, it->cloud_hits )
+ {
+ glVertex2f( it2->first-e, it2->second-e );
+ glVertex2f( it2->first+e, it2->second-e );
+ glVertex2f( it2->first+e, it2->second+e );
+ glVertex2f( it2->first-e, it2->second+e );
+ }
+ }
+ glEnd();
+
+ FOR_EACH( it, vels )
+ {
+ if( it->inwindow )
+ glColor4f( 0,0,0,0.8 ); // dark
+ else
+ glColor4f( 0,0,0,0.15 ); // light
+
+
+ {
+ // if( rob.best_vel == &*it )
+ // {
+ // //glLineWidth(3);
+ // }
+ // else
+ // {
+ // //glLineWidth(1);
+ // }
+
+ // show arcs until obstacles are hit
+
+ const double radius = it->v / it->w;
+ const double dist = it->obstacle_dist;
+ if( dist < 50 )
+ {
+ // sanity check
+ glBegin( GL_LINE_STRIP );
+ for( double d=0; d<fabs(dist); d+=0.05 )
+ {
+ const double w = M_PI - d/radius;
+ const double x = radius * sin(w);
+ const double y = radius + radius * cos(w);
+
+ if( fabs(x) < 100 && fabs(y)<100 )
+ glVertex2f( x, y );
+ }
+ glEnd();
+ }
+ else
+ {
+ //printf( "LONG v %.2f w %.2f dist %.2f\n", it->v, it->w, dist );
+ }
+ }
+ }
+
+ // glLineWidth( 1.0 );
+
+
+ // indicate the closest cloud hit
+ // glColor3f( 1,0,0 );
+ // glBegin( GL_LINES );
+ // FOR_EACH( it, rob.vspace )
+ // {
+ // if( it->cloud_hits.size() )
+ // {
+ // glVertex2f( 0,0 );
+ // glVertex2f( it->ox, it->oy );
+ // }
+ // }
+ // glEnd();
+
+ glPopMatrix();
+ }
+}; // class VelSpace
View
156 examples/ctrl/wander_accel.cc
@@ -0,0 +1,156 @@
+#include "stage.hh"
+using namespace Stg;
+
+static const double cruisespeed = 0.4;
+static const double avoidspeed = 0.05;
+static const double avoidturn = 0.5;
+static const double minfrontdistance = 1.0; // 0.6
+static const bool verbose = false;
+static const double stopdist = 0.3;
+static const int avoidduration = 10;
+
+typedef struct
+{
+ ModelPosition* pos;
+ ModelRanger* laser;
+ int avoidcount, randcount;
+} robot_t;
+
+int LaserUpdate( Model* mod, robot_t* robot );
+int PositionUpdate( Model* mod, robot_t* robot );
+
+// Stage calls this when the model starts up
+extern "C" int Init( Model* mod, CtrlArgs* args )
+{
+ // local arguments
+ /* printf( "\nWander controller initialised with:\n"
+ "\tworldfile string \"%s\"\n"
+ "\tcmdline string \"%s\"",
+ args->worldfile.c_str(),
+ args->cmdline.c_str() );
+ */
+
+ robot_t* robot = new robot_t;
+
+ robot->avoidcount = 0;
+ robot->randcount = 0;
+
+ robot->pos = (ModelPosition*)mod;
+
+ if( verbose )
+ robot->pos->AddCallback( Model::CB_UPDATE, (model_callback_t)PositionUpdate, robot );
+
+ robot->pos->Subscribe(); // starts the position updates
+
+ robot->laser = (ModelRanger*)mod->GetChild( "ranger:1" );
+ robot->laser->AddCallback( Model::CB_UPDATE, (model_callback_t)LaserUpdate, robot );
+ robot->laser->Subscribe(); // starts the ranger updates
+
+ return 0; //ok
+}
+
+
+// inspect the ranger data and decide what to do
+int LaserUpdate( Model* mod, robot_t* robot )
+{
+ // get the data
+ const std::vector<meters_t>& scan = robot->laser->GetRanges();
+ uint32_t sample_count = scan.size();
+ if( sample_count < 1 )
+ return 0;
+
+ bool obstruction = false;
+ bool stop = false;
+
+ // find the closest distance to the left and right and check if
+ // there's anything in front
+ double minleft = 1e6;
+ double minright = 1e6;
+
+ for (uint32_t i = 0; i < sample_count; i++)
+ {
+
+ if( verbose ) printf( "%.3f ", scan[i] );
+
+ if( (i > (sample_count/3))
+ && (i < (sample_count - (sample_count/3)))
+ && scan[i] < minfrontdistance)
+ {
+ if( verbose ) puts( " obstruction!" );
+ obstruction = true;
+ }
+
+ if( scan[i] < stopdist )
+ {
+ if( verbose ) puts( " stopping!" );
+ stop = true;
+ }
+
+ if( i > sample_count/2 )
+ minleft = std::min( minleft, scan[i] );
+ else
+ minright = std::min( minright, scan[i] );
+ }
+
+ if( verbose )
+ {
+ puts( "" );
+ printf( "minleft %.3f \n", minleft );
+ printf( "minright %.3f\n ", minright );
+ }
+
+ if( obstruction || stop || (robot->avoidcount>0) )
+ {
+ if( verbose ) printf( "Avoid %d\n", robot->avoidcount );
+
+ robot->pos->SetXSpeed( stop ? 0.0 : avoidspeed );
+
+ /* once we start avoiding, select a turn direction and stick
+ with it for a few iterations */
+ if( robot->avoidcount < 1 )
+ {
+ if( verbose ) puts( "Avoid START" );
+ robot->avoidcount = random() % avoidduration + avoidduration;
+
+ if( minleft < minright )
+ {
+ robot->pos->SetTurnSpeed( -avoidturn );
+ if( verbose ) printf( "turning right %.2f\n", -avoidturn );
+ }
+ else
+ {
+ robot->pos->SetTurnSpeed( +avoidturn );
+ if( verbose ) printf( "turning left %2f\n", +avoidturn );
+ }
+ }
+
+ robot->avoidcount--;
+ }
+ else
+ {
+ if( verbose ) puts( "Cruise" );
+
+ robot->avoidcount = 0;
+ robot->pos->SetXSpeed( cruisespeed );
+ robot->pos->SetTurnSpeed( 0 );
+ }
+
+ // if( robot->pos->Stalled() )
+ // {
+ // robot->pos->SetSpeed( 0,0,0 );
+ // robot->pos->SetTurnSpeed( 0 );
+ // }
+
+ return 0; // run again
+}
+
+int PositionUpdate( Model* mod, robot_t* robot )
+{
+ Pose pose = robot->pos->GetPose();
+
+ printf( "Pose: [%.2f %.2f %.2f %.2f]\n",
+ pose.x, pose.y, pose.z, pose.a );
+
+ return 0; // run again
+}
+

0 comments on commit b73e86d

Please sign in to comment.
Something went wrong with that request. Please try again.