Port velocity smoother to ROS 2.

CHANGELOG.rst

@@ -1,6 +1,10 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package yocs_velocity_smoother
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package velocity_smoother
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Forthcoming
+-----------
+* Refactored for ROS 2 and renamed to velocity_smoother
 
 0.6.3 (2014-12-05)
 ------------------

CMakeLists.txt

@@ -1,41 +1,52 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(yocs_velocity_smoother)
-find_package(catkin REQUIRED COMPONENTS roscpp pluginlib nodelet ecl_threads dynamic_reconfigure geometry_msgs nav_msgs)
-
-# Dynamic reconfigure support
-generate_dynamic_reconfigure_options(cfg/params.cfg)
-
-catkin_package(
-   INCLUDE_DIRS include
-   LIBRARIES ${PROJECT_NAME}_nodelet
-   CATKIN_DEPENDS roscpp pluginlib nodelet ecl_threads dynamic_reconfigure geometry_msgs nav_msgs
+cmake_minimum_required(VERSION 3.5)
+project(velocity_smoother)
+
+find_package(ament_cmake_ros REQUIRED)
+find_package(ecl_build REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+
+ecl_enable_cxx14_compiler()
+ecl_enable_cxx_warnings()
+
+include_directories(include)
+
+add_library(${PROJECT_NAME} SHARED src/velocity_smoother.cpp)
+ament_target_dependencies(${PROJECT_NAME}
+  "geometry_msgs"
+  "nav_msgs"
+  "rcl_interfaces"
+  "rclcpp"
+  "rclcpp_components"
 )
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-
-include_directories(include ${catkin_INCLUDE_DIRS})
-
-
-# Nodelet library
-add_library(${PROJECT_NAME}_nodelet src/velocity_smoother_nodelet.cpp)
-
-
-target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES})
-add_dependencies(${PROJECT_NAME}_nodelet ${PROJECT_NAME}_gencfg)
+add_executable(velocity_smoother_node src/velocity_smoother_node.cpp)
+ament_target_dependencies(velocity_smoother_node
+  "rclcpp"
+)
+target_link_libraries(velocity_smoother_node
+  ${PROJECT_NAME}
+)
 
+rclcpp_components_register_nodes(${PROJECT_NAME}
+  "velocity_smoother::VelocitySmoother")
 
-install(TARGETS ${PROJECT_NAME}_nodelet
-        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 install(DIRECTORY include/${PROJECT_NAME}/
-        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  DESTINATION include/${PROJECT_NAME}
 )
-install(DIRECTORY plugins
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+install(TARGETS velocity_smoother_node
+  DESTINATION lib/${PROJECT_NAME}
 )
-install(DIRECTORY launch
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-install(DIRECTORY param
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+install(DIRECTORY config launch
+  DESTINATION share/${PROJECT_NAME}
 )
+
+ament_package()

config/velocity_smoother_params.yaml

@@ -0,0 +1,21 @@
+# Example configuration:
+# - velocity limits are around a 10% above the physical limits
+# - acceleration limits are just low enough to avoid jerking
+velocity_smoother_node:
+  ros__parameters:
+    # Mandatory parameters
+    speed_lim_v: 0.8
+    speed_lim_w: 5.4
+
+    accel_lim_v: 0.3
+    accel_lim_w: 3.5
+
+    # Optional parameters
+    frequency: 20.0
+    decel_factor: 1.0
+
+    # Robot velocity feedback type:
+    #  0 - none
+    #  1 - odometry
+    #  2 - robot commands (cmd_vel)
+    robot_feedback: 2

include/velocity_smoother/velocity_smoother.hpp

@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2012, Yujin Robot.
+ * 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 Yujin Robot 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.
+ */
+
+/*****************************************************************************
+ ** Ifdefs
+ *****************************************************************************/
+
+#ifndef VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_
+#define VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_
+
+/*****************************************************************************
+ ** Includes
+ *****************************************************************************/
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+#include <geometry_msgs/msg/twist.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+/*****************************************************************************
+** Namespaces
+*****************************************************************************/
+
+namespace velocity_smoother {
+
+/*****************************************************************************
+** VelocitySmoother
+*****************************************************************************/
+
+class VelocitySmoother final : public rclcpp::Node
+{
+public:
+  explicit VelocitySmoother(const rclcpp::NodeOptions & options);
+
+  ~VelocitySmoother() override;
+  VelocitySmoother(VelocitySmoother && c) = delete;
+  VelocitySmoother & operator=(VelocitySmoother && c) = delete;
+  VelocitySmoother(const VelocitySmoother & c) = delete;
+  VelocitySmoother & operator=(const VelocitySmoother & c) = delete;
+
+private:
+  enum RobotFeedbackType
+  {
+    NONE,
+    ODOMETRY,
+    COMMANDS
+  } robot_feedback;  /**< What source to use as robot velocity feedback */
+
+  bool quiet_;        /**< Quieten some warnings that are unavoidable because of velocity multiplexing. **/
+  double speed_lim_v_, accel_lim_v_, decel_lim_v_;
+  double speed_lim_w_, accel_lim_w_, decel_lim_w_;
+
+  geometry_msgs::msg::Twist current_vel_;
+  geometry_msgs::msg::Twist target_vel_;
+  double last_cmd_vel_linear_x_;
+  double last_cmd_vel_angular_z_;
+
+  double period_;
+  double decel_factor_;
+  bool input_active_;
+  double cb_avg_time_;
+  rclcpp::Time last_velocity_cb_time_;
+  std::vector<double> period_record_; /**< Historic of latest periods between velocity commands */
+  unsigned int pr_next_; /**< Next position to fill in the periods record buffer */
+
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;    /**< Current velocity from odometry */
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr current_vel_sub_; /**< Current velocity from commands sent to the robot, not necessarily by this node */
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr raw_in_vel_sub_;  /**< Incoming raw velocity commands */
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr  smooth_vel_pub_;  /**< Outgoing smoothed velocity commands */
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  void timerCB();
+  void velocityCB(const geometry_msgs::msg::Twist::SharedPtr msg);
+  void robotVelCB(const geometry_msgs::msg::Twist::SharedPtr msg);
+  void odometryCB(const nav_msgs::msg::Odometry::SharedPtr msg);
+
+  rcl_interfaces::msg::SetParametersResult parameterUpdate(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  double sign(double x)  { return x < 0.0 ? -1.0 : +1.0; };
+
+  double median(std::vector<double> & values) {
+    // Return the median element of an doubles vector
+    std::nth_element(values.begin(), values.begin() + values.size()/2, values.end());
+    return values[values.size()/2];
+  }
+};
+
+} // namespace velocity_smoother
+
+#endif /* VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_ */