git commit -m "first commit for v2"

Devices/Packages/ros_kinematics/include/ros_kinematics/base_drive.h

@@ -0,0 +1,41 @@
+#ifndef _ROS_KINEMATICS_BASE_DRIVE_H_INCLUDED_
+#define _ROS_KINEMATICS_BASE_DRIVE_H_INCLUDED_
+
+#include <realtime_tools/realtime_buffer.h>
+
+namespace ros_kinematics
+{
+  class BaseDrive
+  {
+  protected:
+    class DriveCmd
+    {
+    struct Linear_st
+    {
+      double x; 
+      double y;
+      double z;
+      Linear_st() : x(0.0), y(0.0), z(0.0) {}
+    };
+
+    struct Angular_st
+    {
+      double x; 
+      double y;
+      double z;
+      Angular_st() : x(0.0), y(0.0), z(0.0) {}
+    };
+    
+    public:
+      Linear_st linear;
+      Angular_st angular;
+      ros::Time stamp;
+      DriveCmd() : stamp(0.0) {}
+    };
+
+    realtime_tools::RealtimeBuffer<DriveCmd> command_;
+    DriveCmd command_struct_;
+  };
+}
+
+#endif

Devices/Packages/ros_kinematics/include/ros_kinematics/odometry.h

@@ -0,0 +1,212 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  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 PAL Robotics 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 OWNER 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.
+ *********************************************************************/
+
+/*
+ * Author: Luca Marchionni
+ * Author: Bence Magyar
+ * Author: Enrique Fernández
+ * Author: Paul Mathieu
+ */
+
+#ifndef __ROS_KINEMATICS_ODOMETRY_H_INCLUDED_ 
+#define __ROS_KINEMATICS_ODOMETRY_H_INCLUDED_ 
+
+#include <ros/time.h>
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics/stats.hpp>
+#include <boost/accumulators/statistics/rolling_mean.hpp>
+#include <boost/function.hpp>
+
+namespace ros_kinematics
+{
+  namespace bacc = boost::accumulators;
+
+  /**
+   * \brief The Odometry class handles odometry readings
+   * (2D pose and velocity with related timestamp)
+   */
+  class Odometry
+  {
+  public:
+
+    /// Integration function, used to integrate the odometry:
+    typedef boost::function<void(double, double)> IntegrationFunction;
+
+    /**
+     * \brief Constructor
+     * Timestamp will get the current time value
+     * Value will be set to zero
+     * \param velocity_rolling_window_size Rolling window size used to compute the velocity mean
+     */
+    Odometry(size_t velocity_rolling_window_size = 10);
+
+    /**
+     * \brief Initialize the odometry
+     * \param time Current time
+     */
+    void init(const ros::Time &time);
+
+    /**
+     * \brief Updates the odometry class with latest wheels position
+     * \param left_pos  Left  wheel position [rad]
+     * \param right_pos Right wheel position [rad]
+     * \param time      Current time
+     * \return true if the odometry is actually updated
+     */
+    bool update(double left_pos, double right_pos, const ros::Time &time);
+
+    /**
+     * \brief Updates the odometry class with latest velocity command
+     * \param linear  Linear velocity [m/s]
+     * \param angular Angular velocity [rad/s]
+     * \param time    Current time
+     */
+    void updateOpenLoop(double linear, double angular, const ros::Time &time);
+
+    /**
+     * \brief heading getter
+     * \return heading [rad]
+     */
+    double getHeading() const
+    {
+      return heading_;
+    }
+
+    /**
+     * \brief x position getter
+     * \return x position [m]
+     */
+    double getX() const
+    {
+      return x_;
+    }
+
+    /**
+     * \brief y position getter
+     * \return y position [m]
+     */
+    double getY() const
+    {
+      return y_;
+    }
+
+    /**
+     * \brief linear velocity getter
+     * \return linear velocity [m/s]
+     */
+    double getLinear() const
+    {
+      return fabs(linear_) > 1e-9? linear_ : 0.0;
+    }
+
+    /**
+     * \brief angular velocity getter
+     * \return angular velocity [rad/s]
+     */
+    double getAngular() const
+    {
+      return fabs(angular_) > 1e-9? angular_ : 0.0;;
+    }
+
+    /**
+     * \brief Sets the wheel parameters: radius and separation
+     * \param wheel_separation   Separation between left and right wheels [m]
+     * \param left_wheel_radius  Left wheel radius [m]
+     * \param right_wheel_radius Right wheel radius [m]
+     */
+    void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius);
+
+    /**
+     * \brief Velocity rolling window size setter
+     * \param velocity_rolling_window_size Velocity rolling window size
+     */
+    void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
+
+  private:
+
+    /// Rolling mean accumulator and window:
+    typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
+    typedef bacc::tag::rolling_window RollingWindow;
+
+    /**
+     * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
+     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
+     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
+     */
+    void integrateRungeKutta2(double linear, double angular);
+
+    /**
+     * \brief Integrates the velocities (linear and angular) using exact method
+     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
+     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
+     */
+    void integrateExact(double linear, double angular);
+
+    /**
+     *  \brief Reset linear and angular accumulators
+     */
+    void resetAccumulators();
+
+    /// Current timestamp:
+    ros::Time timestamp_;
+
+    /// Current pose:
+    double x_;        //   [m]
+    double y_;        //   [m]
+    double heading_;  // [rad]
+
+    /// Current velocity:
+    double linear_;  //   [m/s]
+    double angular_; // [rad/s]
+
+    /// Wheel kinematic parameters [m]:
+    double wheel_separation_;
+    double left_wheel_radius_;
+    double right_wheel_radius_;
+
+    /// Previou wheel position/state [rad]:
+    double left_wheel_old_pos_;
+    double right_wheel_old_pos_;
+
+    /// Rolling mean accumulators for the linar and angular velocities:
+    size_t velocity_rolling_window_size_;
+    RollingMeanAcc linear_acc_;
+    RollingMeanAcc angular_acc_;
+
+    /// Integration funcion, used to integrate the odometry:
+    IntegrationFunction integrate_fun_;
+  };
+}
+
+#endif

Devices/Packages/ros_kinematics/include/ros_kinematics/ros_diff_drive.h

@@ -0,0 +1,194 @@
+#ifndef _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_
+#define _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <ros_kinematics/base_drive.h>
+#include <ros_kinematics/odometry.h>
+#include <ros_kinematics/speed_limiter.h>
+#include <ros_kinematics/ros_diff_drive_parameters.h>
+
+#include <memory>
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <control_msgs/JointTrajectoryControllerState.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose2D.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/transform_broadcaster.h>
+// Boost
+#include <boost/thread.hpp>
+// plugin
+#include <models/BaseSteerDrive.h>
+#include <models/BaseAbsoluteEncoder.h>
+#include <pluginlib/class_loader.h>
+
+namespace ros_kinematics
+{
+  class RosDiffDrive : public BaseDrive
+  {
+  public:
+    /**
+     * @brief Constructor
+     */
+    RosDiffDrive(ros::NodeHandle& nh, const std::string& name);
+
+    /**
+     * @brief Deconstructor
+     */
+    virtual ~RosDiffDrive();
+
+  private:
+
+    void CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
+    /**
+     * @brief returns true, if publishing of a cmd_vel topic is publishing
+     * 
+    */
+    bool isCmdVelTriggered(void);
+
+    /**
+     *  @brief returns true, if not subcribe cmd_vel
+    */
+    bool isCmdVelLastestTriggered(void);
+
+    /**
+     * @brief schedules MotorController function 
+     * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
+    */
+    void scheduleMotorController(bool schedule);
+    
+    void updateDynamicParams();
+    void updateChild(void);
+    void updateOdometryEncoder(const ros::Time& current_time);
+    void publishOdometry(const ros::Time& current_time);
+    void brake();
+    bool allReady();
+
+    /**
+     * @brief Sets the odometry publishing fields
+     * @param root_nh Root node handle
+     * @param controller_nh Node handle inside the controller namespace
+     */
+    void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
+    
+    /**
+     * @brief Get the wheel names from a wheel param
+     * @param [in]  controller_nh Controller node handler
+     * @param [in]  wheel_param   Param name
+     * @param [out] wheel_names   Vector with the whel names
+     * @return true if the wheel_param is available and the wheel_names are
+     *        retrieved successfully from the param server; false otherwise
+     */
+    bool getWheelNames(ros::NodeHandle& controller_nh, const std::string& wheel_param, std::vector<std::string>& wheel_names);
+
+    bool getWheelParams(ros::NodeHandle& controller_nh, const std::vector<std::string>& wheel_names, 
+                          std::map<std::string, XmlRpc::XmlRpcValue>& drive_param_map);
+
+    // properties
+    bool initialized_;
+    std::string name_;
+    ros::NodeHandle nh_;
+    ros::NodeHandle nh_priv_;
+
+    pluginlib::ClassLoader<models::BaseSteerDrive> motor_loader_;
+    std::vector<boost::shared_ptr<models::BaseSteerDrive> > left_drive_;
+    std::vector<boost::shared_ptr<models::BaseSteerDrive> > right_drive_;
+    
+    std::map<std::string, XmlRpc::XmlRpcValue> left_drive_param_map_;
+    std::map<std::string, XmlRpc::XmlRpcValue> right_drive_param_map_;
+    boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > left_wheel_tf_pub_;
+    boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > right_wheel_tf_pub_;
+
+    bool use_abs_encoder;
+    pluginlib::ClassLoader<models::BaseAbsoluteEncoder> encoder_loader_;
+    std::vector<boost::shared_ptr<models::BaseAbsoluteEncoder> > left_encoder_;
+    std::vector<boost::shared_ptr<models::BaseAbsoluteEncoder> > right_encoder_;
+    double left_wheel_curr_pos_;
+    double right_wheel_curr_pos_;
+
+
+
+    ros::Subscriber cmd_vel_subscriber_;
+    int m_subscribe_queue_size_;
+    boost::shared_ptr<boost::thread> callback_thread_;
+    ros::Duration schedule_delay_;
+    ros::Duration schedule_lastest_delay_;
+    ros::Time publish_time_;
+    ros::Time publish_lastest_time_;
+    boost::mutex publish_mutex_;
+    /// Publish executed commands
+    boost::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> > cmd_vel_pub_;
+
+    ros_kinematics::Odometry odometry_;
+    ros::Duration publish_period_;
+    ros::Time last_odom_state_publish_time_;
+    bool open_loop_;
+    ros::Time last_odom_update_;
+    std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
+    std::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
+
+    /// Wheel separation, wrt the midpoint of the wheel width:
+    double wheel_separation_;
+
+    /// Wheel radius (assuming it's the same for the left and right wheels):
+    double wheel_radius_;
+
+    /// Wheel separation and radius calibration multipliers:
+    double wheel_separation_multiplier_;
+    double left_wheel_radius_multiplier_;
+    double right_wheel_radius_multiplier_;
+
+    /// Timeout to consider cmd_vel commands old:
+    double cmd_vel_timeout_;
+
+    /// Whether to allow multiple publishers on cmd_vel topic or not:
+    bool allow_multiple_cmd_vel_publishers_;
+
+    /// Frame to use for the robot base:
+    std::string base_frame_id_;
+
+    /// Frame to use for odometry and odom tf:
+    std::string odom_frame_id_;
+
+    /// Whether to publish odometry to tf or not:
+    bool enable_odom_tf_;
+
+    /// Whether to publish wheel link to tf or not:
+    bool enable_wheel_tf_;
+
+    /// Number of wheel joints:
+    size_t wheel_joints_size_;
+    boost::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster_;
+
+    /// Speed limiters:
+    ros_kinematics::BaseDrive::DriveCmd last1_cmd_;
+    ros_kinematics::BaseDrive::DriveCmd last0_cmd_;
+    ros_kinematics::SpeedLimiter limiter_lin_;
+    ros_kinematics::SpeedLimiter limiter_ang_;
+
+    /// Publish limited velocity:
+    bool publish_cmd_;
+    // Update Rate
+    double update_rate_;
+
+    
+    /// Dynamic Reconfigure server
+    boost::shared_ptr<ros_kinematics::KinematicDiffDriveParameters> kinematics_param_ptr_;
+
+    std::string odometry_topic_;
+    std::string odometry_frame_;
+    std::string odometry_enc_topic_;
+    std::string odometry_enc_child_frame_;
+    std::string command_topic_;
+
+    // Flags
+    bool publishOdomTF_;
+
+    // bool publishWheelJointState_;
+    bool odom_enc_initialized_;
+  };
+}
+#endif // _ROS_KINEMATICS_DIFF_DRIVE_H_INCLUDED_

Devices/Packages/ros_kinematics/include/ros_kinematics/ros_diff_drive_parameters.h

@@ -0,0 +1,51 @@
+#ifndef __ROS_KINEMATICS_DIFF_DRIVE_PARAMETERS_H_INCLUDED_
+#define __ROS_KINEMATICS_DIFF_DRIVE_PARAMETERS_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <dynamic_reconfigure/server.h>
+#include <ros_kinematics/DiffDriveParametersConfig.h>
+#include <memory>
+#include <boost/thread.hpp>
+#include <dynamic_reconfigure/Config.h>
+
+namespace ros_kinematics
+{
+/**
+ * @class KinematicDiffDriveParameters
+ * @brief A dynamically reconfigurable class containing one representation of the robot's kinematics
+ */
+class KinematicDiffDriveParameters
+{
+public:
+  KinematicDiffDriveParameters();
+  KinematicDiffDriveParameters(const ros::NodeHandle& nh);
+  void initialize(const ros::NodeHandle& nh);
+
+  inline double getLeftWheelRadiusMultiplier()    { return left_wheel_radius_multiplier_; }
+  inline double getRightWheelRadiusMultiplier()   { return right_wheel_radius_multiplier_; }
+  inline double getWheelSeparationMultiplier()    { return wheel_separation_multiplier_; }
+  inline double getPublishRate()                  { return publish_rate_; }
+  inline bool   getEnableOdomTf()                 { return enable_odom_tf_; }
+  inline double isSetup() {return setup_;}
+  using Ptr = std::shared_ptr<KinematicDiffDriveParameters>;
+protected:
+  void reconfigureCB(DiffDriveParametersConfig &config, uint32_t level);
+  void kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param);
+  
+  bool initialized_;
+  bool setup_;
+  // For parameter descriptions, see cfg/KinematicParams.cfg
+  double left_wheel_radius_multiplier_;
+  double right_wheel_radius_multiplier_;
+  double wheel_separation_multiplier_;
+  double publish_rate_;
+  bool   enable_odom_tf_;
+
+  boost::mutex reconfig_mutex;
+  ros::NodeHandle nh_;
+  ros::Subscriber kinematics_sub_;
+  std::shared_ptr<dynamic_reconfigure::Server<DiffDriveParametersConfig> > dsrv_;
+};
+
+}  // namespace ros_kinematics
+#endif

Devices/Packages/ros_kinematics/include/ros_kinematics/ros_steer_drive.h

@@ -0,0 +1,118 @@
+#ifndef _ROS_KINEMATICS_STEER_DRIVE_H_INCLUDED_
+#define _ROS_KINEMATICS_STEER_DRIVE_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <ros_kinematics/base_drive.h>
+#include <ros_kinematics/ros_steer_drive_parameters.h>
+
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose2D.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
+// Boost
+#include <boost/thread.hpp>
+// plugin
+#include <models/BaseSteerDrive.h>
+#include <models/BaseAbsoluteEncoder.h>
+#include <pluginlib/class_loader.h>
+
+namespace ros_kinematics
+{
+  class RosSteerDrive : public BaseDrive
+  {
+  public:
+    RosSteerDrive(ros::NodeHandle & nh, const std::string &name);
+    virtual ~RosSteerDrive();
+  private:
+
+    void CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
+
+    /**
+     * @brief returns true, if publishing of a cmd_vel topic is publishing
+     * 
+    */
+    bool isCmdVelTriggered(void);
+
+    /**
+     *  @brief returns true, if not subcribe cmd_vel
+    */
+    bool isCmdVelLastestTriggered(void);
+
+    /**
+     * @brief schedules MotorController function 
+     * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
+    */
+    void scheduleMotorController(bool schedule);
+
+    void MotorController(double target_speed, double target_steering_speed, double dt);
+
+    void UpdateOdometryEncoder();
+    void UpdateChild(void);
+    void PublishOdometry(double step_time);
+    void publishSteerJoint(double odom_alpha);
+
+    pluginlib::ClassLoader<models::BaseSteerDrive> motor_loader_;
+    std::string steer_motor_mode_, traction_motor_mode_;
+    boost::shared_ptr<models::BaseSteerDrive> steer_motor_;
+    boost::shared_ptr<models::BaseSteerDrive> traction_motor_;
+
+    bool use_abs_encoder;
+    pluginlib::ClassLoader<models::BaseAbsoluteEncoder> encoder_loader_;
+    boost::shared_ptr<models::BaseAbsoluteEncoder> absolute_encoder_;    
+
+    ros::NodeHandle nh_;
+    ros::NodeHandle nh_priv_;
+
+    ros::Publisher odometry_publisher_;
+    ros::Publisher odometry_enc_publisher_;
+    ros::Publisher cmd_vel_feedback_;
+    ros::Publisher steer_angle_pub_;
+    ros::Publisher cmd_vel_rb;
+    ros::Subscriber cmd_vel_subscriber_;
+    boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_;
+    int m_subscribe_queue_size_;
+    boost::mutex callback_mutex_;
+    boost::thread *callback_thread_;
+
+    nav_msgs::Odometry odom_;
+    nav_msgs::Odometry odom_enc_;
+    geometry_msgs::Pose2D pose_encoder_;
+    ros::Time last_odom_update_;
+
+    // Update Rate
+    double update_rate_;
+    double update_period_;
+    ros::Time last_actuator_update_;
+    ros::Time publish_time_;
+    ros::Time publish_lastest_time_;
+    ros::Duration schedule_delay_;
+    ros::Duration schedule_lastest_delay_;
+    boost::mutex publish_mutex_;
+
+    boost::shared_ptr<ros_kinematics::KinematicSteerDriveParameters> kinematics_param_ptr;
+    double steering_fix_wheel_distance_x_;
+    double steering_fix_wheel_distance_y_;
+    double odom_enc_steering_angle_offset_;
+
+    double wheel_diameter_;
+    double wheel_acceleration_;
+    double steering_ratio_;
+    double traction_ratio_;
+
+    std::string odometry_topic_;
+    std::string odometry_frame_;
+    std::string odometry_enc_topic_;
+    std::string odometry_enc_child_frame_;
+    std::string robot_base_frame_;
+    std::string command_topic_;
+    std::string steer_id_;
+
+    // Flags
+    bool publishOdomTF_;
+    // bool publishWheelJointState_;
+    bool odom_enc_initialized_;
+  };
+}
+#endif

Devices/Packages/ros_kinematics/include/ros_kinematics/ros_steer_drive_parameters.h

@@ -0,0 +1,48 @@
+#ifndef __ROS_KINEMATICS_STEER_DRIVE_PARAMETERS_H_INCLUDED_
+#define __ROS_KINEMATICS_STEER_DRIVE_PARAMETERS_H_INCLUDED_
+
+#include <ros/ros.h>
+#include <dynamic_reconfigure/server.h>
+#include <ros_kinematics/SteerDriveParametersConfig.h>
+#include <memory>
+#include <boost/thread.hpp>
+#include <dynamic_reconfigure/Config.h>
+
+namespace ros_kinematics
+{
+/**
+ * @class KinematicSteerDriveParameters
+ * @brief A dynamically reconfigurable class containing one representation of the robot's kinematics
+ */
+class KinematicSteerDriveParameters
+{
+public:
+  KinematicSteerDriveParameters();
+  KinematicSteerDriveParameters(const ros::NodeHandle& nh);
+  void initialize(const ros::NodeHandle& nh);
+
+  inline double getOdomEncSteeringAngleOffset() { return odom_enc_steering_angle_offset_; }
+  inline double getSteeringFixWheelDistanceX()  { return steering_fix_wheel_distance_x_; }
+  inline double getSteeringFixWheelDistanceY()  { return steering_fix_wheel_distance_y_; }
+  inline double getWheelAcceleration()          { return wheel_acceleration_; }
+  inline double isSetup() {return setup_;}
+  using Ptr = std::shared_ptr<KinematicSteerDriveParameters>;
+protected:
+  void reconfigureCB(SteerDriveParametersConfig &config, uint32_t level);
+  void kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param);
+  
+  bool initialized_;
+  bool setup_;
+  // For parameter descriptions, see cfg/KinematicParams.cfg
+  double odom_enc_steering_angle_offset_;
+  double steering_fix_wheel_distance_x_;
+  double steering_fix_wheel_distance_y_;
+  double wheel_acceleration_;
+  boost::mutex reconfig_mutex;
+  ros::NodeHandle nh_;
+  ros::Subscriber kinematics_sub_;
+  std::shared_ptr<dynamic_reconfigure::Server<SteerDriveParametersConfig> > dsrv_;
+};
+
+}  // namespace ros_kinematics
+#endif

Devices/Packages/ros_kinematics/include/ros_kinematics/speed_limiter.h

@@ -0,0 +1,131 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  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 PAL Robotics 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 OWNER 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.
+ *********************************************************************/
+
+/*
+ * Author: Enrique Fernández
+ */
+
+#ifndef __ROS_KINEMATICS_SPEED_LIMITER_H_INCLUDED_
+#define __ROS_KINEMATICS_SPEED_LIMITER_H_INCLUDED_
+
+namespace ros_kinematics
+{
+
+  class SpeedLimiter
+  {
+  public:
+
+    /**
+     * \brief Constructor
+     * \param [in] has_velocity_limits     if true, applies velocity limits
+     * \param [in] has_acceleration_limits if true, applies acceleration limits
+     * \param [in] has_jerk_limits         if true, applies jerk limits
+     * \param [in] min_velocity Minimum velocity [m/s], usually <= 0
+     * \param [in] max_velocity Maximum velocity [m/s], usually >= 0
+     * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0
+     * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
+     * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
+     * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
+     */
+    SpeedLimiter(
+      bool has_velocity_limits = false,
+      bool has_acceleration_limits = false,
+      bool has_jerk_limits = false,
+      double min_velocity = 0.0,
+      double max_velocity = 0.0,
+      double min_acceleration = 0.0,
+      double max_acceleration = 0.0,
+      double min_jerk = 0.0,
+      double max_jerk = 0.0
+    );
+
+    /**
+     * \brief Limit the velocity and acceleration
+     * \param [in, out] v  Velocity [m/s]
+     * \param [in]      v0 Previous velocity to v  [m/s]
+     * \param [in]      v1 Previous velocity to v0 [m/s]
+     * \param [in]      dt Time step [s]
+     * \return Limiting factor (1.0 if none)
+     */
+    double limit(double& v, double v0, double v1, double dt);
+
+    /**
+     * \brief Limit the velocity
+     * \param [in, out] v Velocity [m/s]
+     * \return Limiting factor (1.0 if none)
+     */
+    double limit_velocity(double& v);
+
+    /**
+     * \brief Limit the acceleration
+     * \param [in, out] v  Velocity [m/s]
+     * \param [in]      v0 Previous velocity [m/s]
+     * \param [in]      dt Time step [s]
+     * \return Limiting factor (1.0 if none)
+     */
+    double limit_acceleration(double& v, double v0, double dt);
+
+    /**
+     * \brief Limit the jerk
+     * \param [in, out] v  Velocity [m/s]
+     * \param [in]      v0 Previous velocity to v  [m/s]
+     * \param [in]      v1 Previous velocity to v0 [m/s]
+     * \param [in]      dt Time step [s]
+     * \return Limiting factor (1.0 if none)
+     * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control
+     */
+    double limit_jerk(double& v, double v0, double v1, double dt);
+
+  public:
+    // Enable/Disable velocity/acceleration/jerk limits:
+    bool has_velocity_limits;
+    bool has_acceleration_limits;
+    bool has_jerk_limits;
+
+    // Velocity limits:
+    double min_velocity;
+    double max_velocity;
+
+    // Acceleration limits:
+    double min_acceleration;
+    double max_acceleration;
+
+    // Jerk limits:
+    double min_jerk;
+    double max_jerk;
+  };
+
+} // namespace ros_kinematics
+
+#endif