git commit -m "first commit"

2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
--- a/navigations/dwb_plugins/CMakeLists.txt
+++ b/navigations/dwb_plugins/CMakeLists.txt
@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(dwb_plugins)
+
+set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11;-Wall;-Werror")
+
+find_package(catkin REQUIRED COMPONENTS angles dwb_local_planner dynamic_reconfigure nav_2d_msgs nav_2d_utils nav_core2 pluginlib roscpp)
+
+generate_dynamic_reconfigure_options(cfg/KinematicParams.cfg)
+
+catkin_package(
+    INCLUDE_DIRS include
+    CATKIN_DEPENDS angles dwb_local_planner dynamic_reconfigure nav_2d_msgs nav_2d_utils nav_core2 pluginlib roscpp
+    LIBRARIES simple_goal_checker stopped_goal_checker standard_traj_generator
+)
+
+include_directories(
+  include ${catkin_INCLUDE_DIRS}
+)
+
+add_library(simple_goal_checker src/simple_goal_checker.cpp)
+target_link_libraries(simple_goal_checker ${catkin_LIBRARIES})
+add_dependencies(simple_goal_checker ${catkin_EXPORTED_TARGETS} ${dwb_plugins_EXPORTED_TARGETS})
+
+add_library(stopped_goal_checker src/stopped_goal_checker.cpp)
+target_link_libraries(stopped_goal_checker simple_goal_checker ${catkin_LIBRARIES})
+add_dependencies(stopped_goal_checker ${catkin_EXPORTED_TARGETS} ${dwb_plugins_EXPORTED_TARGETS})
+
+add_library(standard_traj_generator
+            src/standard_traj_generator.cpp
+            src/limited_accel_generator.cpp
+            src/kinematic_parameters.cpp
+            src/xy_theta_iterator.cpp)
+target_link_libraries(standard_traj_generator ${catkin_LIBRARIES})
+add_dependencies(standard_traj_generator ${catkin_EXPORTED_TARGETS} ${dwb_plugins_EXPORTED_TARGETS})
+
+if (CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(vtest test/velocity_iterator_test.cpp)
+
+  find_package(rostest REQUIRED)
+  add_rostest_gtest(goal_checker test/goal_checker.launch test/goal_checker.cpp)
+  target_link_libraries(goal_checker simple_goal_checker stopped_goal_checker ${GTEST_LIBRARIES})
+
+  add_rostest_gtest(twist_gen_test test/twist_gen.launch test/twist_gen.cpp)
+  target_link_libraries(twist_gen_test standard_traj_generator ${GTEST_LIBRARIES})
+
+  find_package(roslint REQUIRED)
+  roslint_cpp()
+  roslint_add_test()
+endif (CATKIN_ENABLE_TESTING)
+
+install(TARGETS simple_goal_checker stopped_goal_checker standard_traj_generator
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+install(FILES plugins.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
--- a/navigations/dwb_plugins/README.md
+++ b/navigations/dwb_plugins/README.md
@@ -0,0 +1,44 @@
+# dwb_plugins
+
+This package contains the standard implementations of the GoalChecker and TrajectoryGenerators for `dwb_local_planner`. The TrajectoryCritic implementations are in `dwb_critics`.
+
+## Goal Checkers
+There are two goal checkers implemented.
+### SimpleGoalChecker
+SimpleGoalChecker will return true indicating a goal has been reached when the query pose is sufficiently close to the goal pose. It does this by comparing the cartesian difference against the `xy_goal_tolerance` and the shortest angular difference to the `yaw_goal_tolerance`. It does not check velocity.
+
+If the `stateful` parameter is set to true (which it is by default), once the desired cartesian difference is obtained, it will not check the cartesian difference again (until reset) and only check the yaw tolerance. This handles cases where the robot may accidentally leave the desired `xy_goal_tolerance` while rotating to the desired yaw. If `stateful` is true, this won't force the robot to try to move closer to the goal again, and instead just rotate to the goal.
+
+### StoppedGoalChecker
+StoppedGoalChecker builds off of the above functionality, but also ensures that the robot's linear velocity is less than `trans_stopped_velocity` and the rotational velocity is less than `rot_stopped_velocity`.
+
+## Trajectory Generation
+Trajectory Generation covers two separate but related components.
+ * First, it defines which command velocities are available, given the current velocity.
+ * Second, given a velocity, it defines the trajectory the robot would take.
+
+Generally, the available velocities are constrained by the robot's velocity and acceleration limits.
+
+![velocity limits diagram](doc/VelocitySpace.png)
+
+In the above diagram, the robot's current velocity is marked with a blue circle, and the grey rectangle marks the allowable velocities, limited by acceleration, and the robot's maximum x velocity. However, the exact size of the rectangle also depends on a time parameter, which we get into below.
+
+When converting the velocities to trajectories, the robot's position is projected ahead in both time and space. This is dependent on a time parameter (called `sim_time`) for how far into the future to project the robot's position. For a simple example, assume we had a robot at `x=0` travelling at `xv=2.0`, and we want to calculate the trajectory for `xv=2.0`. It might result in the following poses if `sim_time=2.0`.
+ * `t=0.0, x=0`
+ * `t=1.0, x=2.0`
+ * `t=2.0, x=4.0`
+
+However, the trajectory is more open to interpretation when the speed is not constant.
+
+### StandardTrajGenerator and LimitedAccelGenerator
+To replicate the functionality of `dwa_local_planner`, this package implements two TrajectoryGenerators. StandardTrajGenerator is equivalent to using DWA with `use_dwa=false` and LimitedAccelGenerator is equivalent to DWA with `use_dwa=true` (which is the default). The key difference in these generators are how time and acceleration are handled.
+
+As discussed above, `sim_time` is the parameter used for determining the time ellapsed in the trajectories (DWA's default was 1.7 seconds). In `StandardTrajGenerator` the available command velocities are limited by velocities reachable given the robot's acceleration in `sim_time`, i.e. the maximum available linear velocity is `vel_x + accel_x * sim_time`. In practice, given the relatively high value of `sim_time`, `StandardTrajGenerator` would allow for high accelerations almost instantaenously. For example, if initially motionless, with an accerlation of 1.0 m/s^2 and `sim_time=1.7`, the initial velocity could be 1.7 m/s.
+
+In `LimitedAccelGenerator` the time used for calculating the accerlation portion is either the parameter `sim_period`, or the inverse of the `controller_frequency` (i.e. if `controller_frequency` is 20.0, `sim_period` is set to 0.05). Then the maximum linear velocity is calculated with `vel_x + accel_x * sim_period`. This results in MUCH lower velocities.
+
+The other key difference between these two is how the acceleration is handled over time in the trajectory generation. In `StandardTrajGenerator` the velocity is allowed to change over the course of the trajectory based on the robot's acceleration, ramping up to the command velocity. In `LimitedAccelGenerator` the velocity is constant throughout the trajectory.
+
+In the below example, the velocities are shown for an initial speed of 0.0 and commanded speed of 3.5 m/s.
+
+![standard position and velocity](doc/std_pv.png)![limited acceleration position and velocity](doc/lim_pv.png)
--- a/navigations/dwb_plugins/cfg/KinematicParams.cfg
+++ b/navigations/dwb_plugins/cfg/KinematicParams.cfg
@@ -0,0 +1,33 @@
+#!/usr/bin/env python
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t
+
+gen = ParameterGenerator()
+
+# velocities
+gen.add('min_vel_x', double_t, 0, 'The minimum x velocity for the robot in m/s', 0.0)
+gen.add('max_vel_x', double_t, 0, 'The maximum x velocity for the robot in m/s', 0.55)
+gen.add('min_vel_y', double_t, 0, 'The minimum y velocity for the robot in m/s', -0.1)
+gen.add('max_vel_y', double_t, 0, 'The maximum y velocity for the robot in m/s', 0.1)
+gen.add('max_vel_theta', double_t, 0, 'The absolute value of the maximum rotational velocity for the robot in rad/s. '
+                                      'The minimum rotational velocity is assumed to be -max_vel_theta', 1.0)
+
+# acceleration
+gen.add('acc_lim_x', double_t, 0, 'The acceleration limit of the robot in the x direction in m/s^2', 2.5)
+gen.add('acc_lim_y', double_t, 0, 'The acceleration limit of the robot in the y direction in m/s^2', 2.5)
+gen.add('acc_lim_theta', double_t, 0, 'The acceleration limit of the robot in the theta direction in rad/s^2', 3.2)
+
+gen.add('decel_lim_x', double_t, 0, 'The deceleration limit of the robot in the x direction in m/s^2', -2.5)
+gen.add('decel_lim_y', double_t, 0, 'The deceleration limit of the robot in the y direction in m/s^2', -2.5)
+gen.add('decel_lim_theta', double_t, 0, 'The deceleration limit of the robot in the theta direction in rad/s^2', -3.2)
+
+gen.add('min_speed_xy', double_t, 0, 'The absolute value of the minimum translational/xy velocity in m/s. '
+                                     'If the value is negative, then the min speed will be arbitrarily close to 0.0. '
+                                     'Previously called min_trans_vel', 0.1)
+gen.add('max_speed_xy', double_t, 0, 'The absolute value of the maximum translational/xy velocity in m/s. '
+                                     'If the value is negative, then the max speed is hypot(max_vel_x, max_vel_y). '
+                                     'Previously called max_trans_vel', 0.55)
+gen.add('min_speed_theta', double_t, 0, 'The absolute value of the minimum rotational velocity in rad/s. '
+                                        'If the value is negative, then the min speed will be arbitrarily close to 0.0.'
+                                        ' Previously called min_rot_vel', 0.4)
+
+exit(gen.generate('dwb_plugins', 'dwb_plugins', 'KinematicParams'))
--- a/navigations/dwb_plugins/doc/VelocitySpace.png
+++ b/navigations/dwb_plugins/doc/VelocitySpace.png
--- a/navigations/dwb_plugins/doc/lim_pv.png
+++ b/navigations/dwb_plugins/doc/lim_pv.png
--- a/navigations/dwb_plugins/doc/std_pv.png
+++ b/navigations/dwb_plugins/doc/std_pv.png
--- a/navigations/dwb_plugins/include/dwb_plugins/kinematic_parameters.h
+++ b/navigations/dwb_plugins/include/dwb_plugins/kinematic_parameters.h
@@ -0,0 +1,113 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DWB_PLUGINS_KINEMATIC_PARAMETERS_H
+#define DWB_PLUGINS_KINEMATIC_PARAMETERS_H
+
+#include <ros/ros.h>
+#include <dynamic_reconfigure/server.h>
+#include <dwb_plugins/KinematicParamsConfig.h>
+#include <memory>
+
+namespace dwb_plugins
+{
+
+/**
+ * @class KinematicParameters
+ * @brief A dynamically reconfigurable class containing one representation of the robot's kinematics
+ */
+class KinematicParameters
+{
+public:
+  KinematicParameters();
+  void initialize(const ros::NodeHandle& nh);
+
+  inline double getMinX() { return min_vel_x_; }
+  inline double getMaxX() { return max_vel_x_; }
+  inline double getAccX() { return acc_lim_x_; }
+  inline double getDecelX() { return decel_lim_x_; }
+
+  inline double getMinY() { return min_vel_y_; }
+  inline double getMaxY() { return max_vel_y_; }
+  inline double getAccY() { return acc_lim_y_; }
+  inline double getDecelY() { return decel_lim_y_; }
+
+  inline double getMinSpeedXY() { return min_speed_xy_; }
+
+  inline double getMinTheta() { return -max_vel_theta_; }
+  inline double getMaxTheta() { return max_vel_theta_; }
+  inline double getAccTheta() { return acc_lim_theta_; }
+  inline double getDecelTheta() { return decel_lim_theta_; }
+  inline double getMinSpeedTheta() { return min_speed_theta_; }
+
+  /**
+   * @brief Check to see whether the combined x/y/theta velocities are valid
+   * @return True if the magnitude hypot(x,y) and theta are within the robot's absolute limits
+   *
+   * This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta.
+   * The speed is valid if
+   *  1) The combined magnitude hypot(x,y) is less than max_speed_xy (or max_speed_xy is negative)
+   *  AND
+   *  2) min_speed_xy is negative or min_speed_theta is negative or
+   *     hypot(x,y) is greater than min_speed_xy or fabs(theta) is greater than min_speed_theta.
+   *
+   * In English, it makes sure the diagonal motion is not too fast,
+   * and that the velocity is moving in some meaningful direction.
+   *
+   * In Latin, quod si motus sit signum quaerit et movere ieiunium et significantissime comprehendite.
+   */
+  bool isValidSpeed(double x, double y, double theta);
+
+  using Ptr = std::shared_ptr<KinematicParameters>;
+protected:
+  // For parameter descriptions, see cfg/KinematicParams.cfg
+  double min_vel_x_, min_vel_y_;
+  double max_vel_x_, max_vel_y_, max_vel_theta_;
+
+  double min_speed_xy_, max_speed_xy_;
+  double min_speed_theta_;
+
+  double acc_lim_x_, acc_lim_y_, acc_lim_theta_;
+  double decel_lim_x_, decel_lim_y_, decel_lim_theta_;
+
+  // Cached square values of min_speed_xy and max_speed_xy
+  double min_speed_xy_sq_, max_speed_xy_sq_;
+
+  void reconfigureCB(KinematicParamsConfig &config, uint32_t level);
+  std::shared_ptr<dynamic_reconfigure::Server<KinematicParamsConfig> > dsrv_;
+};
+
+}  // namespace dwb_plugins
+
+#endif  // DWB_PLUGINS_KINEMATIC_PARAMETERS_H
--- a/navigations/dwb_plugins/include/dwb_plugins/limited_accel_generator.h
+++ b/navigations/dwb_plugins/include/dwb_plugins/limited_accel_generator.h
@@ -0,0 +1,69 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DWB_PLUGINS_LIMITED_ACCEL_GENERATOR_H
+#define DWB_PLUGINS_LIMITED_ACCEL_GENERATOR_H
+
+#include <dwb_plugins/standard_traj_generator.h>
+
+namespace dwb_plugins
+{
+/**
+ * @class LimitedAccelGenerator
+ * @brief Limits the acceleration in the generated trajectories to a fraction of the simulated time.
+ */
+class LimitedAccelGenerator : public StandardTrajectoryGenerator
+{
+public:
+  void initialize(ros::NodeHandle& nh) override;
+  void checkUseDwaParam(const ros::NodeHandle& nh) override;
+  void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity) override;
+protected:
+  /**
+   * @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits
+   *
+   * Unlike the StandardTrajectoryGenerator, the velocity remains constant in the LimitedAccelGenerator
+   *
+   * @param cmd_vel Desired velocity
+   * @param start_vel starting velocity
+   * @param dt amount of time in seconds
+   * @return cmd_vel
+   */
+  nav_2d_msgs::Twist2D computeNewVelocity(const nav_2d_msgs::Twist2D& cmd_vel, const nav_2d_msgs::Twist2D& start_vel,
+                                          const double dt) override;
+  double acceleration_time_;
+};
+}  // namespace dwb_plugins
+
+#endif  // DWB_PLUGINS_LIMITED_ACCEL_GENERATOR_H
--- a/navigations/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.h
+++ b/navigations/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.h
@@ -0,0 +1,215 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DWB_PLUGINS_ONE_D_VELOCITY_ITERATOR_H
+#define DWB_PLUGINS_ONE_D_VELOCITY_ITERATOR_H
+
+#include <algorithm>
+#include <cmath>
+
+namespace dwb_plugins
+{
+
+const double EPSILON = 1E-5;
+
+/**
+ * @brief Given initial conditions and a time, figure out the end velocity
+ *
+ * @param v0 Initial velocity
+ * @param accel The acceleration rate
+ * @param decel The decceleration rate
+ * @param dt Delta time - amount of time to project into the future
+ * @param target target velocity
+ * @return The velocity dt seconds after v0.
+ */
+inline double projectVelocity(double v0, double accel, double decel, double dt, double target)
+{
+  double magnitude;
+  if (fabs(v0) < EPSILON)
+  {
+    // Starting from standstill, always accelerate
+    if (target >= 0.0)
+    {
+      magnitude = fabs(accel);
+    }
+    else
+    {
+      magnitude = -fabs(accel);
+    }
+  }
+  else if (v0 > 0.0)
+  {
+    if (v0 < target)
+    {
+      // Acceleration (speed magnitude increases)
+      magnitude = fabs(accel);
+    }
+    else
+    {
+      // Deceleration (speed magnitude decreases)
+      magnitude = -fabs(decel);
+    }
+  }
+  else
+  {
+    if (v0 < target)
+    {
+      // Deceleration (speed magnitude decreases)
+      magnitude = fabs(decel);
+    }
+    else
+    {
+      // Acceleration (speed magnitude increases)
+      magnitude = -fabs(accel);
+    }
+  }
+
+  double v1 = v0 + magnitude * dt;
+  if (magnitude > 0.0)
+  {
+    return std::min(target, v1);
+  }
+  else
+  {
+    return std::max(target, v1);
+  }
+}
+
+/**
+ * @class OneDVelocityIterator
+ * @brief An iterator for generating a number of samples in a range
+ *
+ * In its simplest usage, this gives us N (num_samples) different velocities that are reachable
+ * given our current velocity. However, there is some fancy logic around zero velocities and
+ * the min/max velocities
+ *
+ * If the current velocity is 2 m/s, and the acceleration limit is 1 m/ss and the acc_time is 1 s,
+ * this class would provide velocities between 1 m/s and 3 m/s.
+ *
+ *
+ *
+ */
+class OneDVelocityIterator
+{
+public:
+  /**
+   * @brief Constructor for the velocity iterator
+   *
+   * @param current Current velocity
+   * @param min Minimum velocity allowable
+   * @param max Maximum velocity allowable
+   * @param acc_limit Acceleration Limit
+   * @param decel_limit Deceleration Limit
+   * @param num_samples The number of samples to return
+   */
+  OneDVelocityIterator(double current, double min, double max, double acc_limit, double decel_limit, double acc_time,
+                       int num_samples)
+  {
+    if (current < min)
+    {
+      current = min;
+    }
+    else if (current > max)
+    {
+      current = max;
+    }
+    max_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, max);
+    min_vel_ = projectVelocity(current, acc_limit, decel_limit, acc_time, min);
+    reset();
+
+    if (fabs(min_vel_ - max_vel_) < EPSILON)
+    {
+      increment_ = 1.0;
+      return;
+    }
+    num_samples = std::max(2, num_samples);
+
+    // e.g. for 4 samples, split distance in 3 even parts
+    increment_ = (max_vel_ - min_vel_) / std::max(1, (num_samples - 1));
+  }
+
+  /**
+   * @brief Get the next velocity available
+   */
+  double getVelocity() const
+  {
+    if (return_zero_now_) return 0.0;
+    return current_;
+  }
+
+  /**
+   * @brief Increment the iterator
+   */
+  OneDVelocityIterator& operator++()
+  {
+    if (return_zero_ && current_ < 0.0 && current_ + increment_ > 0.0 && current_ + increment_ <= max_vel_ + EPSILON)
+    {
+      return_zero_now_ = true;
+      return_zero_ = false;
+    }
+    else
+    {
+      current_ += increment_;
+      return_zero_now_ = false;
+    }
+    return *this;
+  }
+
+  /**
+   * @brief Reset back to the first velocity
+   */
+  void reset()
+  {
+    current_ = min_vel_;
+    return_zero_ = true;
+    return_zero_now_ = false;
+  }
+
+  /**
+   * If we have returned all the velocities for this iteration
+   */
+  bool isFinished() const
+  {
+    return current_ > max_vel_ + EPSILON;
+  }
+
+private:
+  bool return_zero_, return_zero_now_;
+  double min_vel_, max_vel_;
+  double current_;
+  double increment_;
+};
+}  // namespace dwb_plugins
+
+#endif  // DWB_PLUGINS_ONE_D_VELOCITY_ITERATOR_H
--- a/navigations/dwb_plugins/include/dwb_plugins/simple_goal_checker.h
+++ b/navigations/dwb_plugins/include/dwb_plugins/simple_goal_checker.h
@@ -0,0 +1,70 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DWB_PLUGINS_SIMPLE_GOAL_CHECKER_H
+#define DWB_PLUGINS_SIMPLE_GOAL_CHECKER_H
+
+#include <ros/ros.h>
+#include <dwb_local_planner/goal_checker.h>
+
+namespace dwb_plugins
+{
+
+/**
+ * @class SimpleGoalChecker
+ * @brief Goal Checker plugin that only checks the position difference
+ *
+ * This class can be stateful if the stateful parameter is set to true (which it is by default).
+ * This means that the goal checker will not check if the xy position matches again once it is found to be true.
+ */
+class SimpleGoalChecker : public dwb_local_planner::GoalChecker
+{
+public:
+  SimpleGoalChecker();
+  // Standard GoalChecker Interface
+  void initialize(const ros::NodeHandle& nh) override;
+  void reset() override;
+  bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose,
+                     const nav_2d_msgs::Twist2D& velocity) override;
+protected:
+  double xy_goal_tolerance_, yaw_goal_tolerance_;
+  bool stateful_, check_xy_;
+
+  // Cached squared xy_goal_tolerance_
+  double xy_goal_tolerance_sq_;
+};
+
+}  // namespace dwb_plugins
+
+#endif  // DWB_PLUGINS_SIMPLE_GOAL_CHECKER_H
--- a/navigations/dwb_plugins/include/dwb_plugins/standard_traj_generator.h
+++ b/navigations/dwb_plugins/include/dwb_plugins/standard_traj_generator.h
@@ -0,0 +1,148 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DWB_PLUGINS_STANDARD_TRAJ_GENERATOR_H
+#define DWB_PLUGINS_STANDARD_TRAJ_GENERATOR_H
+
+#include <ros/ros.h>
+#include <dwb_local_planner/trajectory_generator.h>
+#include <dwb_plugins/velocity_iterator.h>
+#include <dwb_plugins/kinematic_parameters.h>
+#include <vector>
+#include <memory>
+
+namespace dwb_plugins
+{
+
+/**
+ * @class StandardTrajectoryGenerator
+ * @brief Standard DWA-like trajectory generator.
+ */
+class StandardTrajectoryGenerator : public dwb_local_planner::TrajectoryGenerator
+{
+public:
+  // Standard TrajectoryGenerator interface
+  void initialize(ros::NodeHandle& nh) override;
+  void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity) override;
+  bool hasMoreTwists() override;
+  nav_2d_msgs::Twist2D nextTwist() override;
+
+  dwb_msgs::Trajectory2D generateTrajectory(const geometry_msgs::Pose2D& start_pose,
+      const nav_2d_msgs::Twist2D& start_vel,
+      const nav_2d_msgs::Twist2D& cmd_vel) override;
+protected:
+  /**
+   * @brief Initialize the VelocityIterator pointer. Put in its own function for easy overriding
+   */
+  virtual void initializeIterator(ros::NodeHandle& nh);
+
+  /**
+   * @brief Check if the deprecated use_dwa parameter is set to the functionality that matches this class
+   *
+   * The functionality guarded by the use_dwa parameter has been split between this class and the derived
+   * LimitedAccelGenerator. If use_dwa was false, this class should be used. If it was true, then LimitedAccelGenerator.
+   * If this is NOT the case, this function will throw an exception.
+   */
+  virtual void checkUseDwaParam(const ros::NodeHandle& nh);
+
+  /**
+   * @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits
+   *
+   * @param cmd_vel Desired velocity
+   * @param start_vel starting velocity
+   * @param dt amount of time in seconds
+   * @return new velocity after dt seconds
+   */
+  virtual nav_2d_msgs::Twist2D computeNewVelocity(const nav_2d_msgs::Twist2D& cmd_vel,
+                                                  const nav_2d_msgs::Twist2D& start_vel,
+                                                  const double dt);
+
+  /**
+   * @brief Use the robot's kinematic model to predict new positions for the robot
+   *
+   * @param start_pose Starting pose
+   * @param vel Actual robot velocity (assumed to be within acceleration limits)
+   * @param dt amount of time in seconds
+   * @return New pose after dt seconds
+   */
+  virtual geometry_msgs::Pose2D computeNewPosition(const geometry_msgs::Pose2D start_pose,
+                                                   const nav_2d_msgs::Twist2D& vel,
+                                                   const double dt);
+
+
+  /**
+   * @brief Compute an array of time deltas between the points in the generated trajectory.
+   *
+   * @param cmd_vel The desired command velocity
+   * @return vector of the difference between each time step in the generated trajectory
+   *
+   * If we are discretizing by time, the returned vector will be the same constant time_granularity
+   * for all cmd_vels. Otherwise, you will get times based on the linear/angular granularity.
+   *
+   * Right now the vector contains a single value repeated many times, but this method could be overridden
+   * to allow for dynamic spacing
+   */
+  virtual std::vector<double> getTimeSteps(const nav_2d_msgs::Twist2D& cmd_vel);
+
+  KinematicParameters::Ptr kinematics_;
+  std::shared_ptr<VelocityIterator> velocity_iterator_;
+
+  double sim_time_;
+
+  // Sampling Parameters
+  bool discretize_by_time_;
+  double time_granularity_;     ///< If discretizing by time, the amount of time between each point in the traj
+  double linear_granularity_;   ///< If not discretizing by time, the amount of linear space between points
+  double angular_granularity_;  ///< If not discretizing by time, the amount of angular space between points
+
+  /* Backwards Compatibility Parameter: include_last_point
+   *
+   * dwa had an off-by-one error built into it.
+   * It generated N trajectory points, where N = ceil(sim_time / time_delta).
+   * If for example, sim_time=3.0 and time_delta=1.5, it would generate trajectories with 2 points, which
+   * indeed were time_delta seconds apart. However, the points would be at t=0 and t=1.5, and thus the
+   * actual sim_time was much less than advertised.
+   *
+   * This is remedied by adding one final point at t=sim_time, but only if include_last_point_ is true.
+   *
+   * Nothing I could find actually used the time_delta variable or seemed to care that the trajectories
+   * were not projected out as far as they intended.
+   */
+  bool include_last_point_;
+};
+
+
+}  // namespace dwb_plugins
+
+#endif  // DWB_PLUGINS_STANDARD_TRAJ_GENERATOR_H
--- a/navigations/dwb_plugins/include/dwb_plugins/stopped_goal_checker.h
+++ b/navigations/dwb_plugins/include/dwb_plugins/stopped_goal_checker.h
@@ -0,0 +1,62 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DWB_PLUGINS_STOPPED_GOAL_CHECKER_H
+#define DWB_PLUGINS_STOPPED_GOAL_CHECKER_H
+
+#include <ros/ros.h>
+#include <dwb_plugins/simple_goal_checker.h>
+
+namespace dwb_plugins
+{
+
+/**
+ * @class StoppedGoalChecker
+ * @brief Goal Checker plugin that checks the position difference and velocity
+ */
+class StoppedGoalChecker : public SimpleGoalChecker
+{
+public:
+  StoppedGoalChecker();
+  // Standard GoalChecker Interface
+  void initialize(const ros::NodeHandle& nh) override;
+  bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose,
+                     const nav_2d_msgs::Twist2D& velocity) override;
+protected:
+  double rot_stopped_velocity_, trans_stopped_velocity_;
+};
+
+}  // namespace dwb_plugins
+
+#endif  // DWB_PLUGINS_STOPPED_GOAL_CHECKER_H
--- a/navigations/dwb_plugins/include/dwb_plugins/velocity_iterator.h
+++ b/navigations/dwb_plugins/include/dwb_plugins/velocity_iterator.h
@@ -0,0 +1,55 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DWB_PLUGINS_VELOCITY_ITERATOR_H
+#define DWB_PLUGINS_VELOCITY_ITERATOR_H
+
+#include <ros/ros.h>
+#include <nav_2d_msgs/Twist2D.h>
+#include <dwb_plugins/kinematic_parameters.h>
+
+namespace dwb_plugins
+{
+class VelocityIterator
+{
+public:
+  virtual ~VelocityIterator() {}
+  virtual void initialize(ros::NodeHandle& nh, KinematicParameters::Ptr kinematics) = 0;
+  virtual void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt) = 0;
+  virtual bool hasMoreTwists() = 0;
+  virtual nav_2d_msgs::Twist2D nextTwist() = 0;
+};
+}  // namespace dwb_plugins
+
+#endif  // DWB_PLUGINS_VELOCITY_ITERATOR_H
--- a/navigations/dwb_plugins/include/dwb_plugins/xy_theta_iterator.h
+++ b/navigations/dwb_plugins/include/dwb_plugins/xy_theta_iterator.h
@@ -0,0 +1,62 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DWB_PLUGINS_XY_THETA_ITERATOR_H
+#define DWB_PLUGINS_XY_THETA_ITERATOR_H
+
+#include <dwb_plugins/velocity_iterator.h>
+#include <dwb_plugins/one_d_velocity_iterator.h>
+#include <memory>
+
+namespace dwb_plugins
+{
+class XYThetaIterator : public VelocityIterator
+{
+public:
+  XYThetaIterator() : kinematics_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {}
+  void initialize(ros::NodeHandle& nh, KinematicParameters::Ptr kinematics) override;
+  void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt) override;
+  bool hasMoreTwists() override;
+  nav_2d_msgs::Twist2D nextTwist() override;
+protected:
+  virtual bool isValidVelocity();
+  void iterateToValidVelocity();
+  int vx_samples_, vy_samples_, vtheta_samples_;
+  KinematicParameters::Ptr kinematics_;
+
+  std::shared_ptr<OneDVelocityIterator> x_it_, y_it_, th_it_;
+};
+}  // namespace dwb_plugins
+
+#endif  // DWB_PLUGINS_XY_THETA_ITERATOR_H
--- a/navigations/dwb_plugins/package.xml
+++ b/navigations/dwb_plugins/package.xml
@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>dwb_plugins</name>
+  <version>0.3.0</version>
+  <description>
+      Standard implementations of the GoalChecker
+      and TrajectoryGenerators for dwb_local_planner
+  </description>
+
+  <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>angles</depend>
+  <depend>dwb_local_planner</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>nav_2d_msgs</depend>
+  <depend>nav_2d_utils</depend>
+  <depend>nav_core2</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <test_depend>roslint</test_depend>
+  <test_depend>rostest</test_depend>
+  <test_depend>rosunit</test_depend>
+
+  <export>
+    <dwb_local_planner plugin="${prefix}/plugins.xml"/>
+  </export>
+</package>
--- a/navigations/dwb_plugins/plugins.xml
+++ b/navigations/dwb_plugins/plugins.xml
@@ -0,0 +1,20 @@
+<class_libraries>
+  <library path="lib/libsimple_goal_checker">
+    <class type="dwb_plugins::SimpleGoalChecker" base_class_type="dwb_local_planner::GoalChecker">
+      <description></description>
+    </class>
+  </library>
+  <library path="lib/libstandard_traj_generator">
+    <class type="dwb_plugins::StandardTrajectoryGenerator" base_class_type="dwb_local_planner::TrajectoryGenerator">
+      <description></description>
+    </class>
+    <class type="dwb_plugins::LimitedAccelGenerator" base_class_type="dwb_local_planner::TrajectoryGenerator">
+      <description></description>
+    </class>
+  </library>
+  <library path="lib/libstopped_goal_checker">
+    <class type="dwb_plugins::StoppedGoalChecker" base_class_type="dwb_local_planner::GoalChecker">
+      <description></description>
+    </class>
+  </library>
+</class_libraries>
--- a/navigations/dwb_plugins/src/kinematic_parameters.cpp
+++ b/navigations/dwb_plugins/src/kinematic_parameters.cpp
@@ -0,0 +1,126 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dwb_plugins/kinematic_parameters.h>
+#include <nav_2d_utils/parameters.h>
+#include <memory>
+#include <string>
+
+using nav_2d_utils::moveDeprecatedParameter;
+namespace dwb_plugins
+{
+
+const double EPSILON = 1E-5;
+
+/**
+ * @brief Helper function to set the deceleration to the negative acceleration if it was not already set.
+ * @param nh NodeHandle
+ * @param dimension String representing the dimension, used in constructing parameter names
+ */
+void setDecelerationAsNeeded(const ros::NodeHandle& nh, const std::string dimension)
+{
+  std::string decel_param = "decel_lim_" + dimension;
+  if (nh.hasParam(decel_param)) return;
+
+  std::string accel_param = "acc_lim_" + dimension;
+  if (!nh.hasParam(accel_param)) return;
+
+  double accel;
+  nh.getParam(accel_param, accel);
+  nh.setParam(decel_param, -accel);
+}
+
+KinematicParameters::KinematicParameters() :
+  min_vel_x_(0.0), min_vel_y_(0.0), max_vel_x_(0.0), max_vel_y_(0.0), max_vel_theta_(0.0),
+  min_speed_xy_(0.0), max_speed_xy_(0.0), min_speed_theta_(0.0),
+  acc_lim_x_(0.0), acc_lim_y_(0.0), acc_lim_theta_(0.0),
+  decel_lim_x_(0.0), decel_lim_y_(0.0), decel_lim_theta_(0.0),
+  min_speed_xy_sq_(0.0), max_speed_xy_sq_(0.0),
+  dsrv_(nullptr)
+{
+}
+
+void KinematicParameters::initialize(const ros::NodeHandle& nh)
+{
+  // Special handling for renamed parameters
+  moveDeprecatedParameter<double>(nh, "max_vel_theta", "max_rot_vel");
+  moveDeprecatedParameter<double>(nh, "min_speed_xy", "min_trans_vel");
+  moveDeprecatedParameter<double>(nh, "max_speed_xy", "max_trans_vel");
+  moveDeprecatedParameter<double>(nh, "min_speed_theta", "min_rot_vel");
+
+  // Set the deceleration parameters to negative the acceleration if the deceleration not already specified
+  setDecelerationAsNeeded(nh, "x");
+  setDecelerationAsNeeded(nh, "y");
+  setDecelerationAsNeeded(nh, "theta");
+
+  // the rest of the initial values are loaded through the dynamic reconfigure mechanisms
+  dsrv_ = std::make_shared<dynamic_reconfigure::Server<KinematicParamsConfig> >(nh);
+  dynamic_reconfigure::Server<KinematicParamsConfig>::CallbackType cb =
+    boost::bind(&KinematicParameters::reconfigureCB, this, _1, _2);
+  dsrv_->setCallback(cb);
+}
+
+void KinematicParameters::reconfigureCB(KinematicParamsConfig &config, uint32_t level)
+{
+  min_vel_x_ = config.min_vel_x;
+  min_vel_y_ = config.min_vel_y;
+  max_vel_x_ = config.max_vel_x;
+  max_vel_y_ = config.max_vel_y;
+  max_vel_theta_ = config.max_vel_theta;
+
+  min_speed_xy_ = config.min_speed_xy;
+  max_speed_xy_ = config.max_speed_xy;
+  min_speed_xy_sq_ = min_speed_xy_ * min_speed_xy_;
+  max_speed_xy_sq_ = max_speed_xy_ * max_speed_xy_;
+  min_speed_theta_ = config.min_speed_theta;
+
+  acc_lim_x_ = config.acc_lim_x;
+  acc_lim_y_ = config.acc_lim_y;
+  acc_lim_theta_ = config.acc_lim_theta;
+  decel_lim_x_ = config.decel_lim_x;
+  decel_lim_y_ = config.decel_lim_y;
+  decel_lim_theta_ = config.decel_lim_theta;
+}
+
+bool KinematicParameters::isValidSpeed(double x, double y, double theta)
+{
+  double vmag_sq = x * x + y * y;
+  if (max_speed_xy_ >= 0.0 && vmag_sq > max_speed_xy_sq_ + EPSILON) return false;
+  if (min_speed_xy_ >= 0.0 && vmag_sq + EPSILON < min_speed_xy_sq_ &&
+      min_speed_theta_ >= 0.0 && fabs(theta) + EPSILON < min_speed_theta_) return false;
+  if (vmag_sq == 0.0 && theta == 0.0) return false;
+  return true;
+}
+
+}  // namespace dwb_plugins
--- a/navigations/dwb_plugins/src/limited_accel_generator.cpp
+++ b/navigations/dwb_plugins/src/limited_accel_generator.cpp
@@ -0,0 +1,92 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dwb_plugins/limited_accel_generator.h>
+#include <nav_2d_utils/parameters.h>
+#include <pluginlib/class_list_macros.h>
+#include <nav_core2/exceptions.h>
+#include <vector>
+
+namespace dwb_plugins
+{
+
+void LimitedAccelGenerator::initialize(ros::NodeHandle& nh)
+{
+  StandardTrajectoryGenerator::initialize(nh);
+  if (nh.hasParam("sim_period"))
+  {
+    nh.getParam("sim_period", acceleration_time_);
+  }
+  else
+  {
+    double controller_frequency = nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0);
+    if (controller_frequency > 0)
+    {
+      acceleration_time_ = 1.0 / controller_frequency;
+    }
+    else
+    {
+      ROS_WARN_NAMED("LimitedAccelGenerator", "A controller_frequency less than or equal to 0 has been set. "
+                                              "Ignoring the parameter, assuming a rate of 20Hz");
+      acceleration_time_ = 0.05;
+    }
+  }
+}
+
+void LimitedAccelGenerator::checkUseDwaParam(const ros::NodeHandle& nh)
+{
+  bool use_dwa;
+  nh.param("use_dwa", use_dwa, true);
+  if (!use_dwa)
+  {
+    throw nav_core2::PlannerException("Deprecated parameter use_dwa set to false. "
+                                      "Please use StandardTrajectoryGenerator for that functionality.");
+  }
+}
+
+void LimitedAccelGenerator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity)
+{
+  // Limit our search space to just those within the limited acceleration_time
+  velocity_iterator_->startNewIteration(current_velocity, acceleration_time_);
+}
+
+nav_2d_msgs::Twist2D LimitedAccelGenerator::computeNewVelocity(const nav_2d_msgs::Twist2D& cmd_vel,
+    const nav_2d_msgs::Twist2D& start_vel, const double dt)
+{
+  return cmd_vel;
+}
+
+}  // namespace dwb_plugins
+
+PLUGINLIB_EXPORT_CLASS(dwb_plugins::LimitedAccelGenerator, dwb_local_planner::TrajectoryGenerator)
--- a/navigations/dwb_plugins/src/simple_goal_checker.cpp
+++ b/navigations/dwb_plugins/src/simple_goal_checker.cpp
@@ -0,0 +1,84 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dwb_plugins/simple_goal_checker.h>
+#include <pluginlib/class_list_macros.h>
+#include <angles/angles.h>
+
+namespace dwb_plugins
+{
+
+SimpleGoalChecker::SimpleGoalChecker() :
+  xy_goal_tolerance_(0.25), yaw_goal_tolerance_(0.25), stateful_(true), check_xy_(true), xy_goal_tolerance_sq_(0.0625)
+{
+}
+
+void SimpleGoalChecker::initialize(const ros::NodeHandle& nh)
+{
+  nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.25);
+  nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.25);
+  nh.param("stateful", stateful_, true);
+  xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
+}
+
+void SimpleGoalChecker::reset()
+{
+  check_xy_ = true;
+}
+
+bool SimpleGoalChecker::isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose,
+                                      const nav_2d_msgs::Twist2D& velocity)
+{
+  if (check_xy_)
+  {
+    double dx = query_pose.x - goal_pose.x,
+           dy = query_pose.y - goal_pose.y;
+    if (dx * dx + dy * dy > xy_goal_tolerance_sq_)
+    {
+      return false;
+    }
+    // We are within the window
+    // If we are stateful, change the state.
+    if (stateful_)
+    {
+      check_xy_ = false;
+    }
+  }
+  double dyaw = angles::shortest_angular_distance(query_pose.theta, goal_pose.theta);
+  return fabs(dyaw) < yaw_goal_tolerance_;
+}
+
+}  // namespace dwb_plugins
+
+PLUGINLIB_EXPORT_CLASS(dwb_plugins::SimpleGoalChecker, dwb_local_planner::GoalChecker)
--- a/navigations/dwb_plugins/src/standard_traj_generator.cpp
+++ b/navigations/dwb_plugins/src/standard_traj_generator.cpp
@@ -0,0 +1,203 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dwb_plugins/standard_traj_generator.h>
+#include <dwb_plugins/xy_theta_iterator.h>
+#include <nav_2d_utils/parameters.h>
+#include <pluginlib/class_list_macros.h>
+#include <nav_core2/exceptions.h>
+#include <algorithm>
+#include <memory>
+#include <string>
+#include <vector>
+
+using nav_2d_utils::loadParameterWithDeprecation;
+
+namespace dwb_plugins
+{
+
+void StandardTrajectoryGenerator::initialize(ros::NodeHandle& nh)
+{
+  kinematics_ = std::make_shared<KinematicParameters>();
+  kinematics_->initialize(nh);
+  initializeIterator(nh);
+
+  nh.param("sim_time", sim_time_, 1.7);
+  checkUseDwaParam(nh);
+
+  nh.param("include_last_point", include_last_point_, true);
+
+  /*
+   * If discretize_by_time, then sim_granularity represents the amount of time that should be between
+   *  two successive points on the trajectory.
+   *
+   * If discretize_by_time is false, then sim_granularity is the maximum amount of distance between
+   *  two successive points on the trajectory, and angular_sim_granularity is the maximum amount of
+   *  angular distance between two successive points.
+   */
+  nh.param("discretize_by_time", discretize_by_time_, false);
+  if (discretize_by_time_)
+  {
+    time_granularity_ = loadParameterWithDeprecation(nh, "time_granularity", "sim_granularity", 0.025);
+  }
+  else
+  {
+    linear_granularity_ = loadParameterWithDeprecation(nh, "linear_granularity", "sim_granularity", 0.025);
+    angular_granularity_ = loadParameterWithDeprecation(nh, "angular_granularity", "angular_sim_granularity", 0.1);
+  }
+}
+
+void StandardTrajectoryGenerator::initializeIterator(ros::NodeHandle& nh)
+{
+  velocity_iterator_ = std::make_shared<XYThetaIterator>();
+  velocity_iterator_->initialize(nh, kinematics_);
+}
+
+void StandardTrajectoryGenerator::checkUseDwaParam(const ros::NodeHandle& nh)
+{
+  bool use_dwa;
+  nh.param("use_dwa", use_dwa, false);
+  if (use_dwa)
+  {
+    throw nav_core2::PlannerException("Deprecated parameter use_dwa set to true. "
+                                      "Please use LimitedAccelGenerator for that functionality.");
+  }
+}
+
+void StandardTrajectoryGenerator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity)
+{
+  velocity_iterator_->startNewIteration(current_velocity, sim_time_);
+}
+
+bool StandardTrajectoryGenerator::hasMoreTwists()
+{
+  return velocity_iterator_->hasMoreTwists();
+}
+
+nav_2d_msgs::Twist2D StandardTrajectoryGenerator::nextTwist()
+{
+  return velocity_iterator_->nextTwist();
+}
+
+std::vector<double> StandardTrajectoryGenerator::getTimeSteps(const nav_2d_msgs::Twist2D& cmd_vel)
+{
+  std::vector<double> steps;
+  if (discretize_by_time_)
+  {
+    steps.resize(ceil(sim_time_ / time_granularity_));
+  }
+  else  // discretize by distance
+  {
+    double vmag = hypot(cmd_vel.x, cmd_vel.y);
+
+    // the distance the robot would travel in sim_time if it did not change velocity
+    double projected_linear_distance = vmag * sim_time_;
+
+    // the angle the robot would rotate in sim_time
+    double projected_angular_distance = fabs(cmd_vel.theta) * sim_time_;
+
+    // Pick the maximum of the two
+    int num_steps = ceil(std::max(projected_linear_distance / linear_granularity_,
+                                  projected_angular_distance / angular_granularity_));
+    steps.resize(num_steps);
+  }
+  if (steps.size() == 0)
+  {
+    steps.resize(1);
+  }
+  std::fill(steps.begin(), steps.end(), sim_time_ / steps.size());
+  return steps;
+}
+
+dwb_msgs::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(const geometry_msgs::Pose2D& start_pose,
+    const nav_2d_msgs::Twist2D& start_vel, // from odometry
+    const nav_2d_msgs::Twist2D& cmd_vel)  // twist = traj_generator_->nextTwist();
+{
+  dwb_msgs::Trajectory2D traj;
+  traj.velocity = cmd_vel;
+
+  //  simulate the trajectory
+  geometry_msgs::Pose2D pose = start_pose;
+  nav_2d_msgs::Twist2D vel = start_vel;
+  double running_time = 0.0;
+  std::vector<double> steps = getTimeSteps(cmd_vel); 
+  
+  for (double dt : steps)
+  {
+    traj.poses.push_back(pose);
+    traj.time_offsets.push_back(ros::Duration(running_time));
+    //  calculate velocities
+    vel = computeNewVelocity(cmd_vel, vel, dt);
+
+    //  update the position of the robot using the velocities passed in
+    pose = computeNewPosition(pose, vel, dt);
+    running_time += dt;
+  }  //  end for simulation steps
+
+  if (include_last_point_)
+  {
+    traj.poses.push_back(pose);
+    traj.time_offsets.push_back(ros::Duration(running_time));
+  }
+
+  return traj;
+}
+
+/**
+ * change vel using acceleration limits to converge towards sample_target-vel
+ */
+nav_2d_msgs::Twist2D StandardTrajectoryGenerator::computeNewVelocity(const nav_2d_msgs::Twist2D& cmd_vel,
+    const nav_2d_msgs::Twist2D& start_vel, const double dt)
+{
+  nav_2d_msgs::Twist2D new_vel;
+  new_vel.x = projectVelocity(start_vel.x, kinematics_->getAccX(), kinematics_->getDecelX(), dt, cmd_vel.x);
+  new_vel.y = projectVelocity(start_vel.y, kinematics_->getAccY(), kinematics_->getDecelY(), dt, cmd_vel.y);
+  new_vel.theta = projectVelocity(start_vel.theta, kinematics_->getAccTheta(), kinematics_->getDecelTheta(),
+                                  dt, cmd_vel.theta);
+  return new_vel;
+}
+
+geometry_msgs::Pose2D StandardTrajectoryGenerator::computeNewPosition(const geometry_msgs::Pose2D start_pose,
+                                                                      const nav_2d_msgs::Twist2D& vel, const double dt)
+{
+  geometry_msgs::Pose2D new_pose;
+  new_pose.x = start_pose.x + (vel.x * cos(start_pose.theta) + vel.y * cos(M_PI_2 + start_pose.theta)) * dt;
+  new_pose.y = start_pose.y + (vel.x * sin(start_pose.theta) + vel.y * sin(M_PI_2 + start_pose.theta)) * dt;
+  new_pose.theta = start_pose.theta + vel.theta * dt;
+  return new_pose;
+}
+
+}  // namespace dwb_plugins
+
+PLUGINLIB_EXPORT_CLASS(dwb_plugins::StandardTrajectoryGenerator, dwb_local_planner::TrajectoryGenerator)
--- a/navigations/dwb_plugins/src/stopped_goal_checker.cpp
+++ b/navigations/dwb_plugins/src/stopped_goal_checker.cpp
@@ -0,0 +1,67 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dwb_plugins/stopped_goal_checker.h>
+#include <pluginlib/class_list_macros.h>
+
+namespace dwb_plugins
+{
+
+StoppedGoalChecker::StoppedGoalChecker() :
+  SimpleGoalChecker(), rot_stopped_velocity_(0.25), trans_stopped_velocity_(0.25)
+{
+}
+
+void StoppedGoalChecker::initialize(const ros::NodeHandle& nh)
+{
+  SimpleGoalChecker::initialize(nh);
+  nh.param("rot_stopped_velocity", rot_stopped_velocity_, 0.25);
+  nh.param("trans_stopped_velocity", trans_stopped_velocity_, 0.25);
+}
+
+bool StoppedGoalChecker::isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose,
+                                       const nav_2d_msgs::Twist2D& velocity)
+{
+  bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity);
+  if (!ret)
+  {
+    return ret;
+  }
+  return fabs(velocity.theta) <= rot_stopped_velocity_
+         && fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.y) <= trans_stopped_velocity_;
+}
+
+}  // namespace dwb_plugins
+
+PLUGINLIB_EXPORT_CLASS(dwb_plugins::StoppedGoalChecker, dwb_local_planner::GoalChecker)
--- a/navigations/dwb_plugins/src/xy_theta_iterator.cpp
+++ b/navigations/dwb_plugins/src/xy_theta_iterator.cpp
@@ -0,0 +1,107 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <dwb_plugins/xy_theta_iterator.h>
+#include <nav_2d_utils/parameters.h>
+#include <memory>
+
+namespace dwb_plugins
+{
+void XYThetaIterator::initialize(ros::NodeHandle& nh, KinematicParameters::Ptr kinematics)
+{
+  kinematics_ = kinematics;
+  nh.param("vx_samples", vx_samples_, 20);
+  nh.param("vy_samples", vy_samples_, 5);
+  vtheta_samples_ = nav_2d_utils::loadParameterWithDeprecation(nh, "vtheta_samples", "vth_samples", 20);
+}
+
+void XYThetaIterator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt)
+{
+  x_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.x, kinematics_->getMinX(), kinematics_->getMaxX(),
+                                                 kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_);
+  y_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.y, kinematics_->getMinY(), kinematics_->getMaxY(),
+                                                 kinematics_->getAccY(), kinematics_->getDecelY(), dt, vy_samples_);
+  th_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.theta,
+                                                  kinematics_->getMinTheta(), kinematics_->getMaxTheta(),
+                                                  kinematics_->getAccTheta(), kinematics_->getDecelTheta(),
+                                                  dt, vtheta_samples_);
+  if (!isValidVelocity())
+  {
+    iterateToValidVelocity();
+  }
+}
+
+bool XYThetaIterator::isValidVelocity()
+{
+  return kinematics_->isValidSpeed(x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity());
+}
+
+bool XYThetaIterator::hasMoreTwists()
+{
+  return x_it_ && !x_it_->isFinished();
+}
+
+
+nav_2d_msgs::Twist2D XYThetaIterator::nextTwist()
+{
+  nav_2d_msgs::Twist2D velocity;
+  velocity.x = x_it_->getVelocity();
+  velocity.y = y_it_->getVelocity();
+  velocity.theta = th_it_->getVelocity();
+  iterateToValidVelocity();
+  
+  return velocity;
+}
+
+void XYThetaIterator::iterateToValidVelocity()
+{
+  bool valid = false;
+  while (!valid && hasMoreTwists())
+  {
+    ++(*th_it_);
+    if (th_it_->isFinished())
+    {
+      th_it_->reset();
+      ++(*y_it_);
+      if (y_it_->isFinished())
+      {
+        y_it_->reset();
+        ++(*x_it_);
+      }
+    }
+    valid = isValidVelocity();
+  }
+}
+
+}  // namespace dwb_plugins
--- a/navigations/dwb_plugins/test/goal_checker.cpp
+++ b/navigations/dwb_plugins/test/goal_checker.cpp
@@ -0,0 +1,107 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <gtest/gtest.h>
+#include <dwb_plugins/simple_goal_checker.h>
+#include <dwb_plugins/stopped_goal_checker.h>
+
+using dwb_plugins::SimpleGoalChecker;
+using dwb_plugins::StoppedGoalChecker;
+
+void checkMacro(dwb_local_planner::GoalChecker& gc,
+                double x0, double y0, double theta0,
+                double x1, double y1, double theta1,
+                double xv, double yv, double thetav,
+                bool expected_result)
+{
+  gc.reset();
+  geometry_msgs::Pose2D pose0, pose1;
+  pose0.x = x0;
+  pose0.y = y0;
+  pose0.theta = theta0;
+  pose1.x = x1;
+  pose1.y = y1;
+  pose1.theta = theta1;
+  nav_2d_msgs::Twist2D v;
+  v.x = xv;
+  v.y = yv;
+  v.theta = thetav;
+  if (expected_result)
+    EXPECT_TRUE(gc.isGoalReached(pose0, pose1, v));
+  else
+    EXPECT_FALSE(gc.isGoalReached(pose0, pose1, v));
+}
+
+void sameResult(dwb_local_planner::GoalChecker& gc0, dwb_local_planner::GoalChecker& gc1,
+                double x0, double y0, double theta0,
+                double x1, double y1, double theta1,
+                double xv, double yv, double thetav,
+                bool expected_result)
+{
+  checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
+  checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
+}
+
+void trueFalse(dwb_local_planner::GoalChecker& gc0, dwb_local_planner::GoalChecker& gc1,
+               double x0, double y0, double theta0,
+               double x1, double y1, double theta1,
+               double xv, double yv, double thetav)
+{
+  checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true);
+  checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false);
+}
+
+TEST(VelocityIterator, two_checks)
+{
+  ros::NodeHandle x;
+  SimpleGoalChecker gc;
+  StoppedGoalChecker sgc;
+  gc.initialize(x);
+  sgc.initialize(x);
+  sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true);
+  sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false);
+  sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false);
+  sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, false);
+  sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, true);
+  trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0);
+  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0);
+  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0);
+  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1);
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "goal_checker");
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
--- a/navigations/dwb_plugins/test/goal_checker.launch
+++ b/navigations/dwb_plugins/test/goal_checker.launch
@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="goal_checker" pkg="dwb_plugins" type="goal_checker" />
+</launch>
--- a/navigations/dwb_plugins/test/twist_gen.cpp
+++ b/navigations/dwb_plugins/test/twist_gen.cpp
@@ -0,0 +1,422 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <gtest/gtest.h>
+#include <dwb_plugins/standard_traj_generator.h>
+#include <dwb_plugins/limited_accel_generator.h>
+#include <nav_core2/exceptions.h>
+#include <vector>
+#include <algorithm>
+
+using dwb_plugins::StandardTrajectoryGenerator;
+
+geometry_msgs::Pose2D origin;
+nav_2d_msgs::Twist2D zero;
+nav_2d_msgs::Twist2D forward;
+
+void checkLimits(const std::vector<nav_2d_msgs::Twist2D>& twists,
+                 double exp_min_x, double exp_max_x, double exp_min_y, double exp_max_y,
+                 double exp_min_theta, double exp_max_theta,
+                 double exp_max_xy = -1.0,
+                 double exp_min_xy = -1.0, double exp_min_speed_theta = -1.0)
+{
+  ASSERT_GT(twists.size(), 0U);
+  nav_2d_msgs::Twist2D first = twists[0];
+
+  double min_x = first.x, max_x = first.x, min_y = first.y, max_y = first.y;
+  double min_theta = first.theta, max_theta = first.theta;
+  double max_xy = hypot(first.x, first.y);
+
+  for (nav_2d_msgs::Twist2D twist : twists)
+  {
+    min_x = std::min(min_x, twist.x);
+    min_y = std::min(min_y, twist.y);
+    min_theta = std::min(min_theta, twist.theta);
+    max_x = std::max(max_x, twist.x);
+    max_y = std::max(max_y, twist.y);
+    max_theta = std::max(max_theta, twist.theta);
+    double hyp = hypot(twist.x, twist.y);
+    max_xy = std::max(max_xy, hyp);
+
+    if (exp_min_xy >= 0 && exp_min_speed_theta >= 0)
+    {
+      EXPECT_TRUE(fabs(twist.theta) >= exp_min_speed_theta || hyp >= exp_min_xy);
+    }
+  }
+  EXPECT_DOUBLE_EQ(min_x, exp_min_x);
+  EXPECT_DOUBLE_EQ(max_x, exp_max_x);
+  EXPECT_DOUBLE_EQ(min_y, exp_min_y);
+  EXPECT_DOUBLE_EQ(max_y, exp_max_y);
+  EXPECT_DOUBLE_EQ(min_theta, exp_min_theta);
+  EXPECT_DOUBLE_EQ(max_theta, exp_max_theta);
+  if (exp_max_xy >= 0)
+  {
+    EXPECT_DOUBLE_EQ(max_xy, exp_max_xy);
+  }
+}
+
+TEST(VelocityIterator, standard_gen)
+{
+  ros::NodeHandle nh("st_gen");
+  StandardTrajectoryGenerator gen;
+  gen.initialize(nh);
+  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
+  EXPECT_EQ(twists.size(), 1926U);
+  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, 0.55, 0.1, 0.4);
+}
+
+TEST(VelocityIterator, max_xy)
+{
+  ros::NodeHandle nh("max_xy");
+  nh.setParam("max_speed_xy", 1.0);
+  StandardTrajectoryGenerator gen;
+  gen.initialize(nh);
+
+  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
+  // Expect more twists since max_speed_xy is now beyond feasible limits
+  EXPECT_EQ(twists.size(), 2010U);
+  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1));
+}
+
+TEST(VelocityIterator, min_xy)
+{
+  ros::NodeHandle nh("min_xy");
+  nh.setParam("min_speed_xy", -1);
+  StandardTrajectoryGenerator gen;
+  gen.initialize(nh);
+  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
+  // Expect even more since theres no min_speed_xy
+  EXPECT_EQ(twists.size(), 2015U);
+  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
+}
+
+TEST(VelocityIterator, min_theta)
+{
+  ros::NodeHandle nh("min_theta");
+  nh.setParam("min_speed_theta", -1);
+  StandardTrajectoryGenerator gen;
+  gen.initialize(nh);
+  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
+  // Expect even more since theres no min_speed_xy
+  EXPECT_EQ(twists.size(), 2015U);
+  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
+}
+
+TEST(VelocityIterator, no_limits)
+{
+  ros::NodeHandle nh("no_limits");
+  nh.setParam("max_speed_xy", -1.0);
+  nh.setParam("min_speed_xy", -1);
+  nh.setParam("min_speed_theta", -1);
+  StandardTrajectoryGenerator gen;
+  gen.initialize(nh);
+  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
+  // vx_samples * vtheta_samples * vy_samples + added zero theta samples - (0,0,0)
+  EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
+  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
+}
+
+TEST(VelocityIterator, no_limits_samples)
+{
+  ros::NodeHandle nh("no_limits_samples");
+  nh.setParam("max_speed_xy", -1.0);
+  nh.setParam("min_speed_xy", -1);
+  nh.setParam("min_speed_theta", -1);
+  int x_samples = 10, y_samples = 3, theta_samples = 5;
+  nh.setParam("vx_samples", x_samples);
+  nh.setParam("vy_samples", y_samples);
+  nh.setParam("vtheta_samples", theta_samples);
+  StandardTrajectoryGenerator gen;
+  gen.initialize(nh);
+  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
+  EXPECT_EQ(twists.size(), static_cast<unsigned int>(x_samples * y_samples * theta_samples - 1));
+  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
+}
+
+TEST(VelocityIterator, dwa_gen_exception)
+{
+  ros::NodeHandle nh("dwa_gen_exception");
+  nh.setParam("use_dwa", true);
+  StandardTrajectoryGenerator gen;
+  EXPECT_THROW(gen.initialize(nh), nav_core2::PlannerException);
+}
+
+TEST(VelocityIterator, no_dwa_gen_exception)
+{
+  ros::NodeHandle nh("no_dwa_gen_exception");
+  nh.setParam("use_dwa", false);
+  dwb_plugins::LimitedAccelGenerator gen;
+  EXPECT_THROW(gen.initialize(nh), nav_core2::PlannerException);
+}
+
+TEST(VelocityIterator, dwa_gen)
+{
+  ros::NodeHandle nh("dwa_gen");
+  nh.setParam("use_dwa", true);
+  nh.setParam("min_speed_theta", -1);
+  dwb_plugins::LimitedAccelGenerator gen;
+  gen.initialize(nh);
+  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
+  // Same as no-limits since everything is within our velocity limits
+  EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
+  checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1);
+}
+
+TEST(VelocityIterator, dwa_gen_no_param)
+{
+  ros::NodeHandle nh("dwa_gen_no_param");
+  nh.setParam("min_speed_theta", -1);
+  dwb_plugins::LimitedAccelGenerator gen;
+  gen.initialize(nh);
+  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
+  EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
+  checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1);
+}
+
+TEST(VelocityIterator, nonzero)
+{
+  ros::NodeHandle nh("nonzero");
+  nh.setParam("use_dwa", true);
+  nh.setParam("min_speed_theta", -1);
+  dwb_plugins::LimitedAccelGenerator gen;
+  gen.initialize(nh);
+  nav_2d_msgs::Twist2D initial;
+  initial.x = 0.1;
+  initial.y = -0.08;
+  initial.theta = 0.05;
+  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(initial);
+  EXPECT_EQ(twists.size(), 2519U);
+  checkLimits(twists, 0.0, 0.225, -0.1, 0.045, -0.11000000000000003, 0.21,
+                      0.24622144504490268, 0.0, 0.1);
+}
+
+void matchPose(const geometry_msgs::Pose2D& a, const geometry_msgs::Pose2D& b)
+{
+  EXPECT_DOUBLE_EQ(a.x, b.x);
+  EXPECT_DOUBLE_EQ(a.y, b.y);
+  EXPECT_DOUBLE_EQ(a.theta, b.theta);
+}
+
+void matchPose(const geometry_msgs::Pose2D& a, const double x, const double y, const double theta)
+{
+  EXPECT_DOUBLE_EQ(a.x, x);
+  EXPECT_DOUBLE_EQ(a.y, y);
+  EXPECT_DOUBLE_EQ(a.theta, theta);
+}
+
+void matchTwist(const nav_2d_msgs::Twist2D& a, const nav_2d_msgs::Twist2D& b)
+{
+  EXPECT_DOUBLE_EQ(a.x, b.x);
+  EXPECT_DOUBLE_EQ(a.y, b.y);
+  EXPECT_DOUBLE_EQ(a.theta, b.theta);
+}
+
+void matchTwist(const nav_2d_msgs::Twist2D& a, const double x, const double y, const double theta)
+{
+  EXPECT_DOUBLE_EQ(a.x, x);
+  EXPECT_DOUBLE_EQ(a.y, y);
+  EXPECT_DOUBLE_EQ(a.theta, theta);
+}
+
+const double DEFAULT_SIM_TIME = 1.7;
+
+TEST(TrajectoryGenerator, basic)
+{
+  ros::NodeHandle nh("basic");
+  StandardTrajectoryGenerator gen;
+  nh.setParam("linear_granularity", 0.5);
+  gen.initialize(nh);
+  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
+  matchTwist(res.velocity, forward);
+  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
+  int n = res.poses.size();
+  EXPECT_EQ(n, 3);
+  ASSERT_GT(n, 0);
+
+  matchPose(res.poses[0], origin);
+  matchPose(res.poses[n - 1], DEFAULT_SIM_TIME * forward.x, 0, 0);
+}
+
+TEST(TrajectoryGenerator, basic_no_last_point)
+{
+  ros::NodeHandle nh("basic_no_last_point");
+  StandardTrajectoryGenerator gen;
+  nh.setParam("include_last_point", false);
+  nh.setParam("linear_granularity", 0.5);
+  gen.initialize(nh);
+  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
+  matchTwist(res.velocity, forward);
+  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME / 2);
+  int n = res.poses.size();
+  EXPECT_EQ(n, 2);
+  ASSERT_GT(n, 0);
+
+  matchPose(res.poses[0], origin);
+  matchPose(res.poses[n - 1], 0.255, 0, 0);
+}
+
+TEST(TrajectoryGenerator, too_slow)
+{
+  ros::NodeHandle nh("too_slow");
+  StandardTrajectoryGenerator gen;
+  nh.setParam("linear_granularity", 0.5);
+  gen.initialize(nh);
+  nav_2d_msgs::Twist2D cmd;
+  cmd.x = 0.2;
+  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
+  matchTwist(res.velocity, cmd);
+  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
+  int n = res.poses.size();
+  EXPECT_EQ(n, 2);
+  ASSERT_GT(n, 0);
+
+  matchPose(res.poses[0], origin);
+}
+
+TEST(TrajectoryGenerator, holonomic)
+{
+  ros::NodeHandle nh("holonomic");
+  StandardTrajectoryGenerator gen;
+  nh.setParam("linear_granularity", 0.5);
+  gen.initialize(nh);
+  nav_2d_msgs::Twist2D cmd;
+  cmd.x = 0.3;
+  cmd.y = 0.2;
+  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
+  matchTwist(res.velocity, cmd);
+  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
+  int n = res.poses.size();
+  EXPECT_EQ(n, 3);
+  ASSERT_GT(n, 0);
+
+  matchPose(res.poses[0], origin);
+  matchPose(res.poses[n - 1], cmd.x*DEFAULT_SIM_TIME, cmd.y*DEFAULT_SIM_TIME, 0);
+}
+
+TEST(TrajectoryGenerator, twisty)
+{
+  ros::NodeHandle nh("twisty");
+  StandardTrajectoryGenerator gen;
+  nh.setParam("linear_granularity", 0.5);
+  nh.setParam("angular_granularity", 0.025);
+  gen.initialize(nh);
+  nav_2d_msgs::Twist2D cmd;
+  cmd.x = 0.3;
+  cmd.y = -0.2;
+  cmd.theta = 0.111;
+  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
+  matchTwist(res.velocity, cmd);
+  EXPECT_NEAR(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME, 1.0E-5);
+              // Given the number of poses, the resulting time is slightly less precise
+  int n = res.poses.size();
+  EXPECT_EQ(n, 9);
+  ASSERT_GT(n, 0);
+
+  matchPose(res.poses[0], origin);
+  matchPose(res.poses[n - 1], 0.5355173615993063, -0.29635287789821596, cmd.theta * DEFAULT_SIM_TIME);
+}
+
+TEST(TrajectoryGenerator, sim_time)
+{
+  ros::NodeHandle nh("sim_time");
+  const double sim_time = 2.5;
+  nh.setParam("sim_time", sim_time);
+  nh.setParam("linear_granularity", 0.5);
+  StandardTrajectoryGenerator gen;
+  gen.initialize(nh);
+  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
+  matchTwist(res.velocity, forward);
+  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), sim_time);
+  int n = res.poses.size();
+  EXPECT_EQ(n, 3);
+  ASSERT_GT(n, 0);
+
+  matchPose(res.poses[0], origin);
+  matchPose(res.poses[n - 1], sim_time * forward.x, 0, 0);
+}
+
+TEST(TrajectoryGenerator, accel)
+{
+  ros::NodeHandle nh("accel");
+  nh.setParam("sim_time", 5.0);
+  nh.setParam("discretize_by_time", true);
+  nh.setParam("sim_granularity", 1.0);
+  nh.setParam("acc_lim_x", 0.1);
+  nh.setParam("min_speed_xy", -1.0);
+  StandardTrajectoryGenerator gen;
+  gen.initialize(nh);
+
+  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
+  matchTwist(res.velocity, forward);
+  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), 5.0);
+  ASSERT_EQ(res.poses.size(), 6U);
+  matchPose(res.poses[0], origin);
+  matchPose(res.poses[1], 0.1, 0, 0);
+  matchPose(res.poses[2], 0.3, 0, 0);
+  matchPose(res.poses[3], 0.6, 0, 0);
+  matchPose(res.poses[4], 0.9, 0, 0);
+  matchPose(res.poses[5], 1.2, 0, 0);
+}
+
+TEST(TrajectoryGenerator, dwa)
+{
+  ros::NodeHandle nh("dwa");
+  nh.setParam("use_dwa", true);
+  nh.setParam("sim_period", 1.0);
+  nh.setParam("sim_time", 5.0);
+  nh.setParam("discretize_by_time", true);
+  nh.setParam("sim_granularity", 1.0);
+  nh.setParam("acc_lim_x", 0.1);
+  nh.setParam("min_speed_xy", -1.0);
+  dwb_plugins::LimitedAccelGenerator gen;
+  gen.initialize(nh);
+
+  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
+  matchTwist(res.velocity, forward);
+  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), 5.0);
+  ASSERT_EQ(res.poses.size(), 6U);
+  matchPose(res.poses[0], origin);
+  matchPose(res.poses[1], 0.3, 0, 0);
+  matchPose(res.poses[2], 0.6, 0, 0);
+  matchPose(res.poses[3], 0.9, 0, 0);
+  matchPose(res.poses[4], 1.2, 0, 0);
+  matchPose(res.poses[5], 1.5, 0, 0);
+}
+
+int main(int argc, char **argv)
+{
+  forward.x = 0.3;
+  ros::init(argc, argv, "twist_gen");
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
--- a/navigations/dwb_plugins/test/twist_gen.launch
+++ b/navigations/dwb_plugins/test/twist_gen.launch
@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="twist_gen_test" pkg="dwb_plugins" type="twist_gen_test" />
+</launch>
--- a/navigations/dwb_plugins/test/velocity_iterator_test.cpp
+++ b/navigations/dwb_plugins/test/velocity_iterator_test.cpp
@@ -0,0 +1,155 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <gtest/gtest.h>
+#include <dwb_plugins/one_d_velocity_iterator.h>
+
+using dwb_plugins::OneDVelocityIterator;
+using dwb_plugins::projectVelocity;
+
+const double EPSILON = 1e-3;
+
+TEST(VelocityIterator, basics)
+{
+  OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 2);
+  EXPECT_FALSE(it.isFinished());
+  EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
+  EXPECT_FALSE(it.isFinished());
+  ++it;
+  EXPECT_FALSE(it.isFinished());
+  EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
+  EXPECT_FALSE(it.isFinished());
+  ++it;
+  EXPECT_TRUE(it.isFinished());
+  it.reset();
+  EXPECT_FALSE(it.isFinished());
+  EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
+}
+
+TEST(VelocityIterator, limits)
+{
+  OneDVelocityIterator it(2.0, 1.5, 2.5, 1.0, -1.0, 1.0, 2);
+  EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
+}
+
+TEST(VelocityIterator, acceleration)
+{
+  OneDVelocityIterator it(2.0, 0.0, 5.0, 0.5, -0.5, 1.0, 2);
+  EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
+}
+
+
+TEST(VelocityIterator, time)
+{
+  OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 0.5, 2);
+  EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
+}
+
+TEST(VelocityIterator, samples)
+{
+  OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 3);
+  EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 2.0, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
+  ++it;
+  EXPECT_TRUE(it.isFinished());
+}
+
+
+TEST(VelocityIterator, samples2)
+{
+  OneDVelocityIterator it(2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 5);
+  EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 1.5, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 2.0, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 2.5, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 3.0, EPSILON);
+  ++it;
+  EXPECT_TRUE(it.isFinished());
+}
+
+TEST(VelocityIterator, around_zero)
+{
+  OneDVelocityIterator it(0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 2);
+  EXPECT_NEAR(it.getVelocity(), -1.0, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 0.0, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
+  ++it;
+}
+
+TEST(VelocityIterator, around_zero2)
+{
+  OneDVelocityIterator it(0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 4);
+  EXPECT_NEAR(it.getVelocity(), -1.0, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), -0.3333, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 0.0, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 0.3333, EPSILON);
+  ++it;
+  EXPECT_NEAR(it.getVelocity(), 1.0, EPSILON);
+  ++it;
+}
+
+TEST(VelocityIterator, acceleration_magnitude)
+{
+  double accel_rate = 2.0;
+  double decel_rate = -3.0;
+  EXPECT_NEAR(projectVelocity(+0.0, accel_rate, decel_rate, 0.1, +1.5), +0.2, EPSILON);
+  EXPECT_NEAR(projectVelocity(+0.0, accel_rate, decel_rate, 0.1, -1.5), -0.2, EPSILON);
+  EXPECT_NEAR(projectVelocity(+1.0, accel_rate, decel_rate, 0.1, +1.5), +1.2, EPSILON);
+  EXPECT_NEAR(projectVelocity(+1.0, accel_rate, decel_rate, 0.1, +0.5), +0.7, EPSILON);
+  EXPECT_NEAR(projectVelocity(-1.0, accel_rate, decel_rate, 0.1, -1.5), -1.2, EPSILON);
+  EXPECT_NEAR(projectVelocity(-1.0, accel_rate, decel_rate, 0.1, -0.5), -0.7, EPSILON);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}