extended (unfinished)

core/localization/LocalizationModule.cpp

@@ -109,7 +109,8 @@ void LocalizationModule::processFrame() {
   self.vx = speed.translation[0];
   self.vy = speed.translation[1];
   self.vth = speed.rotation;
-  bool stopped = (self.vx == 0 and self.vy == 0 and self.vth == 0);
+  bool stopped = (self.vx == 0 and self.vy == 0);
+  bool stopped_th = (self.vth == 0);
 
   self.flying = flying;
   self.flying_inst = flying_inst;
@@ -130,7 +131,7 @@ void LocalizationModule::processFrame() {
       beacon_data.push_back( {object.visionDistance, object.visionBearing, beacon.second[0], beacon.second[1]} );
   }
 
-  pfilter_->processFrame(beacon_data, stopped, flying);
+  pfilter_->processFrame(beacon_data, stopped, stopped_th, flying);
 
   self.loc = pfilter_->pose().translation;
   self.orientation = pfilter_->pose().rotation;

core/localization/ParticleFilter.cpp

@@ -44,7 +44,7 @@ double calcGaussianLogProb(Matrix<double, D, 1> mu, Matrix<double, D, D> cov,
   return -0.5 * quadform - D * logSqrt2Pi - log(L.determinant());
 }
 
-void ParticleFilter::processFrame(vector<vector<float> > beacon_data, bool stopped, bool flying) {
+void ParticleFilter::processFrame(vector<vector<float> > beacon_data, bool stopped, bool stopped_th, bool flying) {
   // Indicate that the cached mean needs to be updated
   dirty_ = true;
   auto start_time = get_time();
@@ -66,9 +66,11 @@ void ParticleFilter::processFrame(vector<vector<float> > beacon_data, bool stopp
     p.y = p.y + dx * sn + dy * cs;
     p.t = normAngle(p.t + dth);
 
-    if(!stopped or true) {
+    if(!stopped) {
       p.x = p.x + Random::inst().sampleN(0.f, MOTION_ERR);
       p.y = p.y + Random::inst().sampleN(0.f, MOTION_ERR);
+    }
+    if(!stopped_th) {
       p.t = normAngle(p.t + Random::inst().sampleN(0.f, MOTION_ERR_TH));
     }
   }  
@@ -123,7 +125,8 @@ void ParticleFilter::processFrame(vector<vector<float> > beacon_data, bool stopp
   array<Particle, PARTICLE_NUM> new_particle;
 
   for(int m=0; m<M; m++){
-    double u = (r + m/(double)M) / 0.997;
+    double u = (r + m/(double)M);
+    if(!stopped) u /= 0.997;
     while(u > c and i < M-1){
       i = i + 1;
       c = c + P[i].w;

core/localization/ParticleFilter.h

@@ -26,7 +26,7 @@ class ParticleFilter {
   public:
     ParticleFilter(MemoryCache& cache, TextLogger*& tlogger);
     void init(Point2D loc, float orientation);
-    void processFrame(vector< vector<float> >, bool, bool);
+    void processFrame(vector< vector<float> >, bool, bool, bool);
     const Pose2D& pose() const;
     inline const std::array<Particle, PARTICLE_NUM>& particles() const {
       return cache_.localization_mem->particles;

core/python/behaviors/local2.py

@@ -0,0 +1,129 @@
+"""Sample behavior."""
+
+from __future__ import print_function
+from __future__ import division
+from __future__ import absolute_import
+
+import core
+import memory
+import pose
+import commands
+import cfgstiff
+import lights
+import math
+from task import Task
+from state_machine import Node, C, T, StateMachine
+import mem_objects
+import math
+
+def normAngle(x):
+  while x >= math.pi:
+    x = x - 2 * math.pi
+  while x < -math.pi: 
+    x = x + 2 * math.pi
+  return x
+
+
+class Ready(Task):
+    def run(self):
+        commands.standStraight()
+        if self.getTime() > 5.0:
+            memory.speech.say("ready to play")
+            self.finish()
+
+def clip(x, mx):
+    if x >= mx:
+        x = mx
+    if x <= -mx:
+        x = -mx
+    return x
+
+
+
+state = 0
+
+headth = 0.01
+dth = 0.02
+walk_time = 15
+
+start_time = 0
+
+class Playing(Task):
+    def run(self):
+        robot = memory.world_objects.getObjPtr(memory.robot_state.WO_SELF)
+        global headth, dth, state, start_time, start_time2, walk_time
+
+        if state == 0:
+            print('stage 0 : flying')
+        elif state == 1:
+            print('stage 1 : turn head')
+        elif state == 2:
+            print('stage 2 : turn')
+        elif state == 3:
+            print('stage 3 : walk %s s' % (self.getTime() - start_time))
+
+
+        if robot.flying:
+            state = 0
+
+        if state == 0:
+            if not robot.flying:
+                state = 1
+                headth = -1.8
+                dth = abs(dth)
+                start_time = self.getTime()
+                commands.setWalkVelocity(0., 0., 0.)
+
+        elif state == 1:
+            commands.setHeadPan(headth, 0.1)
+            headth = headth + dth * 2
+            if headth >= 1.8:
+                state = 2
+                start_time = self.getTime()
+
+        elif state == 2:
+            commands.setWalkVelocity(0, 0, 0.4)
+            if self.getTime() - start_time > 5.0:
+                state = 3
+                dth = -abs(dth)
+                start_time = self.getTime()
+
+                x = robot.loc.x
+                y = robot.loc.y
+                dis = math.sqrt(x*x+y*y)
+                if dis < 300:
+                    walk_time = 5.
+                else:
+                    walk_time = 15.
+
+        elif state == 3:
+            commands.setHeadPan(headth, 0.1)
+            headth = headth + dth
+            if headth > 1.8:
+                dth = -abs(dth)
+            if headth < -1.8:
+                dth = abs(dth)
+
+            x = robot.loc.x
+            y = robot.loc.y
+            th = robot.orientation
+
+            dis = math.sqrt(x*x+y*y)
+            vabs = min(0.3, dis)
+            vw = normAngle(math.atan2(-y,-x) - th)
+            if abs(vw) < 0.05:
+                vw = 0
+
+            vx = (+ (-x) * math.cos(th) + (-y) * math.sin(th)) / dis * vabs
+            vy = (- (-x) * math.sin(th) + (-y) * math.cos(th)) / dis * vabs
+
+            if dis > 100:
+                vw = clip(vw, 0.2)
+            else:
+                vw = 0.
+
+            commands.setWalkVelocity(vx, vy, vw)
+
+            if self.getTime() - start_time > walk_time*100:
+                state = 0
+                start_time = self.getTime()