diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp
index 02998d99..dd8d227e 100644
--- a/src/optimal_planner.cpp
+++ b/src/optimal_planner.cpp
@@ -759,7 +759,7 @@ void TebOptimalPlanner::AddEdgesVelocity()
       return; // if weight equals zero skip adding edges!
 
     int n = teb_.sizePoses();
-    switch (sfg_->robot.omni_type) {
+    switch (cfg_->robot.omni_type) {
       case 0:
         {
           Eigen::Matrix<double,1,1> information;
@@ -940,7 +940,7 @@ void TebOptimalPlanner::AddEdgesAcceleration()
   }
   else // holonomic robot
   {
-    switch (sfg_->robot.omni_type) {
+    switch (cfg_->robot.omni_type) {
       case 0:
         {
           Eigen::Matrix<double,1,1> information;
diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp
index 95500151..46b9bb00 100644
--- a/src/teb_local_planner_ros.cpp
+++ b/src/teb_local_planner_ros.cpp
@@ -872,7 +872,7 @@ void TebLocalPlannerROS::saturateVelocityHolonomic(double& vx, double& vy, doubl
       l = 0.0;
       break;
   }
-  scale = (l < 1.0) ? 1.0 : 1.0 / l;
+  double scale = (l < 1.0) ? 1.0 : 1.0 / l;
   vx *= scale;
   vy *= scale;
   omega *= scale;