git commit -m "first commit for v2"

Devices/Packages/ros_kinematics/.gitignore

@@ -0,0 +1,414 @@
+# ---> VisualStudio
+## Ignore Visual Studio temporary files, build results, and
+## files generated by popular Visual Studio add-ons.
+##
+## Get latest from https://github.com/github/gitignore/blob/main/VisualStudio.gitignore
+
+# User-specific files
+*.rsuser
+*.suo
+*.user
+*.userosscache
+*.sln.docstates
+
+# User-specific files (MonoDevelop/Xamarin Studio)
+*.userprefs
+
+# Mono auto generated files
+mono_crash.*
+
+# Build results
+[Dd]ebug/
+[Dd]ebugPublic/
+[Rr]elease/
+[Rr]eleases/
+x64/
+x86/
+[Ww][Ii][Nn]32/
+[Aa][Rr][Mm]/
+[Aa][Rr][Mm]64/
+bld/
+[Bb]in/
+[Oo]bj/
+[Ll]og/
+[Ll]ogs/
+
+# Visual Studio 2015/2017 cache/options directory
+.vs/
+# Uncomment if you have tasks that create the project's static files in wwwroot
+#wwwroot/
+
+# Visual Studio 2017 auto generated files
+Generated\ Files/
+
+# MSTest test Results
+[Tt]est[Rr]esult*/
+[Bb]uild[Ll]og.*
+
+# NUnit
+*.VisualState.xml
+TestResult.xml
+nunit-*.xml
+
+# Build Results of an ATL Project
+[Dd]ebugPS/
+[Rr]eleasePS/
+dlldata.c
+
+# Benchmark Results
+BenchmarkDotNet.Artifacts/
+
+# .NET Core
+project.lock.json
+project.fragment.lock.json
+artifacts/
+
+# ASP.NET Scaffolding
+ScaffoldingReadMe.txt
+
+# StyleCop
+StyleCopReport.xml
+
+# Files built by Visual Studio
+*_i.c
+*_p.c
+*_h.h
+*.ilk
+*.meta
+*.obj
+*.iobj
+*.pch
+*.pdb
+*.ipdb
+*.pgc
+*.pgd
+*.rsp
+*.sbr
+*.tlb
+*.tli
+*.tlh
+*.tmp
+*.tmp_proj
+*_wpftmp.csproj
+*.log
+*.tlog
+*.vspscc
+*.vssscc
+.builds
+*.pidb
+*.svclog
+*.scc
+
+# Chutzpah Test files
+_Chutzpah*
+
+# Visual C++ cache files
+ipch/
+*.aps
+*.ncb
+*.opendb
+*.opensdf
+*.sdf
+*.cachefile
+*.VC.db
+*.VC.VC.opendb
+
+# Visual Studio profiler
+*.psess
+*.vsp
+*.vspx
+*.sap
+
+# Visual Studio Trace Files
+*.e2e
+
+# TFS 2012 Local Workspace
+$tf/
+
+# Guidance Automation Toolkit
+*.gpState
+
+# ReSharper is a .NET coding add-in
+_ReSharper*/
+*.[Rr]e[Ss]harper
+*.DotSettings.user
+
+# TeamCity is a build add-in
+_TeamCity*
+
+# DotCover is a Code Coverage Tool
+*.dotCover
+
+# AxoCover is a Code Coverage Tool
+.axoCover/*
+!.axoCover/settings.json
+
+# Coverlet is a free, cross platform Code Coverage Tool
+coverage*.json
+coverage*.xml
+coverage*.info
+
+# Visual Studio code coverage results
+*.coverage
+*.coveragexml
+
+# NCrunch
+_NCrunch_*
+.*crunch*.local.xml
+nCrunchTemp_*
+
+# MightyMoose
+*.mm.*
+AutoTest.Net/
+
+# Web workbench (sass)
+.sass-cache/
+
+# Installshield output folder
+[Ee]xpress/
+
+# DocProject is a documentation generator add-in
+DocProject/buildhelp/
+DocProject/Help/*.HxT
+DocProject/Help/*.HxC
+DocProject/Help/*.hhc
+DocProject/Help/*.hhk
+DocProject/Help/*.hhp
+DocProject/Help/Html2
+DocProject/Help/html
+
+# Click-Once directory
+publish/
+
+# Publish Web Output
+*.[Pp]ublish.xml
+*.azurePubxml
+# Note: Comment the next line if you want to checkin your web deploy settings,
+# but database connection strings (with potential passwords) will be unencrypted
+*.pubxml
+*.publishproj
+
+# Microsoft Azure Web App publish settings. Comment the next line if you want to
+# checkin your Azure Web App publish settings, but sensitive information contained
+# in these scripts will be unencrypted
+PublishScripts/
+
+# NuGet Packages
+*.nupkg
+# NuGet Symbol Packages
+*.snupkg
+# The packages folder can be ignored because of Package Restore
+**/[Pp]ackages/*
+# except build/, which is used as an MSBuild target.
+!**/[Pp]ackages/build/
+# Uncomment if necessary however generally it will be regenerated when needed
+#!**/[Pp]ackages/repositories.config
+# NuGet v3's project.json files produces more ignorable files
+*.nuget.props
+*.nuget.targets
+
+# Microsoft Azure Build Output
+csx/
+*.build.csdef
+
+# Microsoft Azure Emulator
+ecf/
+rcf/
+
+# Windows Store app package directories and files
+AppPackages/
+BundleArtifacts/
+Package.StoreAssociation.xml
+_pkginfo.txt
+*.appx
+*.appxbundle
+*.appxupload
+
+# Visual Studio cache files
+# files ending in .cache can be ignored
+*.[Cc]ache
+# but keep track of directories ending in .cache
+!?*.[Cc]ache/
+
+# Others
+ClientBin/
+~$*
+*~
+*.dbmdl
+*.dbproj.schemaview
+*.jfm
+*.pfx
+*.publishsettings
+orleans.codegen.cs
+
+# Including strong name files can present a security risk
+# (https://github.com/github/gitignore/pull/2483#issue-259490424)
+#*.snk
+
+# Since there are multiple workflows, uncomment next line to ignore bower_components
+# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
+#bower_components/
+
+# RIA/Silverlight projects
+Generated_Code/
+
+# Backup & report files from converting an old project file
+# to a newer Visual Studio version. Backup files are not needed,
+# because we have git ;-)
+_UpgradeReport_Files/
+Backup*/
+UpgradeLog*.XML
+UpgradeLog*.htm
+ServiceFabricBackup/
+*.rptproj.bak
+
+# SQL Server files
+*.mdf
+*.ldf
+*.ndf
+
+# Business Intelligence projects
+*.rdl.data
+*.bim.layout
+*.bim_*.settings
+*.rptproj.rsuser
+*- [Bb]ackup.rdl
+*- [Bb]ackup ([0-9]).rdl
+*- [Bb]ackup ([0-9][0-9]).rdl
+
+# Microsoft Fakes
+FakesAssemblies/
+
+# GhostDoc plugin setting file
+*.GhostDoc.xml
+
+# Node.js Tools for Visual Studio
+.ntvs_analysis.dat
+node_modules/
+
+# Visual Studio 6 build log
+*.plg
+
+# Visual Studio 6 workspace options file
+*.opt
+
+# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
+*.vbw
+
+# Visual Studio 6 auto-generated project file (contains which files were open etc.)
+*.vbp
+
+# Visual Studio 6 workspace and project file (working project files containing files to include in project)
+*.dsw
+*.dsp
+
+# Visual Studio 6 technical files
+*.ncb
+*.aps
+
+# Visual Studio LightSwitch build output
+**/*.HTMLClient/GeneratedArtifacts
+**/*.DesktopClient/GeneratedArtifacts
+**/*.DesktopClient/ModelManifest.xml
+**/*.Server/GeneratedArtifacts
+**/*.Server/ModelManifest.xml
+_Pvt_Extensions
+
+# Paket dependency manager
+.paket/paket.exe
+paket-files/
+
+# FAKE - F# Make
+.fake/
+
+# CodeRush personal settings
+.cr/personal
+
+# Python Tools for Visual Studio (PTVS)
+__pycache__/
+*.pyc
+
+# Cake - Uncomment if you are using it
+# tools/**
+# !tools/packages.config
+
+# Tabs Studio
+*.tss
+
+# Telerik's JustMock configuration file
+*.jmconfig
+
+# BizTalk build output
+*.btp.cs
+*.btm.cs
+*.odx.cs
+*.xsd.cs
+
+# OpenCover UI analysis results
+OpenCover/
+
+# Azure Stream Analytics local run output
+ASALocalRun/
+
+# MSBuild Binary and Structured Log
+*.binlog
+
+# NVidia Nsight GPU debugger configuration file
+*.nvuser
+
+# MFractors (Xamarin productivity tool) working folder
+.mfractor/
+
+# Local History for Visual Studio
+.localhistory/
+
+# Visual Studio History (VSHistory) files
+.vshistory/
+
+# BeatPulse healthcheck temp database
+healthchecksdb
+
+# Backup folder for Package Reference Convert tool in Visual Studio 2017
+MigrationBackup/
+
+# Ionide (cross platform F# VS Code tools) working folder
+.ionide/
+
+# Fody - auto-generated XML schema
+FodyWeavers.xsd
+
+# VS Code files for those working on multiple tools
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+*.code-workspace
+
+# Local History for Visual Studio Code
+.history/
+
+# Windows Installer files from build outputs
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# JetBrains Rider
+*.sln.iml
+
+# ---> VisualStudioCode
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+

Devices/Packages/ros_kinematics/CMakeLists.txt

@@ -0,0 +1,239 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(ros_kinematics)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++17)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  pluginlib
+  models
+  geometry_msgs
+  std_msgs
+  dynamic_reconfigure
+  control_msgs
+  realtime_tools
+  urdf
+  tf
+  tf2
+  tf2_ros
+)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):ros_plugins
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   geometry_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+generate_dynamic_reconfigure_options(
+  cfg/SteerDriveParameters.cfg 
+  cfg/DiffDriveParameters.cfg
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ros_kinematics
+  CATKIN_DEPENDS 
+    geometry_msgs 
+    roscpp 
+    rospy 
+    models 
+    pluginlib 
+    tf 
+    tf2 
+    tf2_ros
+    dynamic_reconfigure 
+    control_msgs
+    realtime_tools
+  DEPENDS Boost
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}
+  src/ros_steer_drive.cpp
+  src/ros_steer_drive_parameters.cpp
+  src/ros_diff_drive_parameters.cpp
+  src/odometry.cpp
+  src/speed_limiter.cpp
+  src/ros_diff_drive.cpp
+  ##src/ros_diff_drive_controller.cpp
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/ros_kinematics_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+
+target_link_libraries(${PROJECT_NAME}
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(${PROJECT_NAME}_node
+  ${PROJECT_NAME}
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+install(TARGETS ${PROJECT_NAME}_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_kinematics.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

Devices/Packages/ros_kinematics/README.md

@@ -0,0 +1,2 @@
+# ros_kinematics
+

Devices/Packages/ros_kinematics/cfg/DiffDriveParameters.cfg

@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+
+PACKAGE = 'ros_kinematics'
+
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t
+
+gen = ParameterGenerator()
+
+# Kinematic parameters related
+gen.add("left_wheel_radius_multiplier", double_t, 0, "Left wheel radius multiplier.", 1.0, 0.5, 1.5)
+gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multiplier.", 1.0, 0.5, 1.5)
+gen.add("wheel_separation_multiplier", double_t, 0, "Wheel separation multiplier.", 1.0, 0.5, 1.5)
+
+# Publication related
+gen.add("publish_rate", double_t, 0, "Publish rate of odom.", 50.0, 0.0, 2000.0)
+gen.add("enable_odom_tf", bool_t, 0, "Publish odom frame to tf.", False)
+
+exit(gen.generate(PACKAGE, "ros_kinematics", "DiffDriveParameters"))

Devices/Packages/ros_kinematics/cfg/SteerDriveParameters.cfg

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t
+
+gen = ParameterGenerator()
+
+gen.add('odomEncSteeringAngleOffset', double_t, 0, 'Góc bẻ lái động cơ lái được offset so với gốc origin ', 1.757546557, 0.0, 6.283185)
+gen.add('steering_fix_wheel_distance_x', double_t, 0, 'Khoảng cách tâm O quay quanh trục Z động cơ Steer theo trục X',  1.3268, 0.0, 5.0)
+gen.add('steering_fix_wheel_distance_y', double_t, 0, 'Khoảng cách tâm O quay quanh trục Z động cơ Steer theo trục X', 0.0, 0.0, 5.0)
+gen.add('wheelAcceleration', double_t, 0, 'Gia tốc bánh kéo', 0.0, 0.0, 3.0)
+
+exit(gen.generate('ros_kinematics', 'ros_kinematics', 'SteerDriveParameters'))

Devices/Packages/ros_kinematics/config/ros_diff_drive_controller.yaml

@@ -0,0 +1,64 @@
+# -----------------------------------
+left_wheel  : 'left_wheel_joint'
+right_wheel : 'right_wheel_joint'
+publish_rate: 60              # this is what the real MiR platform publishes (default: 50)
+# These covariances are exactly what the real MiR platform publishes
+pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+enable_odom_tf: false
+# open_loop: false
+# Wheel separation and diameter. These are both optional.
+# diff_drive_controller will attempt to read either one or both from the
+# URDF if not specified as a parameter.
+# We don't set the value here because it's different for each MiR type (100, 250, ...), and
+# the plugin figures out the correct values.
+wheel_separation : 0.445208
+wheel_radius : 0.0625
+
+# Wheel separation and radius multipliers
+wheel_separation_multiplier: 1.0 # default: 1.0
+wheel_radius_multiplier    : 1.0 # default: 1.0
+
+# Velocity commands timeout [s], default 0.5
+cmd_vel_timeout: 0.5
+
+# frame_ids (same as real MiR platform)
+base_frame_id: base_footprint # default: base_link base_footprint
+odom_frame_id: odom           # default: odom 
+
+# Velocity and acceleration limits
+# Whenever a min_* is unspecified, default to -max_*
+linear:
+  x:
+    has_velocity_limits    : true
+    max_velocity           : 1.5  # m/s; move_base max_vel_x: 0.8
+    min_velocity           : -1.5 # m/s
+    has_acceleration_limits: true
+    max_acceleration       : 2.0  # m/s^2; move_base acc_lim_x: 1.5
+    min_acceleration       : -2.0 # m/s^2
+    has_jerk_limits        : true
+    max_jerk               : 3.0  # m/s^3
+angular:
+  z:
+    has_velocity_limits    : true
+    max_velocity           : 2.0  # rad/s; move_base max_rot_vel: 1.0
+    has_acceleration_limits: true
+    max_acceleration       : 1.5 # rad/s^2; move_base acc_lim_th: 2.0
+    has_jerk_limits        : true
+    max_jerk               : 2.5  # rad/s^3
+
+left_wheel_joint: 
+  lookup_name: WheelPlugin
+  max_publish_rate: 50
+  mesurement_topic: left_encoder
+  wheel_topic: left_wheel
+  subscribe_queue_size: 1
+  command_timeout: 5.0
+
+right_wheel_joint:  
+  lookup_name: WheelPlugin
+  max_publish_rate: 50
+  mesurement_topic: right_encoder
+  wheel_topic: right_wheel
+  subscribe_queue_size: 1
+  command_timeout: 5.0

Devices/Packages/ros_kinematics/config/ros_steer_drive.yaml

@@ -0,0 +1,29 @@
+commandTopic: $(arg prefix)cmd_vel
+odometryTopic: odom
+odometryFrame: $(arg prefix)odom
+odometryEncTopic: odom_enc
+odometryEncChildFrame: $(arg prefix)odom_enc
+robotBaseFrame: $(arg prefix)base_link
+steerChildFrame: $(arg prefix)steer_link
+subscribe_queue_size: 1
+max_update_rate: 60
+odomEncSteeringAngleOffset: 0.0
+steering_fix_wheel_distance_x: 0.0
+steering_fix_wheel_distance_y: 0.0
+wheelAcceleration: 0.0
+use_abs_encoder: false
+
+steer_drive: tzbot_motor_wheel::SteerMotor
+SteerMotor:
+  ratio: 171
+  node_id: node1
+  max_publish_rate: 100
+  subscribe_queue_size: 1
+
+traction_drive: tzbot_motor_wheel::TractionMotor
+TractionMotor:
+  wheel_diameter: 0.210
+  ratio: 24.68
+  node_id: node2
+  max_publish_rate: 100
+  subscribe_queue_size: 1

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

Devices/Packages/ros_kinematics/launch/diff_drive.launch

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="ros_kinematics_yaml" default="$(find ros_kinematics)/config/ros_diff_drive_controller.yaml"/>
+  <arg name="use_encoder" default="false"/>
+  <arg name="type" default="2"/>
+  <node pkg="ros_kinematics" type="ros_kinematics_node" name="ros_kinematics" output="screen"> 
+    <rosparam file="$(arg ros_kinematics_yaml)" command="load" />
+    <param name="use_encoder" type="bool" value="$(arg use_encoder)"/>
+    <param name="type" type="int" value="$(arg type)"/>
+    <remap from="/ros_kinematics/odom" to="/odom" />
+  </node>
+   
+</launch>  

Devices/Packages/ros_kinematics/launch/steer_drive.launch

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="ros_kinematics_yaml" default="$(find ros_kinematics)/config/ros_steer_drive.yaml"/>
+  <arg name="use_encoder" default="false"/>
+  <arg name="type" default="1"/>
+  <node pkg="ros_kinematics" type="ros_kinematics_node" name="ros_kinematics"> 
+    <rosparam file="$(arg ros_kinematics_yaml)" command="load" />
+    <param name="use_encoder" type="bool" value="$(arg use_encoder)"/>
+    <param name="type" type="int" value="$(arg type)"/>
+  </node>
+   
+</launch>  

Devices/Packages/ros_kinematics/package.xml

@@ -0,0 +1,96 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ros_kinematics</name>
+  <version>0.0.0</version>
+  <description>The ros_kinematics package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="robotics@todo.todo">robotics</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ros_kinematics</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+    <exec_depend>message_runtime</exec_depend>
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>models</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>control_msgs</build_depend>
+  <build_depend>realtime_tools</build_depend>
+
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>models</build_export_depend>
+  <build_export_depend>pluginlib</build_export_depend>
+  <build_export_depend>tf</build_export_depend>
+  <build_export_depend>dynamic_reconfigure</build_export_depend>
+  <build_export_depend>tf2</build_export_depend>
+  <build_export_depend>tf2_ros</build_export_depend>
+  <build_export_depend>control_msgs</build_export_depend>
+  <build_export_depend>realtime_tools</build_export_depend>
+
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>models</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>tf</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+  <exec_depend>tf2</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>control_msgs</exec_depend>
+  <exec_depend>realtime_tools</exec_depend>
+  
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+  <!-- <controller_interface plugin="${prefix}/plugins.xml"/> -->
+  </export>
+</package>

Devices/Packages/ros_kinematics/src/odometry.cpp

@@ -0,0 +1,173 @@
+/*********************************************************************
+ * 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
+ */
+#include <ros/ros.h>
+#include <ros_kinematics/odometry.h>
+
+namespace ros_kinematics
+{
+  namespace bacc = boost::accumulators;
+
+  Odometry::Odometry(size_t velocity_rolling_window_size)
+  : timestamp_(0.0)
+  , x_(0.0)
+  , y_(0.0)
+  , heading_(0.0)
+  , linear_(0.0)
+  , angular_(0.0)
+  , wheel_separation_(0.0)
+  , left_wheel_radius_(0.0)
+  , right_wheel_radius_(0.0)
+  , left_wheel_old_pos_(0.0)
+  , right_wheel_old_pos_(0.0)
+  , velocity_rolling_window_size_(velocity_rolling_window_size)
+  , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
+  , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
+  , integrate_fun_(std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2))
+  {
+  }
+
+  void Odometry::init(const ros::Time& time)
+  {
+    // Reset accumulators and timestamp:
+    resetAccumulators();
+    timestamp_ = time;
+  }
+
+  bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
+  {
+    /// Get current wheel joint positions:
+    const double left_wheel_cur_pos  = left_pos  * left_wheel_radius_;
+    const double right_wheel_cur_pos = right_pos * right_wheel_radius_;
+    
+    /// Estimate velocity of wheels using old and current position:
+    const double left_wheel_est_vel  = left_wheel_cur_pos  - left_wheel_old_pos_;
+    const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
+
+    /// Update old position with current:
+    left_wheel_old_pos_  = left_wheel_cur_pos;
+    right_wheel_old_pos_ = right_wheel_cur_pos;
+
+    /// Compute linear and angular diff:
+    const double linear  = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
+    const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;
+
+    /// Integrate odometry:
+    integrate_fun_(linear, angular);
+
+    /// We cannot estimate the speed with very small time intervals:
+    const double dt = (time - timestamp_).toSec();
+    if (dt < 0.0001)
+      return false; // Interval too small to integrate with
+
+    timestamp_ = time;
+
+    /// Estimate speeds using a rolling mean to filter them out:
+    linear_acc_(linear/dt);
+    angular_acc_(angular/dt);
+
+    linear_ = bacc::rolling_mean(linear_acc_);
+    angular_ = bacc::rolling_mean(angular_acc_);
+    return true;
+  }
+
+  void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
+  {
+    /// Save last linear and angular velocity:
+    linear_ = linear;
+    angular_ = angular;
+
+    /// Integrate odometry:
+    const double dt = (time - timestamp_).toSec();
+    timestamp_ = time;
+    integrate_fun_(linear * dt, angular * dt);
+  }
+
+  void Odometry::setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius)
+  {
+    wheel_separation_   = wheel_separation;
+    left_wheel_radius_  = left_wheel_radius;
+    right_wheel_radius_ = right_wheel_radius;
+  }
+
+  void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
+  {
+    velocity_rolling_window_size_ = velocity_rolling_window_size;
+
+    resetAccumulators();
+  }
+
+  void Odometry::integrateRungeKutta2(double linear, double angular)
+  {
+    const double direction = heading_ + angular * 0.5;
+
+    /// Runge-Kutta 2nd order integration:
+    x_       += linear * cos(direction);
+    y_       += linear * sin(direction);
+    heading_ += angular;
+  }
+
+  /**
+   * \brief Other possible integration method provided by the class
+   * \param linear
+   * \param angular
+   */
+  void Odometry::integrateExact(double linear, double angular)
+  {
+    if (fabs(angular) < 1e-6)
+      integrateRungeKutta2(linear, angular);
+    else
+    {
+      /// Exact integration (should solve problems when angular is zero):
+      const double heading_old = heading_;
+      const double r = linear/angular;
+      heading_ += angular;
+      x_       +=  r * (sin(heading_) - sin(heading_old));
+      y_       += -r * (cos(heading_) - cos(heading_old));
+    }
+  }
+
+  void Odometry::resetAccumulators()
+  {
+    linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+    angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
+  }
+
+} // namespace ros_kinematics

Devices/Packages/ros_kinematics/src/ros_diff_drive.cpp

@@ -0,0 +1,824 @@
+#include "ros_kinematics/ros_diff_drive.h"
+#include <std_msgs/UInt16.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Float32.h>
+
+
+ros_kinematics::RosDiffDrive::RosDiffDrive(ros::NodeHandle& nh, const std::string& name)
+  : nh_(nh),
+  name_(name),
+  motor_loader_("models", "models::BaseSteerDrive"),
+  encoder_loader_("models", "models::BaseAbsoluteEncoder"),
+  last_odom_state_publish_time_(ros::Time(0)),
+  left_wheel_curr_pos_(0),
+  right_wheel_curr_pos_(0),
+  publish_time_(ros::Time(0)),
+  initialized_(false)
+{
+  if(!initialized_)
+  {
+    nh_priv_ = ros::NodeHandle(nh_, name_);
+    ROS_INFO("RosDiffDrive get node name is %s", name_.c_str());
+
+    bool init_odom_enc;
+    nh_priv_.param("initialize_odom_enc", init_odom_enc, true);
+    odom_enc_initialized_ = !init_odom_enc;
+
+    nh_priv_.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_);
+    ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by " << wheel_separation_multiplier_ << ".");
+
+    if (nh_priv_.hasParam("wheel_radius_multiplier"))
+    {
+      double wheel_radius_multiplier;
+      nh_priv_.getParam("wheel_radius_multiplier", wheel_radius_multiplier);
+
+      left_wheel_radius_multiplier_ = wheel_radius_multiplier;
+      right_wheel_radius_multiplier_ = wheel_radius_multiplier;
+    }
+    else
+    {
+      nh_priv_.param("left_wheel_radius_multiplier", left_wheel_radius_multiplier_, left_wheel_radius_multiplier_);
+      nh_priv_.param("right_wheel_radius_multiplier", right_wheel_radius_multiplier_, right_wheel_radius_multiplier_);
+    }
+
+    ROS_INFO_STREAM_NAMED(name_, "Left wheel radius will be multiplied by " << left_wheel_radius_multiplier_ << ".");
+    ROS_INFO_STREAM_NAMED(name_, "Right wheel radius will be multiplied by " << right_wheel_radius_multiplier_ << ".");
+
+    int velocity_rolling_window_size = 10;
+    nh_priv_.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
+    ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of " << velocity_rolling_window_size << ".");
+
+    odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
+
+    // Twist command related:
+    nh_priv_.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
+    ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than " << cmd_vel_timeout_ << "s.");
+
+    nh_priv_.param("allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_);
+    ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is " << (allow_multiple_cmd_vel_publishers_ ? "enabled" : "disabled"));
+
+    nh_priv_.param("base_frame_id", base_frame_id_, base_frame_id_);
+    ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
+
+    nh_priv_.param("odom_frame_id", odom_frame_id_, odom_frame_id_);
+    ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_);
+
+    nh_priv_.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
+    ROS_INFO_STREAM_NAMED(name_, "Publishing base frame to tf is " << (enable_odom_tf_ ? "enabled" : "disabled"));
+
+    nh_priv_.param("enable_wheel_tf", enable_wheel_tf_, enable_wheel_tf_);
+    ROS_INFO_STREAM_NAMED(name_, "Publishing wheel frame to tf is " << (enable_wheel_tf_ ? "enabled" : "disabled"));
+
+    nh_priv_.param("commandTopic", command_topic_, std::string("cmd_vel"));
+    // nh_priv_.param("publishOdomTF", publishOdomTF_, false);
+    nh_priv_.param("odometryTopic", odometry_topic_, std::string("odom"));
+    // nh_priv_.param("odometryFrame", odometry_frame_, std::string("odom"));
+    nh_priv_.param("odometryEncTopic", odometry_enc_topic_, std::string("odom_enc"));
+    nh_priv_.param("odometryEncChildFrame", odometry_enc_child_frame_, std::string("odom_enc"));
+    // nh_priv_.param("robotBaseFrame", robot_base_frame_, std::string("base_link"));
+    nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1);
+    nh_priv_.param("publish_rate", update_rate_, 100.0);
+    publish_period_ = ros::Duration(1.0 / update_rate_);
+    nh_priv_.param("open_loop", open_loop_, false);
+
+    // Velocity and acceleration limits:
+    nh_priv_.param("linear/x/has_velocity_limits", limiter_lin_.has_velocity_limits, limiter_lin_.has_velocity_limits);
+    nh_priv_.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
+    nh_priv_.param("linear/x/has_jerk_limits", limiter_lin_.has_jerk_limits, limiter_lin_.has_jerk_limits);
+    nh_priv_.param("linear/x/max_velocity", limiter_lin_.max_velocity, limiter_lin_.max_velocity);
+    nh_priv_.param("linear/x/min_velocity", limiter_lin_.min_velocity, -limiter_lin_.max_velocity);
+    nh_priv_.param("linear/x/max_acceleration", limiter_lin_.max_acceleration, limiter_lin_.max_acceleration);
+    nh_priv_.param("linear/x/min_acceleration", limiter_lin_.min_acceleration, -limiter_lin_.max_acceleration);
+    nh_priv_.param("linear/x/max_jerk", limiter_lin_.max_jerk, limiter_lin_.max_jerk);
+    nh_priv_.param("linear/x/min_jerk", limiter_lin_.min_jerk, -limiter_lin_.max_jerk);
+
+    nh_priv_.param("angular/z/has_velocity_limits", limiter_ang_.has_velocity_limits, limiter_ang_.has_velocity_limits);
+    nh_priv_.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
+    nh_priv_.param("angular/z/has_jerk_limits", limiter_ang_.has_jerk_limits, limiter_ang_.has_jerk_limits);
+    nh_priv_.param("angular/z/max_velocity", limiter_ang_.max_velocity, limiter_ang_.max_velocity);
+    nh_priv_.param("angular/z/min_velocity", limiter_ang_.min_velocity, -limiter_ang_.max_velocity);
+    nh_priv_.param("angular/z/max_acceleration", limiter_ang_.max_acceleration, limiter_ang_.max_acceleration);
+    nh_priv_.param("angular/z/min_acceleration", limiter_ang_.min_acceleration, -limiter_ang_.max_acceleration);
+    nh_priv_.param("angular/z/max_jerk", limiter_ang_.max_jerk, limiter_ang_.max_jerk);
+    nh_priv_.param("angular/z/min_jerk", limiter_ang_.min_jerk, -limiter_ang_.max_jerk);
+    // Publish limited velocity:
+    nh_priv_.param("publish_cmd", publish_cmd_, publish_cmd_);
+    // Use encoder
+    nh_.param(name_ + "/use_encoder", use_abs_encoder, false);
+    // If either parameter is not available, we need to look up the value in the URDF
+    bool lookup_wheel_separation = !nh_priv_.getParam("wheel_separation", wheel_separation_);
+    bool lookup_wheel_radius = !nh_priv_.getParam("wheel_radius", wheel_radius_);
+
+    if(lookup_wheel_separation || lookup_wheel_radius) 
+      ROS_INFO("URDF if not specified as a parameter. We don't set the value here \n wheel_separation: %f,  wheel_radius %f",
+              wheel_separation_, wheel_radius_);
+    
+    // Regardless of how we got the separation and radius, use them
+    // to set the odometry parameters
+    const double ws = wheel_separation_multiplier_ * wheel_separation_;
+    const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
+    const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
+
+    odometry_.setWheelParams(ws, lwr, rwr);
+    ROS_INFO_STREAM_NAMED(name_,
+      "Odometry params : wheel separation " << ws
+      << ", left wheel radius " << lwr
+      << ", right wheel radius " << rwr);
+
+    if (publish_cmd_)
+    {
+      cmd_vel_pub_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped>(nh_priv_, "cmd_vel_out", 100));
+    }
+
+    // Get joint names from the parameter server
+    std::vector<std::string> left_wheel_names, right_wheel_names;
+    if (!getWheelNames(nh_priv_, "left_wheel", left_wheel_names) ||
+      !getWheelNames(nh_priv_, "right_wheel", right_wheel_names))
+    {
+      exit(1);
+    }
+
+    if (!getWheelParams(nh_priv_, left_wheel_names, left_drive_param_map_) ||
+        !getWheelParams(nh_priv_, right_wheel_names, right_drive_param_map_))
+    {
+      exit(1);
+    }
+
+    if (left_wheel_names.size() != right_wheel_names.size()     || 
+        left_drive_param_map_.size() != left_wheel_names.size() || 
+        right_drive_param_map_.size() != right_wheel_names.size()
+       )
+    {
+      ROS_ERROR_STREAM_NAMED(name_,
+        "#left wheels (" << left_wheel_names.size() << ") != " <<
+        "#right wheels (" << right_wheel_names.size() << ").");
+      exit(1);
+    }
+    else
+    {
+      wheel_joints_size_ = left_wheel_names.size();
+
+      left_drive_.resize(wheel_joints_size_);
+      right_drive_.resize(wheel_joints_size_);
+      if (use_abs_encoder)
+      {
+        left_encoder_.resize(wheel_joints_size_);
+        right_encoder_.resize(wheel_joints_size_);
+      }
+    }
+
+    // plugin models
+    ROS_INFO("RosDiffDrive is plugining models...");
+    for (size_t i = 0; i < wheel_joints_size_; ++i)
+    {
+      std::string left_drive;
+      nh_priv_.param(left_wheel_names[i] + "/lookup_name", left_drive, std::string("motor_wheel::LeftMotor"));
+      try
+      {
+        left_drive_[i] = motor_loader_.createInstance(left_drive);
+        // left_drive_[i]->initialize(nh_, name_ + "/" + motor_loader_.getName(left_drive));
+        left_drive_[i]->initialize(nh_, name_ + "/" + left_wheel_names[i]);
+        if (use_abs_encoder)
+        {
+          std::string right_encoder;
+          nh_priv_.param(right_wheel_names[i] + "/encoder/lookup_name", right_encoder, std::string("absolute_encoder::Measurements"));
+          try
+          {
+            right_encoder_[i] = encoder_loader_.createInstance(right_encoder);
+            right_encoder_[i]->initialize(nh_, name_ + "/" + right_wheel_names[i] + "/encoder");
+          }
+          catch (const pluginlib::PluginlibException& ex)
+          {
+            ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
+              right_encoder.c_str(), ex.what());
+            exit(1);
+          }
+        }
+
+      }
+      catch (const pluginlib::PluginlibException& ex)
+      {
+        ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
+          left_drive.c_str(), ex.what());
+        exit(1);
+      }
+
+
+      std::string right_drive;
+      nh_priv_.param(right_wheel_names[i] + "/lookup_name", right_drive, std::string("motor_wheel::RightMotor"));
+      try
+      {
+        right_drive_[i] = motor_loader_.createInstance(right_drive);
+        // right_drive_[i]->initialize(nh_, name_ + "/" + motor_loader_.getName(right_drive));
+        right_drive_[i]->initialize(nh_, name_ + "/" + right_wheel_names[i]);
+
+        if (use_abs_encoder)
+        {
+          std::string left_encoder;
+          nh_priv_.param(left_wheel_names[i] + "/encoder/lookup_name", left_encoder, std::string("absolute_encoder::Measurements"));
+          try
+          {
+            left_encoder_[i] = encoder_loader_.createInstance(left_encoder);
+            left_encoder_[i]->initialize(nh_, name_ + "/" + left_wheel_names[i] + "/encoder");
+          }
+          catch (const pluginlib::PluginlibException& ex)
+          {
+            ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
+              left_encoder.c_str(), ex.what());
+            exit(1);
+          }
+        }
+      }
+      catch (const pluginlib::PluginlibException& ex)
+      {
+        ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
+          right_drive.c_str(), ex.what());
+        exit(1);
+      }
+    }
+
+    if(enable_wheel_tf_) 
+    {
+      left_wheel_tf_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(nh_, "/tf", 100));
+      for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator itr = left_drive_param_map_.begin(); itr != left_drive_param_map_.end(); ++itr)
+      {
+        geometry_msgs::TransformStamped tfs;
+        tfs.child_frame_id = static_cast<std::string>(itr->second["frame_id"]);
+        tfs.header.frame_id = "base_link";
+        tfs.transform.translation.x = static_cast<double>(itr->second["origin"][0]);
+        tfs.transform.translation.y = static_cast<double>(itr->second["origin"][1]);
+        tfs.transform.translation.z = static_cast<double>(itr->second["origin"][2]);
+        double roll = static_cast<double>(itr->second["origin"][3]);
+        double pitch = static_cast<double>(itr->second["origin"][4]);
+        double yaw = static_cast<double>(itr->second["origin"][5]);
+        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+        tfs.transform.rotation = q;
+        left_wheel_tf_pub_->msg_.transforms.push_back(tfs);
+      }
+
+      right_wheel_tf_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(nh_, "/tf", 100));
+      for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator itr = right_drive_param_map_.begin(); itr != right_drive_param_map_.end(); ++itr)
+      {
+        geometry_msgs::TransformStamped tfs;
+        tfs.child_frame_id = static_cast<std::string>(itr->second["frame_id"]);
+        tfs.header.frame_id = "base_link";
+        tfs.transform.translation.x = static_cast<double>(itr->second["origin"][0]);
+        tfs.transform.translation.y = static_cast<double>(itr->second["origin"][1]);
+        tfs.transform.translation.z = static_cast<double>(itr->second["origin"][2]);
+        double roll = static_cast<double>(itr->second["origin"][3]);
+        double pitch = static_cast<double>(itr->second["origin"][4]);
+        double yaw = static_cast<double>(itr->second["origin"][5]);
+        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+        tfs.transform.rotation = q;
+        right_wheel_tf_pub_->msg_.transforms.push_back(tfs);
+      }
+    }
+
+    schedule_delay_ = ros::Duration(1/update_rate_);
+    schedule_lastest_delay_ = ros::Duration(0.5);
+
+    this->setOdomPubFields(nh_, nh_priv_);
+    transform_broadcaster_  = boost::shared_ptr<tf2_ros::TransformBroadcaster>(new tf2_ros::TransformBroadcaster());
+    cmd_vel_subscriber_     = nh_.subscribe(command_topic_, m_subscribe_queue_size_, &ros_kinematics::RosDiffDrive::CmdVelCallback, this);
+    
+    // Kinematics
+    kinematics_param_ptr_ = boost::make_shared<ros_kinematics::KinematicDiffDriveParameters>();
+    kinematics_param_ptr_->initialize(nh_);
+
+    // Thread
+    callback_thread_ =  boost::make_shared<boost::thread>(&ros_kinematics::RosDiffDrive::updateChild, this);
+    
+    ROS_INFO_NAMED("RosDiffDrive","Initializing on %s is successed", name_.c_str());
+    initialized_ = true;
+  }
+}
+
+ros_kinematics::RosDiffDrive::~RosDiffDrive()
+{
+  callback_thread_ = nullptr;
+  kinematics_param_ptr_ = nullptr;
+}
+
+void ros_kinematics::RosDiffDrive::CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg)
+{
+  // check that we don't have multiple publishers on the command topic
+  if (!allow_multiple_cmd_vel_publishers_ && cmd_vel_subscriber_.getNumPublishers() > 1)
+  {
+    ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << cmd_vel_subscriber_.getNumPublishers()
+      << " publishers " << command_topic_ << ". Only 1 publisher is allowed. Going to brake.");
+    brake();
+    return;
+  }
+
+  if (!std::isfinite(cmd_msg->angular.z) || !std::isfinite(cmd_msg->linear.x))
+  {
+    ROS_WARN_THROTTLE(1.0, "Received NaN in velocity command. Ignoring.");
+    return;
+  }
+
+  command_struct_.angular.z = cmd_msg->angular.z;
+  command_struct_.linear.x = cmd_msg->linear.x;
+  command_struct_.stamp = ros::Time::now();
+  command_.writeFromNonRT(command_struct_);
+  this->scheduleMotorController(true);
+  ROS_DEBUG_STREAM_NAMED(name_,
+    "Added values to command. "
+    << "Ang: " << command_struct_.angular.z << ", "
+    << "Lin: " << command_struct_.linear.x << ", "
+    << "Stamp: " << command_struct_.stamp);
+}
+
+bool ros_kinematics::RosDiffDrive::isCmdVelTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  return !publish_time_.isZero() && ros::Time::now() > publish_time_;
+}
+
+
+bool ros_kinematics::RosDiffDrive::isCmdVelLastestTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
+}
+
+
+void ros_kinematics::RosDiffDrive::scheduleMotorController(bool schedule)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  if(schedule && publish_time_.isZero())
+  {
+    publish_time_ = ros::Time::now() + schedule_delay_;
+    publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_;
+  }
+  if(!schedule && !publish_time_.isZero())
+  {
+    publish_time_ = ros::Time(0);
+    publish_lastest_time_ = ros::Time(0);
+  }
+}
+
+void ros_kinematics::RosDiffDrive::updateOdometryEncoder(const ros::Time& current_time)
+{
+  // ros::Time current_time = ros::Time::now();
+  double step_time = ros::Duration(current_time - last_odom_update_).toSec();
+  last_odom_update_ = current_time;
+
+  // Apply (possibly new) multipliers:
+  const double ws = wheel_separation_multiplier_ * wheel_separation_;
+  const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
+  const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
+  odometry_.setWheelParams(ws, lwr, rwr);
+  // COMPUTE AND PUBLISH ODOMETRY
+  if (open_loop_)
+  {
+    odometry_.updateOpenLoop(last0_cmd_.linear.x, last0_cmd_.angular.z, current_time);
+  }
+  else
+  {
+    double left_pos = 0.0;
+    double right_pos = 0.0;
+    for (size_t i = 0; i < wheel_joints_size_; ++i)
+    {
+      double left_speed, right_speed;
+      if (use_abs_encoder)
+      {
+        left_speed = left_encoder_[i]->Velocity();
+        right_speed = right_encoder_[i]->Velocity();
+      }
+      else
+      {
+        left_speed = left_drive_[i]->GetVelocity();
+        right_speed = right_drive_[i]->GetVelocity();
+      }
+
+      const double lp = left_speed  * step_time;  // rad/s -> rad
+      const double rp = right_speed * step_time;
+  
+      if (std::isnan(lp) || std::isnan(rp))
+        return;
+
+      left_pos += lp;
+      right_pos += rp;
+    }
+    left_pos /= wheel_joints_size_;
+    right_pos /= wheel_joints_size_;
+    left_wheel_curr_pos_ += left_pos;
+    right_wheel_curr_pos_ += right_pos;
+    // Estimate linear and angular velocity using joint information
+    odometry_.update(left_wheel_curr_pos_, right_wheel_curr_pos_, current_time);
+
+    if(enable_wheel_tf_)
+    {
+      if(left_wheel_tf_pub_->trylock())
+      {
+        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0, left_wheel_curr_pos_, 0);
+        for(auto &msg_tf : left_wheel_tf_pub_->msg_.transforms)
+        {
+          msg_tf.header.stamp = current_time;
+          msg_tf.transform.rotation = q;
+        }
+        left_wheel_tf_pub_->unlockAndPublish();
+      }
+
+      if(right_wheel_tf_pub_->trylock())
+      {
+        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0, right_wheel_curr_pos_, 0);
+        for(auto &msg_tf : right_wheel_tf_pub_->msg_.transforms)
+        {
+          msg_tf.header.stamp = current_time;
+          msg_tf.transform.rotation = q;
+        }
+        right_wheel_tf_pub_->unlockAndPublish();
+      }
+    }
+
+  }
+}
+
+void ros_kinematics::RosDiffDrive::publishOdometry(const ros::Time& current_time)
+{
+  // Publish odometry message
+  if (last_odom_state_publish_time_ + publish_period_ < current_time)
+  {
+    last_odom_state_publish_time_ += publish_period_;
+    // Compute and store orientation info
+    const geometry_msgs::Quaternion orientation(
+      tf::createQuaternionMsgFromYaw(odometry_.getHeading()));
+
+    // Populate odom message and publish
+    if (odom_pub_ && odom_pub_->trylock())
+    {
+      odom_pub_->msg_.header.stamp = current_time;
+      odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
+      odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
+      odom_pub_->msg_.pose.pose.orientation = orientation;
+      odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
+      odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
+      odom_pub_->unlockAndPublish();
+    }
+
+    // Publish tf /odom frame
+    if (enable_odom_tf_ && tf_odom_pub_->trylock())
+    {
+      geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
+      odom_frame.header.stamp = current_time;
+      odom_frame.transform.translation.x = odometry_.getX();
+      odom_frame.transform.translation.y = odometry_.getY();
+      odom_frame.transform.rotation = orientation;
+      tf_odom_pub_->unlockAndPublish();
+    }
+  }
+}
+
+void ros_kinematics::RosDiffDrive::updateChild(void)
+{
+  ros::Rate rate(update_rate_);
+  last_odom_state_publish_time_ = ros::Time::now();
+  last_odom_update_ = ros::Time::now();
+  odometry_.init(ros::Time::now());
+
+  while (ros::ok())
+  {
+    ros::Time current_time = ros::Time::now();
+    // update parameter from dynamic reconf
+    this->updateDynamicParams();
+
+    const double ws  = wheel_separation_multiplier_ * wheel_separation_;
+    const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
+    const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
+    
+    if (this->allReady())
+    {
+      this->updateOdometryEncoder(current_time);
+      this->publishOdometry(current_time);
+    }
+
+    // MOVE ROBOT
+    DriveCmd curr_cmd = *(command_.readFromRT());
+    const double dt = (current_time - curr_cmd.stamp).toSec();
+
+    // Brake if cmd_vel has timeout:
+    if (dt > cmd_vel_timeout_)
+    {
+      curr_cmd.linear.x = 0.0;
+      curr_cmd.angular.z = 0.0;
+    }
+    
+    // Limit velocities and accelerations:
+    const double cmd_dt(dt);
+
+    limiter_lin_.limit(curr_cmd.linear.x, last0_cmd_.linear.x, last1_cmd_.linear.x, cmd_dt);
+    limiter_ang_.limit(curr_cmd.angular.z, last0_cmd_.angular.z, last1_cmd_.angular.z, cmd_dt);
+
+    last1_cmd_ = last0_cmd_;
+    last0_cmd_ = curr_cmd;
+
+    // Compute wheels velocities:
+    const double vel_left = (curr_cmd.linear.x - curr_cmd.angular.z * ws / 2.0) / lwr;
+    const double vel_right = (curr_cmd.linear.x + curr_cmd.angular.z * ws / 2.0) / rwr;
+    
+    if(this->isCmdVelTriggered())
+    {
+      // Publish limited velocity:
+      if (publish_cmd_ && cmd_vel_pub_ && cmd_vel_pub_->trylock())
+      {
+        cmd_vel_pub_->msg_.header.stamp = current_time;
+        cmd_vel_pub_->msg_.twist.linear.x = curr_cmd.linear.x;
+        cmd_vel_pub_->msg_.twist.angular.z = curr_cmd.angular.z;
+        cmd_vel_pub_->unlockAndPublish();
+      }
+
+      for (size_t i = 0; i < wheel_joints_size_; ++i)
+      {
+        left_drive_[i]->SetVelocity(vel_left);
+        right_drive_[i]->SetVelocity(vel_right);
+      }
+      scheduleMotorController(false);
+    }
+    
+    if(this->isCmdVelLastestTriggered())
+    {
+      int counter = 3;
+      do
+      {
+        for (size_t i = 0; i < wheel_joints_size_; ++i)
+        {
+          left_drive_[i]->SetVelocity(0.0);
+          right_drive_[i]->SetVelocity(0.0);
+        } 
+        counter--;
+        rate.sleep();
+        ros::spinOnce();
+      }while(ros::ok() && counter > 0);
+      scheduleMotorController(false);
+    }
+
+    rate.sleep();
+    ros::spinOnce();
+  }
+
+}
+
+void ros_kinematics::RosDiffDrive::brake()
+{
+  const double vel = 0.0;
+  for (size_t i = 0; i < wheel_joints_size_; ++i)
+  {
+    left_drive_[i]->SetVelocity(vel);
+    right_drive_[i]->SetVelocity(vel);
+  }
+}
+
+void ros_kinematics::RosDiffDrive::updateDynamicParams()
+{
+  // Retreive dynamic params:
+  if (kinematics_param_ptr_ != nullptr)
+  {
+    left_wheel_radius_multiplier_ = kinematics_param_ptr_->getLeftWheelRadiusMultiplier();
+    right_wheel_radius_multiplier_ = kinematics_param_ptr_->getRightWheelRadiusMultiplier();
+    wheel_separation_multiplier_ = kinematics_param_ptr_->getWheelSeparationMultiplier();
+    publish_period_ = ros::Duration(1.0 / kinematics_param_ptr_->getPublishRate());
+    // enable_odom_tf_ = kinematics_param_ptr_->getEnableOdomTf();
+  }
+}
+
+void ros_kinematics::RosDiffDrive::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
+{
+  // Get and check params for covariances
+  XmlRpc::XmlRpcValue pose_cov_list;
+  controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
+  ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
+  ROS_ASSERT(pose_cov_list.size() == 6);
+  for (int i = 0; i < pose_cov_list.size(); ++i)
+    ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+
+  XmlRpc::XmlRpcValue twist_cov_list;
+  controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
+  ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
+  ROS_ASSERT(twist_cov_list.size() == 6);
+  for (int i = 0; i < twist_cov_list.size(); ++i)
+    ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+
+  // Setup odometry realtime publisher + odom message constant fields
+  odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
+  odom_pub_->msg_.header.frame_id = odom_frame_id_;
+  odom_pub_->msg_.child_frame_id = base_frame_id_;
+  odom_pub_->msg_.pose.pose.position.z = 0;
+  odom_pub_->msg_.pose.covariance = {
+      static_cast<double>(pose_cov_list[0]), 0., 0., 0., 0., 0.,
+      0., static_cast<double>(pose_cov_list[1]), 0., 0., 0., 0.,
+      0., 0., static_cast<double>(pose_cov_list[2]), 0., 0., 0.,
+      0., 0., 0., static_cast<double>(pose_cov_list[3]), 0., 0.,
+      0., 0., 0., 0., static_cast<double>(pose_cov_list[4]), 0.,
+      0., 0., 0., 0., 0., static_cast<double>(pose_cov_list[5]) };
+  odom_pub_->msg_.twist.twist.linear.y  = 0;
+  odom_pub_->msg_.twist.twist.linear.z  = 0;
+  odom_pub_->msg_.twist.twist.angular.x = 0;
+  odom_pub_->msg_.twist.twist.angular.y = 0;
+  odom_pub_->msg_.twist.covariance = {
+      static_cast<double>(twist_cov_list[0]), 0., 0., 0., 0., 0.,
+      0., static_cast<double>(twist_cov_list[1]), 0., 0., 0., 0.,
+      0., 0., static_cast<double>(twist_cov_list[2]), 0., 0., 0.,
+      0., 0., 0., static_cast<double>(twist_cov_list[3]), 0., 0.,
+      0., 0., 0., 0., static_cast<double>(twist_cov_list[4]), 0.,
+      0., 0., 0., 0., 0., static_cast<double>(twist_cov_list[5]) };
+  tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
+  tf_odom_pub_->msg_.transforms.resize(1);
+  tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
+  tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
+  tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
+}
+
+
+bool ros_kinematics::RosDiffDrive::getWheelNames(ros::NodeHandle& _nh,
+  const std::string& wheel_param,
+  std::vector<std::string>& wheel_names)
+{
+  XmlRpc::XmlRpcValue wheel_list;
+  if (!nh_priv_.getParam(wheel_param, wheel_list))
+  {
+    ROS_ERROR_STREAM_NAMED(name_,
+      "Couldn't retrieve wheel param '" << wheel_param << "'.");
+    return false;
+  }
+
+  if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
+  {
+    if (wheel_list.size() == 0)
+    {
+      ROS_ERROR_STREAM_NAMED(name_,
+        "Wheel param '" << wheel_param << "' is an empty list");
+      return false;
+    }
+
+    for (int i = 0; i < wheel_list.size(); ++i)
+    {
+      if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
+      {
+        ROS_ERROR_STREAM_NAMED(name_,
+          "Wheel param '" << wheel_param << "' #" << i <<
+          " isn't a string.");
+        return false;
+      }
+    }
+
+    wheel_names.resize(wheel_list.size());
+    for (int i = 0; i < wheel_list.size(); ++i)
+    {
+      wheel_names[i] = static_cast<std::string>(wheel_list[i]);
+    }
+  }
+  else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
+  {
+    wheel_names.push_back(wheel_list);
+  }
+  else
+  {
+    ROS_ERROR_STREAM_NAMED(name_,
+      "Wheel param '" << wheel_param <<
+      "' is neither a list of strings nor a string.");
+    return false;
+  }
+
+  return true;
+}
+
+
+bool ros_kinematics::RosDiffDrive::getWheelParams(ros::NodeHandle& controller_nh, 
+  const std::vector<std::string>& wheel_names,
+  std::map<std::string, XmlRpc::XmlRpcValue>& drive_param_map)
+{
+  for (int i = 0; i < wheel_names.size(); ++i)
+  {
+    XmlRpc::XmlRpcValue element_param;
+    if (!nh_priv_.getParam(wheel_names[i], element_param))
+    {
+      ROS_ERROR_STREAM_NAMED(name_,
+        "Couldn't retrieve wheel param '" << wheel_names[i] << "'.");
+      return false;
+    }
+    // check type of member
+    if(element_param.hasMember("lookup_name"))
+    {
+      if(element_param["lookup_name"].getType() != XmlRpc::XmlRpcValue::TypeString)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->lookup_name" << " isn't a string.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("mesurement_topic"))
+    {
+      if(element_param["mesurement_topic"].getType() != XmlRpc::XmlRpcValue::TypeString)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->mesurement_topic" << " isn't a string.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("frame_id"))
+    {
+      if(element_param["frame_id"].getType() != XmlRpc::XmlRpcValue::TypeString)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->frame_id" << " isn't a string.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("wheel_topic"))
+    {
+      if(element_param["wheel_topic"].getType() != XmlRpc::XmlRpcValue::TypeString)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->wheel_topic" << " isn't a string.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("max_publish_rate"))
+    {
+      if( element_param["max_publish_rate"].getType() != XmlRpc::XmlRpcValue::TypeDouble && 
+          element_param["max_publish_rate"].getType() != XmlRpc::XmlRpcValue::TypeInt 
+        )
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->max_publish_rate" << " isn't a double/int.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("subscribe_queue_size"))
+    {
+      if( element_param["subscribe_queue_size"].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
+          element_param["subscribe_queue_size"].getType() != XmlRpc::XmlRpcValue::TypeInt
+        )
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->subscribe_queue_size" << " isn't a double/int.");
+        return false;
+      }
+    }
+
+    if(element_param.hasMember("command_timeout"))
+    {
+      if( element_param["command_timeout"].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
+          element_param["command_timeout"].getType() != XmlRpc::XmlRpcValue::TypeInt
+        )
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->command_timeout" << " isn't a double/int.");
+        return false;
+      }
+    }
+
+    if(!element_param.hasMember("origin"))
+    {
+      ROS_ERROR_STREAM_NAMED(name_,
+        "Couldn't retrieve wheel param '" << wheel_names[i] << "'->origin.");
+      return false;
+    }
+    else
+    {
+      if(element_param["origin"].getType() != XmlRpc::XmlRpcValue::TypeArray)
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->origin" << " isn't a arrays.");
+        return false;
+      }
+      else
+      {
+        if(element_param["origin"].size() < 6) // x y z roll pich yaw
+        {
+          ROS_ERROR_STREAM_NAMED(name_,
+          "Wheel param '" << wheel_names[i] << "'->origin is an error array");
+          return false;
+        }
+        else
+        {
+          for (int j = 0; j < element_param["origin"].size(); ++j)
+          {
+            if( element_param["origin"][j].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
+                element_param["origin"][j].getType() != XmlRpc::XmlRpcValue::TypeInt
+              )
+            {
+              ROS_ERROR_STREAM_NAMED(name_,
+                "Wheel param '" << wheel_names[i] << "'->origin #" << j <<
+                " isn't a double/int.");
+              return false;
+            }
+          }
+        }
+      }
+    }
+    // Done
+    drive_param_map[wheel_names[i]] = element_param;
+  }
+  return true;
+}
+
+
+bool ros_kinematics::RosDiffDrive::allReady()
+{
+  bool result = true;
+  for (size_t i = 0; i < wheel_joints_size_; ++i)
+  {
+    result = result && left_drive_[i]->Ready() && right_drive_[i]->Ready();
+  }
+  return result;
+}
+

Devices/Packages/ros_kinematics/src/ros_diff_drive_parameters.cpp

@@ -0,0 +1,117 @@
+#include <ros_kinematics/ros_diff_drive_parameters.h>
+#include <memory>
+#include <string>
+#include <map>
+
+namespace ros_kinematics
+{
+
+  KinematicDiffDriveParameters::KinematicDiffDriveParameters()
+    : left_wheel_radius_multiplier_(0.0),
+    right_wheel_radius_multiplier_(0.0),
+    wheel_separation_multiplier_(0.0),
+    publish_rate_(0.0),
+    enable_odom_tf_(false),
+    setup_(false),
+    dsrv_(nullptr)
+  {
+  }
+
+  KinematicDiffDriveParameters::KinematicDiffDriveParameters(const ros::NodeHandle& nh)
+    : left_wheel_radius_multiplier_(0.0),
+    right_wheel_radius_multiplier_(0.0),
+    wheel_separation_multiplier_(0.0),
+    publish_rate_(0.0),
+    enable_odom_tf_(false),
+    setup_(false),
+    dsrv_(nullptr)
+  {
+    initialize(nh);
+  }
+
+  void KinematicDiffDriveParameters::initialize(const ros::NodeHandle& nh)
+  {
+    if (!initialized_)
+    {
+      nh_ = ros::NodeHandle(nh);
+      // the rest of the initial values are loaded through the dynamic reconfigure mechanisms
+      dsrv_ = std::make_shared<dynamic_reconfigure::Server<DiffDriveParametersConfig> >(nh_);
+      dynamic_reconfigure::Server<DiffDriveParametersConfig>::CallbackType cb =
+        boost::bind(&KinematicDiffDriveParameters::reconfigureCB, this, _1, _2);
+      dsrv_->setCallback(cb);
+      int m_subscribe_queue_size = 1000;
+      kinematics_sub_ = nh_.subscribe<dynamic_reconfigure::Config>("/agv_dynparam/parameter_updates", m_subscribe_queue_size, &KinematicDiffDriveParameters::kinematicsCallback, this);
+      initialized_ = true;
+    }
+  }
+
+  void KinematicDiffDriveParameters::reconfigureCB(DiffDriveParametersConfig& config, uint32_t level)
+  {
+    boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
+    // ROS_WARN("reconfigureCB");
+    left_wheel_radius_multiplier_   = config.left_wheel_radius_multiplier;
+    right_wheel_radius_multiplier_  = config.right_wheel_radius_multiplier;
+    wheel_separation_multiplier_    = config.wheel_separation_multiplier;
+    publish_rate_                   = config.publish_rate;
+    enable_odom_tf_                 = config.enable_odom_tf;
+    // reconfig_mutex.unlock();
+  }
+
+  void KinematicDiffDriveParameters::kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param)
+  {
+    boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
+    // ROS_WARN("dynamic_reconfigure");
+    if (param != nullptr)
+    {
+      std::map<std::string, double> param_m;
+      for (auto db : param->doubles) param_m[db.name] = db.value;
+
+      auto it = param_m.find("left_wheel_radius_multiplier");
+      if (it != param_m.end())
+      {
+        left_wheel_radius_multiplier_ = param_m["left_wheel_radius_multiplier"];
+        ROS_INFO("[ros_kinematics]-left_wheel_radius_multiplier %f", left_wheel_radius_multiplier_);
+      }
+      else ROS_WARN("[ros_kinematics]-left_wheel_radius_multiplier %f", left_wheel_radius_multiplier_);
+
+      it = param_m.find("right_wheel_radius_multiplier");
+      if (it != param_m.end())
+      {
+        right_wheel_radius_multiplier_ = param_m["right_wheel_radius_multiplier"];
+        ROS_INFO("[ros_kinematics]-right_wheel_radius_multiplier %f", right_wheel_radius_multiplier_);
+      }
+      else ROS_WARN("[ros_kinematics]-right_wheel_radius_multiplier %f", right_wheel_radius_multiplier_);
+
+      it = param_m.find("wheel_separation_multiplier");
+      if (it != param_m.end())
+      {
+        wheel_separation_multiplier_ = param_m["wheel_separation_multiplier"];
+        ROS_INFO("[ros_kinematics]-wheel_separation_multiplier %f", wheel_separation_multiplier_);
+      }
+      else ROS_WARN("[ros_kinematics]-wheel_separation_multiplier %f", wheel_separation_multiplier_);
+
+      it = param_m.find("publish_rate");
+      if (it != param_m.end())
+      {
+        publish_rate_ = param_m["publish_rate"];
+        ROS_INFO("[ros_kinematics]-publish_rate %f", publish_rate_);
+      }
+      else ROS_WARN("[ros_kinematics]-publish_rate %f", publish_rate_);
+
+      // it = param_m.find("enable_odom_tf");
+      // if (it != param_m.end())
+      // {
+      //   enable_odom_tf_ = param_m["enable_odom_tf"];
+      //   ROS_INFO("[ros_kinematics]-enable_odom_tf %x", enable_odom_tf_);
+      // }
+      // else ROS_WARN("[ros_kinematics]-enable_odom_tf %x", enable_odom_tf_);
+
+      param_m.clear();
+      setup_ = true;
+    }
+    else ROS_WARN("dynamic_reconfigure param is null");
+    // reconfig_mutex.unlock();
+  }
+
+
+} // namespace ros_kinematics

Devices/Packages/ros_kinematics/src/ros_kinematics_node.cpp

@@ -0,0 +1,33 @@
+#include <ros/ros.h>
+#include <boost/shared_ptr.hpp>
+#include "ros_kinematics/base_drive.h"
+#include "ros_kinematics/ros_steer_drive.h"
+#include "ros_kinematics/ros_diff_drive.h"
+
+int main(int argc, char **argv)
+{
+  /* Khoi tao Node */
+  ros::init(argc, argv, "ros_kinematics");
+  ros::NodeHandle nh;
+  int type;
+  nh.param(ros::this_node::getName()+"/type", type, 1);
+  /* Constructors */
+  boost::shared_ptr<ros_kinematics::BaseDrive> ros_drive = nullptr;
+  switch (type)
+  {
+    case 1:
+      ros_drive = boost::make_shared<ros_kinematics::RosSteerDrive>(nh, ros::this_node::getName());
+      break;
+
+    case 2:
+      ros_drive = boost::make_shared<ros_kinematics::RosDiffDrive>(nh, ros::this_node::getName());
+      break;
+
+    default:
+      ros_drive = boost::make_shared<ros_kinematics::RosSteerDrive>(nh, ros::this_node::getName());
+      break;
+  }
+  ros::spin();
+  ros_drive = nullptr;
+  return 0;
+}

Devices/Packages/ros_kinematics/src/ros_steer_drive.cpp

@@ -0,0 +1,447 @@
+#include "ros_kinematics/ros_steer_drive.h"
+#include <std_msgs/UInt16.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Float32.h>
+
+ros_kinematics::RosSteerDrive::RosSteerDrive(ros::NodeHandle & nh, const std::string &name)
+ : nh_(nh),
+   motor_loader_("models", "models::BaseSteerDrive"), 
+   encoder_loader_("models", "models::BaseAbsoluteEncoder"),
+   last_odom_update_(ros::Time(0)),
+   last_actuator_update_(ros::Time(0)),
+   publish_time_(ros::Time(0)),
+   publish_lastest_time_(ros::Time(0))
+{
+
+  nh_priv_ = ros::NodeHandle(nh_, name);
+  ROS_INFO("RosSteerDrive get node name is %s", name.c_str());
+
+  bool init_odom_enc;
+  nh_priv_.param("initialize_odom_enc", init_odom_enc, true);
+  odom_enc_initialized_ = !init_odom_enc;
+  
+  double schedule_delay, schedule_lastest_delay;
+  nh_priv_.param("commandTopic", command_topic_, std::string("cmd_vel"));
+  nh_priv_.param("publishOdomTF", publishOdomTF_, false);
+  nh_priv_.param("odometryTopic", odometry_topic_, std::string("odom"));
+  nh_priv_.param("odometryFrame", odometry_frame_, std::string("odom"));
+  nh_priv_.param("odometryEncTopic", odometry_enc_topic_, std::string("odom_enc"));
+  nh_priv_.param("odometryEncChildFrame", odometry_enc_child_frame_, std::string("odom_enc"));
+  nh_priv_.param("robotBaseFrame", robot_base_frame_, std::string("base_link"));
+  nh_priv_.param("steerChildFrame", steer_id_, std::string("steer_link"));
+
+  nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1);
+  nh_priv_.param("max_update_rate", update_rate_, 100.0);
+  nh_priv_.param("schedule_delay", schedule_delay, 0.005);
+  nh_priv_.param("schedule_lastest_delay", schedule_lastest_delay, 0.5);
+  nh_priv_.param("odomEncSteeringAngleOffset",odom_enc_steering_angle_offset_, 0.);
+  nh_priv_.param("wheelAcceleration", wheel_acceleration_, 0.);
+  nh_priv_.param("steering_fix_wheel_distance_x", steering_fix_wheel_distance_x_, 0.68);
+  nh_priv_.param("steering_fix_wheel_distance_y", steering_fix_wheel_distance_y_, 0.);
+  
+
+  std::string steer_drive;
+  nh_priv_.param("steer_drive", steer_drive, std::string("motor_wheel::SteerMotor"));
+  try
+  {
+    steer_motor_ =  motor_loader_.createInstance(steer_drive);
+    steer_motor_->initialize(nh_, name + "/" + motor_loader_.getName(steer_drive));
+    ros::NodeHandle nh(nh_priv_, motor_loader_.getName(steer_drive));
+    nh.param("method", steer_motor_mode_, std::string(""));
+  }
+  catch(const pluginlib::PluginlibException &ex)
+  {
+    ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", steer_drive.c_str(), ex.what());
+    exit(1);
+  }
+
+  std::string traction_drive;
+  nh_priv_.param("traction_drive", traction_drive, std::string("motor_wheel::TractionMotor"));
+  nh_priv_.param("wheel_diameter", wheel_diameter_, 0.210); 
+  try
+  {
+    traction_motor_ =  motor_loader_.createInstance(traction_drive);
+    traction_motor_->initialize(nh_, name + "/" + motor_loader_.getName(traction_drive));
+    ros::NodeHandle nh(nh_priv_, motor_loader_.getName(traction_drive));
+    nh.param("method", traction_motor_mode_, std::string(""));
+  }
+  catch(const pluginlib::PluginlibException &ex)
+  {
+    ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", traction_drive.c_str(), ex.what());
+    exit(1);
+  }
+
+  nh_.param(name + "/use_encoder", use_abs_encoder, false);
+  if(use_abs_encoder)
+  {
+    std::string abs_encoder;
+    nh_priv_.param("absolute_encoder", abs_encoder, std::string("absolute_encoder::Measurements"));
+    try
+    {
+      absolute_encoder_ =  encoder_loader_.createInstance(abs_encoder);
+      absolute_encoder_->initialize(nh, "AbsoluteEncoder");
+    }
+    catch(const pluginlib::PluginlibException &ex)
+    {
+      ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", abs_encoder.c_str(), ex.what());
+      exit(1);
+    }
+  }
+
+  transform_broadcaster_  = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster());
+  odometry_publisher_     = nh_.advertise<nav_msgs::Odometry>(odometry_topic_, 1);
+  odometry_enc_publisher_ = nh_.advertise<nav_msgs::Odometry>(odometry_enc_topic_, 1);
+  cmd_vel_subscriber_     = nh_.subscribe(command_topic_, m_subscribe_queue_size_, &ros_kinematics::RosSteerDrive::CmdVelCallback, this);
+  cmd_vel_feedback_       = nh_.advertise<geometry_msgs::Twist>("cmd_vel_feedback", 1);
+  cmd_vel_rb              = nh_.advertise<geometry_msgs::Twist>("cmd_vel_rb", 1);
+  steer_angle_pub_        = nh_.advertise<std_msgs::Float32>("steer_angle", 1);
+  
+  // Initialize update rate stuff
+  if (this->update_rate_ > 0.0)
+    this->update_period_ = 1.0 / this->update_rate_;
+  else
+    this->update_period_ = 0.0;
+    
+  schedule_delay_ = ros::Duration(schedule_delay);
+  schedule_lastest_delay_ = ros::Duration(schedule_lastest_delay);
+
+  // Kinematics
+  kinematics_param_ptr = boost::make_shared<ros_kinematics::KinematicSteerDriveParameters>();
+  kinematics_param_ptr->initialize(nh_);
+
+  // Thread
+  callback_thread_ = new boost::thread(&ros_kinematics::RosSteerDrive::UpdateChild,this);
+}
+
+ros_kinematics::RosSteerDrive::~RosSteerDrive()
+{
+  if (callback_thread_)
+  {
+    callback_thread_->join();
+    delete (callback_thread_);
+    callback_thread_ = 0;
+  }
+}
+
+void ros_kinematics::RosSteerDrive::CmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(callback_mutex_);
+  command_struct_.linear.x = cmd_msg->linear.x;
+  command_struct_.angular.z = cmd_msg->angular.z;
+  scheduleMotorController(true);
+}
+
+/**
+ * @brief returns true, if publishing of a cmd_vel topic is publishing
+*/ 
+bool ros_kinematics::RosSteerDrive::isCmdVelTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  return !publish_time_.isZero() && ros::Time::now() > publish_time_;
+}
+
+/**
+ * @brief returns true, if publishing of a cmd_vel topic is publishing
+*/ 
+bool ros_kinematics::RosSteerDrive::isCmdVelLastestTriggered(void)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
+}
+
+/**
+ * @brief schedules MotorController function 
+ * @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
+*/
+void ros_kinematics::RosSteerDrive::scheduleMotorController(bool schedule)
+{
+  boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
+  if(schedule && publish_time_.isZero())
+  {
+    publish_time_ = ros::Time::now() + schedule_delay_;
+    publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_;
+  }
+  if(!schedule && !publish_time_.isZero())
+  {
+    publish_time_ = ros::Time(0);
+    // publish_lastest_time_ = ros::Time(0);
+  }
+}
+
+void ros_kinematics::RosSteerDrive::MotorController(double target_speed, double target_steering_speed, double dt)
+{
+  // TODO add the accelerations etc. properly
+  double applied_speed = target_speed;
+  double applied_steering_speed = target_steering_speed;
+
+  if(traction_motor_->Ready() && steer_motor_->Ready())
+  {
+    double current_speed = traction_motor_->GetVelocity();
+    double current_steering_speed = steer_motor_->GetVelocity();
+
+    if(kinematics_param_ptr->isSetup())
+    {
+      wheel_acceleration_ = kinematics_param_ptr->getWheelAcceleration();
+    }
+
+    if (wheel_acceleration_ > 0)
+    {
+      // TODO
+      // applied_speed = ...;
+      // applied_steering_speed = ...;
+    }
+    if(traction_motor_mode_ == std::string("speed_mode")) 
+      traction_motor_->SetVelocity(applied_speed);
+
+    if(steer_motor_mode_ == std::string("position_mode")) 
+      steer_motor_->SetPosition(applied_steering_speed);
+    else if(steer_motor_mode_ == std::string("speed_mode")) 
+      steer_motor_->SetVelocity(applied_steering_speed);
+  }
+  if(!traction_motor_->Ready())
+    ROS_WARN_THROTTLE_NAMED(2.0, "ros_kinematics", "traction_motor_ is not Ready.");
+  if(!steer_motor_->Ready())
+    ROS_WARN_THROTTLE_NAMED(2.0, "ros_kinematics", "steer_motor_ is not Ready.");
+}
+
+void ros_kinematics::RosSteerDrive::publishSteerJoint(double odom_alpha)
+{
+  ros::Time current_time = ros::Time::now();
+  tf::Quaternion qt;
+  tf::Vector3 vt;
+  qt.setRPY(0, 0, odom_alpha);
+  if(kinematics_param_ptr->isSetup())
+  {
+    steering_fix_wheel_distance_x_ = kinematics_param_ptr->getSteeringFixWheelDistanceX();
+    steering_fix_wheel_distance_y_ = kinematics_param_ptr->getSteeringFixWheelDistanceY();
+  }
+  vt = tf::Vector3(steering_fix_wheel_distance_x_, steering_fix_wheel_distance_y_, 0);
+  tf::Transform base_to_steer_link(qt, vt);
+  transform_broadcaster_->sendTransform( tf::StampedTransform(base_to_steer_link, current_time, 
+                                                              robot_base_frame_, steer_id_));
+}
+
+void ros_kinematics::RosSteerDrive::UpdateOdometryEncoder()
+{
+  ros::Time current_time = ros::Time::now();
+  double step_time = ros::Duration(current_time - last_odom_update_).toSec();
+  last_odom_update_ = current_time;
+
+  double odom_alpha;
+  if(use_abs_encoder)
+    odom_alpha = absolute_encoder_->Position();
+  else
+    odom_alpha = steer_motor_->Position();
+
+  if(kinematics_param_ptr->isSetup())
+  {
+    odom_enc_steering_angle_offset_= kinematics_param_ptr->getOdomEncSteeringAngleOffset();
+    // ROS_WARN("odom_enc_steering_angle_offset_ %f", odom_enc_steering_angle_offset_);
+  }
+  odom_alpha += odom_enc_steering_angle_offset_;
+  
+  std_msgs::Float32 msg;
+  msg.data = odom_alpha/M_PI*180;
+  steer_angle_pub_.publish(msg);
+  
+  publishSteerJoint(odom_alpha);
+  // ROS_WARN_STREAM("Odom alpha " << odom_alpha << " "<< odom_alpha / M_PI *180);
+  // Distance travelled drive wheel
+  double drive_dist = traction_motor_->GetVelocity() * (wheel_diameter_ / 2) * step_time; 
+
+  double dd = 0.;
+  double da = 0.;
+
+  if (fabs(odom_alpha) < 0.000001) // Avoid dividing with a very small number...
+  {
+    dd = drive_dist;
+    da = 0.;
+  }
+  else
+  {
+    if(kinematics_param_ptr->isSetup())
+    {
+      steering_fix_wheel_distance_x_ = kinematics_param_ptr->getSteeringFixWheelDistanceX();
+      steering_fix_wheel_distance_y_ = kinematics_param_ptr->getSteeringFixWheelDistanceY();
+    }
+    double r_stear = steering_fix_wheel_distance_x_ / sin(odom_alpha);
+    double r_fix = r_stear * cos(odom_alpha) - steering_fix_wheel_distance_y_;
+
+    dd = r_fix / r_stear * drive_dist; // Adjust the actual forward movement (located between the fixed front wheels) based on the radius of the drive wheel).
+    da = drive_dist / r_stear;
+  }
+  
+  // Update the current estimate
+  double dx = dd * cos(pose_encoder_.theta + da / 2.);
+  double dy = dd * sin(pose_encoder_.theta + da / 2.);
+
+  // Compute odometric pose
+  pose_encoder_.x += dx;
+  pose_encoder_.y += dy;
+  pose_encoder_.theta += da;
+
+  double w = da / step_time;
+  
+  double v = dd / step_time;
+  geometry_msgs::Twist fb;
+  fb.linear.x = v;
+  fb.angular.z = w;
+  cmd_vel_feedback_.publish(fb);
+
+  tf::Quaternion qt;
+  tf::Vector3 vt;
+  qt.setRPY(0, 0, pose_encoder_.theta);
+  vt = tf::Vector3(pose_encoder_.x, pose_encoder_.y, 0);
+
+  odom_enc_.pose.pose.position.x = vt.x();
+  odom_enc_.pose.pose.position.y = vt.y();
+  odom_enc_.pose.pose.position.z = vt.z();
+
+  odom_enc_.pose.pose.orientation.x = qt.x();
+  odom_enc_.pose.pose.orientation.y = qt.y();
+  odom_enc_.pose.pose.orientation.z = qt.z();
+  odom_enc_.pose.pose.orientation.w = qt.w();
+
+  odom_enc_.twist.twist.angular.z = w;
+  odom_enc_.twist.twist.linear.x = v;
+  odom_enc_.twist.twist.linear.y = 0.0;
+
+}
+
+void ros_kinematics::RosSteerDrive::PublishOdometry(double step_time)
+{
+  // getting data form encoder integration
+  odom_ = odom_enc_;
+  ros::Time current_time = ros::Time::now();
+  std::string odom_frame = odometry_frame_;
+  std::string base_footprint_frame = robot_base_frame_;
+  tf::Quaternion qt;
+  tf::Vector3 vt;
+  if(publishOdomTF_)
+  {
+    qt = tf::Quaternion(odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w);
+    vt = tf::Vector3(odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z);
+    tf::Transform base_footprint_to_odom(qt, vt);
+    transform_broadcaster_->sendTransform(
+        tf::StampedTransform(base_footprint_to_odom, current_time,
+                              odom_frame, base_footprint_frame));
+  }
+  // set covariance - TODO, fix this(!)
+  odom_.pose.covariance[0] = 0.00001;
+  odom_.pose.covariance[7] = 0.00001;
+  odom_.pose.covariance[14] = 1000000000000.0;
+  odom_.pose.covariance[21] = 1000000000000.0;
+  odom_.pose.covariance[28] = 1000000000000.0;
+  odom_.pose.covariance[35] = 0.001;
+
+  // set header
+  odom_.header.stamp = current_time;
+  odom_.header.frame_id = odom_frame;
+  odom_.child_frame_id = base_footprint_frame;
+
+  odometry_publisher_.publish(odom_);
+
+  // publish the encoder based odometry
+  {
+    if (!odom_enc_initialized_)
+    {
+      pose_encoder_.x = odom_.pose.pose.position.x;
+      pose_encoder_.y = odom_.pose.pose.position.y;
+      pose_encoder_.theta = tf::getYaw(odom_.pose.pose.orientation);
+      odom_enc_initialized_ = true;
+
+      odom_enc_ = odom_;
+    }
+    std::string odom_enc_child_frame = odometry_enc_child_frame_;
+    if(publishOdomTF_)
+    {
+      qt = tf::Quaternion(odom_enc_.pose.pose.orientation.x, odom_enc_.pose.pose.orientation.y, odom_enc_.pose.pose.orientation.z, odom_enc_.pose.pose.orientation.w);
+      vt = tf::Vector3(odom_enc_.pose.pose.position.x, odom_enc_.pose.pose.position.y, odom_enc_.pose.pose.position.z);
+      
+      tf::Transform odom_enc_child_to_odom(qt, vt);
+      transform_broadcaster_->sendTransform(
+          tf::StampedTransform(odom_enc_child_to_odom, current_time,
+                                odom_frame, odom_enc_child_frame));
+    }
+    odom_enc_.pose.covariance = odom_.pose.covariance; // TODO...
+    odom_enc_.header.stamp = current_time;
+    odom_enc_.header.frame_id = odom_frame; // Note - this is typically /world
+    odom_enc_.child_frame_id = odom_enc_child_frame;
+    odometry_enc_publisher_.publish(odom_enc_);
+  }
+}
+
+void ros_kinematics::RosSteerDrive::UpdateChild(void)
+{
+  ros::Rate rate(update_rate_);
+  bool stopped;
+  while (ros::ok())
+  {
+    if(traction_motor_->Ready() && steer_motor_->Ready())
+    {
+      UpdateOdometryEncoder();
+    }
+    ros::Time current_time = ros::Time::now();
+    double seconds_since_last_update = ros::Duration(current_time - last_actuator_update_).toSec();
+
+    if (seconds_since_last_update > update_period_)
+    {
+      if(traction_motor_->Ready() && steer_motor_->Ready())
+      {
+        PublishOdometry(seconds_since_last_update);
+      }
+      // if (publishWheelTF_)
+      //   publishWheelTF();
+      // if (publishWheelJointState_)
+      //   publishWheelJointState();
+      double target_steering_angle_speed_saved_;
+      if(isCmdVelTriggered())
+      {
+        double target_wheel_rotation_speed = command_struct_.linear.x / (wheel_diameter_ / 2.0);
+        double target_steering_angle_speed = command_struct_.angular.z;
+        double odom_alpha;
+        if(use_abs_encoder)
+          odom_alpha = absolute_encoder_->Position();
+        else
+          odom_alpha = steer_motor_->Position();
+
+        if(kinematics_param_ptr->isSetup())
+        {
+          odom_enc_steering_angle_offset_= kinematics_param_ptr->getOdomEncSteeringAngleOffset();
+        }
+        odom_alpha += odom_enc_steering_angle_offset_;
+
+        geometry_msgs::Twist fb;
+
+        fb.linear.x = command_struct_.linear.x * fabs(cos(odom_alpha));
+
+        cmd_vel_rb.publish(fb);
+        
+        MotorController(target_wheel_rotation_speed, target_steering_angle_speed, seconds_since_last_update);
+        target_steering_angle_speed_saved_ = target_steering_angle_speed;
+        scheduleMotorController(false);
+        stopped = false;
+      }
+      if(isCmdVelLastestTriggered() && !stopped)
+      {
+        if(steer_motor_mode_ == std::string("position_mode")) 
+        {
+          MotorController(0, target_steering_angle_speed_saved_, seconds_since_last_update);
+          MotorController(0, target_steering_angle_speed_saved_, seconds_since_last_update);
+        }
+        else
+        {
+          MotorController(0, 0, seconds_since_last_update);
+          MotorController(0, 0, seconds_since_last_update);
+        }
+        stopped = true;
+      }
+      last_actuator_update_ = current_time;
+    }
+    rate.sleep();
+    ros::spinOnce();
+  }
+}
+
+
+

Devices/Packages/ros_kinematics/src/ros_steer_drive_parameters.cpp

@@ -0,0 +1,106 @@
+#include <ros_kinematics/ros_steer_drive_parameters.h>
+#include <memory>
+#include <string>
+#include <map>
+
+namespace ros_kinematics
+{
+
+  KinematicSteerDriveParameters::KinematicSteerDriveParameters()
+    : odom_enc_steering_angle_offset_(0.0),
+    steering_fix_wheel_distance_x_(0.0),
+    steering_fix_wheel_distance_y_(0.0),
+    wheel_acceleration_(0.0),
+    setup_(false),
+    dsrv_(nullptr)
+  {
+  }
+
+  KinematicSteerDriveParameters::KinematicSteerDriveParameters(const ros::NodeHandle& nh)
+    : odom_enc_steering_angle_offset_(0.0),
+    steering_fix_wheel_distance_x_(0.0),
+    steering_fix_wheel_distance_y_(0.0),
+    wheel_acceleration_(0.0),
+    setup_(false),
+    dsrv_(nullptr)
+  {
+    initialize(nh);
+  }
+
+  void KinematicSteerDriveParameters::initialize(const ros::NodeHandle& nh)
+  {
+    if (!initialized_)
+    {
+      nh_ = ros::NodeHandle(nh);
+      // the rest of the initial values are loaded through the dynamic reconfigure mechanisms
+      dsrv_ = std::make_shared<dynamic_reconfigure::Server<SteerDriveParametersConfig> >(nh_);
+      dynamic_reconfigure::Server<SteerDriveParametersConfig>::CallbackType cb =
+        boost::bind(&KinematicSteerDriveParameters::reconfigureCB, this, _1, _2);
+      dsrv_->setCallback(cb);
+      int m_subscribe_queue_size = 1000;
+      kinematics_sub_ = nh_.subscribe<dynamic_reconfigure::Config>("/agv_dynparam/parameter_updates", m_subscribe_queue_size, &KinematicSteerDriveParameters::kinematicsCallback, this);
+      initialized_ = true;
+    }
+  }
+
+  void KinematicSteerDriveParameters::reconfigureCB(SteerDriveParametersConfig& config, uint32_t level)
+  {
+    boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
+    // ROS_WARN("reconfigureCB");
+    odom_enc_steering_angle_offset_ = config.odomEncSteeringAngleOffset;
+    steering_fix_wheel_distance_x_ = config.steering_fix_wheel_distance_x;
+    steering_fix_wheel_distance_y_ = config.steering_fix_wheel_distance_y;
+    wheel_acceleration_ = config.wheelAcceleration;
+    // reconfig_mutex.unlock();
+  }
+
+  void KinematicSteerDriveParameters::kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param)
+  {
+    boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
+    // ROS_WARN("dynamic_reconfigure");
+    if (param != nullptr)
+    {
+      std::map<std::string, double> param_m;
+      for (auto db : param->doubles) param_m[db.name] = db.value;
+
+      auto it = param_m.find("odomEncSteeringAngleOffset");
+      if (it != param_m.end())
+      {
+        odom_enc_steering_angle_offset_ = param_m["odomEncSteeringAngleOffset"];
+        ROS_INFO("[ros_kinematics]-odomEncSteeringAngleOffset %f", odom_enc_steering_angle_offset_);
+      }
+      else ROS_WARN("[ros_kinematics]-odomEncSteeringAngleOffset %f", odom_enc_steering_angle_offset_);
+
+      it = param_m.find("steering_fix_wheel_distance_x");
+      if (it != param_m.end())
+      {
+        steering_fix_wheel_distance_x_ = param_m["steering_fix_wheel_distance_x"];
+        ROS_INFO("[ros_kinematics]-steering_fix_wheel_distance_x %f", steering_fix_wheel_distance_x_);
+      }
+      else ROS_WARN("[ros_kinematics]-steering_fix_wheel_distance_x %f", steering_fix_wheel_distance_x_);
+
+      it = param_m.find("steering_fix_wheel_distance_y");
+      if (it != param_m.end())
+      {
+        steering_fix_wheel_distance_y_ = param_m["steering_fix_wheel_distance_y"];
+        ROS_INFO("[ros_kinematics]-steering_fix_wheel_distance_y %f", steering_fix_wheel_distance_y_);
+      }
+      else ROS_WARN("[ros_kinematics]-steering_fix_wheel_distance_y %f", steering_fix_wheel_distance_y_);
+
+      it = param_m.find("wheelAcceleration");
+      if (it != param_m.end())
+      {
+        wheel_acceleration_ = param_m["wheelAcceleration"];
+        ROS_INFO("[ros_kinematics]-wheelAcceleration %f", wheel_acceleration_);
+      }
+      else ROS_WARN("[ros_kinematics]-wheelAcceleration %f", wheel_acceleration_);
+
+      param_m.clear();
+      setup_ = true;
+    }
+    else ROS_WARN("dynamic_reconfigure param is null");
+    // reconfig_mutex.unlock();
+  }
+
+
+} // namespace ros_kinematics

Devices/Packages/ros_kinematics/src/speed_limiter.cpp

@@ -0,0 +1,137 @@
+/*********************************************************************
+ * 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
+ */
+
+#include <algorithm>
+
+#include <ros_kinematics/speed_limiter.h>
+
+template<typename T>
+T clamp(T x, T min, T max)
+{
+  return std::min(std::max(min, x), max);
+}
+
+namespace ros_kinematics
+{
+
+  SpeedLimiter::SpeedLimiter(
+    bool has_velocity_limits,
+    bool has_acceleration_limits,
+    bool has_jerk_limits,
+    double min_velocity,
+    double max_velocity,
+    double min_acceleration,
+    double max_acceleration,
+    double min_jerk,
+    double max_jerk
+  )
+  : has_velocity_limits(has_velocity_limits)
+  , has_acceleration_limits(has_acceleration_limits)
+  , has_jerk_limits(has_jerk_limits)
+  , min_velocity(min_velocity)
+  , max_velocity(max_velocity)
+  , min_acceleration(min_acceleration)
+  , max_acceleration(max_acceleration)
+  , min_jerk(min_jerk)
+  , max_jerk(max_jerk)
+  {
+  }
+
+  double SpeedLimiter::limit(double& v, double v0, double v1, double dt)
+  {
+    const double tmp = v;
+
+    limit_jerk(v, v0, v1, dt);
+    limit_acceleration(v, v0, dt);
+    limit_velocity(v);
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+  double SpeedLimiter::limit_velocity(double& v)
+  {
+    const double tmp = v;
+
+    if (has_velocity_limits)
+    {
+      v = clamp(v, min_velocity, max_velocity);
+    }
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+  double SpeedLimiter::limit_acceleration(double& v, double v0, double dt)
+  {
+    const double tmp = v;
+
+    if (has_acceleration_limits)
+    {
+      const double dv_min = min_acceleration * dt;
+      const double dv_max = max_acceleration * dt;
+
+      const double dv = clamp(v - v0, dv_min, dv_max);
+
+      v = v0 + dv;
+    }
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+  double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt)
+  {
+    const double tmp = v;
+
+    if (has_jerk_limits)
+    {
+      const double dv  = v  - v0;
+      const double dv0 = v0 - v1;
+
+      const double dt2 = 2. * dt * dt;
+
+      const double da_min = min_jerk * dt2;
+      const double da_max = max_jerk * dt2;
+
+      const double da = clamp(dv - dv0, da_min, da_max);
+
+      v = v0 + dv0 + da;
+    }
+
+    return tmp != 0.0 ? v / tmp : 1.0;
+  }
+
+} // namespace ros_kinematics