File tree Expand file tree Collapse file tree 1 file changed +6
-4
lines changed 
nav2_mppi_controller/include/nav2_mppi_controller Expand file tree Collapse file tree 1 file changed +6
-4
lines changed Original file line number Diff line number Diff line change @@ -89,13 +89,15 @@ class MotionModel
8989        0 ).select (state.vx .col (i - 1 ) + max_delta_vx,
9090        state.vx .col (i - 1 ) - min_delta_vx);
9191
92-       state.vx .col (i) = state.cvx .col (i - 1 )
92+       state.cvx .col (i -  1 ) = state.cvx .col (i - 1 )
9393        .cwiseMax (lower_bound_vx)
9494        .cwiseMin (upper_bound_vx);
95+       state.vx .col (i) = state.cvx .col (i - 1 );
9596
96-       state.wz .col (i) = state.cwz .col (i - 1 )
97+       state.cwz .col (i -  1 ) = state.cwz .col (i - 1 )
9798        .cwiseMax (state.wz .col (i - 1 ) - max_delta_wz)
9899        .cwiseMin (state.wz .col (i - 1 ) + max_delta_wz);
100+       state.wz .col (i) = state.cwz .col (i - 1 );
99101
100102      if  (is_holo) {
101103        auto  lower_bound_vy = (state.vy .col (i - 1 ) >
@@ -104,10 +106,10 @@ class MotionModel
104106        auto  upper_bound_vy = (state.vy .col (i - 1 ) >
105107          0 ).select (state.vy .col (i - 1 ) + max_delta_vy,
106108          state.vy .col (i - 1 ) - min_delta_vy);
107- 
108-         state.vy .col (i) = state.cvy .col (i - 1 )
109+         state.cvy .col (i - 1 ) = state.cvy .col (i - 1 )
109110          .cwiseMax (lower_bound_vy)
110111          .cwiseMin (upper_bound_vy);
112+         state.vy .col (i) = state.cvy .col (i - 1 );
111113      }
112114    }
113115  }
 
 
   
 
     
   
   
          
    
    
     
    
      
     
     
    You can’t perform that action at this time.
  
 
    
  
    
      
        
     
       
      
     
   
 
    
    
  
 
  
 
     
    
0 commit comments