amcl: Fixed the fix for #20 in amcl_node.cpp so it actually uses the …

…corrected model types.
ros-planning · Mar 18, 2013 · c5717c1 · c5717c1
1 parent 5e9c781
commit c5717c1
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 8 deletions.
diff --git a/amcl/src/amcl_node.cpp b/amcl/src/amcl_node.cpp
@@ -512,10 +512,7 @@ void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level)
   delete odom_;
   odom_ = new AMCLOdom();
   ROS_ASSERT(odom_);
-  if(odom_model_type_ == ODOM_MODEL_OMNI)
-    odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
-  else
-    odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);
+  odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
   // Laser
   delete laser_;
   laser_ = new AMCLLaser(max_beams_, map_);
@@ -641,10 +638,7 @@ AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)
   delete odom_;
   odom_ = new AMCLOdom();
   ROS_ASSERT(odom_);
-  if(odom_model_type_ == ODOM_MODEL_OMNI)
-    odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
-  else
-    odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);
+  odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
   // Laser
   delete laser_;
   laser_ = new AMCLLaser(max_beams_, map_);

diff --git a/amcl/src/sensors/amcl_odom.cpp b/amcl/src/sensors/amcl_odom.cpp
@@ -92,6 +92,21 @@ AMCLOdom::SetModelOmni(double alpha1,
   this->alpha5 = alpha5;
 }
 
+void
+AMCLOdom::SetModel( odom_model_t type,
+                    double alpha1,
+                    double alpha2,
+                    double alpha3,
+                    double alpha4,
+                    double alpha5 )
+{
+  this->model_type = type;
+  this->alpha1 = alpha1;
+  this->alpha2 = alpha2;
+  this->alpha3 = alpha3;
+  this->alpha4 = alpha4;
+  this->alpha5 = alpha5;
+}
 
 ////////////////////////////////////////////////////////////////////////////////
 // Apply the action model

diff --git a/amcl/src/sensors/amcl_odom.h b/amcl/src/sensors/amcl_odom.h
@@ -71,6 +71,13 @@ class AMCLOdom : public AMCLSensor
                             double alpha4,
                             double alpha5);
 
+  public: void SetModel( odom_model_t type,
+                         double alpha1,
+                         double alpha2,
+                         double alpha3,
+                         double alpha4,
+                         double alpha5 = 0 );
+
   // Update the filter based on the action model.  Returns true if the filter
   // has been updated.
   public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);