Migrate to ROS2 Humble

.gitignore

@@ -1,4 +1,3 @@
 bin
 build
 lib
-

CMakeLists.txt

@@ -1,41 +1,49 @@
-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)
+cmake_minimum_required(VERSION 3.5)
 
-# Dynamic reconfigure support
-generate_dynamic_reconfigure_options(cfg/params.cfg)
+project(yocs_velocity_smoother)
 
-catkin_package(
-   INCLUDE_DIRS include
-   LIBRARIES ${PROJECT_NAME}_nodelet
-   CATKIN_DEPENDS roscpp pluginlib nodelet ecl_threads dynamic_reconfigure geometry_msgs nav_msgs
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+
+include_directories(include)
+
+add_executable(velocity_smoother src/velocity_smoother.cpp)
+ament_target_dependencies(velocity_smoother
+  rclcpp
+  std_msgs
+  geometry_msgs
+  nav_msgs
 )
 
-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)
-
-
-install(TARGETS ${PROJECT_NAME}_nodelet
-        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+install(TARGETS
+  velocity_smoother
+  DESTINATION lib/${PROJECT_NAME}
 )
+
 install(DIRECTORY include/${PROJECT_NAME}/
-        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-)
-install(DIRECTORY plugins
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  DESTINATION include/${PROJECT_NAME}
 )
-install(DIRECTORY launch
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}
 )
-install(DIRECTORY param
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
 )
+
+ament_package()

README.md

@@ -1,4 +1,7 @@
-# champ_teleop
-Champ Quadruped Robot's teleoperation node. This is a forked version of [yocs_velocity_smooter](https://github.com/yujinrobot/yujin_ocs). 
+### Velocity Smoother
 
-The software has been modified to add support velocity smoothing in the y axis.
+Champ Quadruped Robot's velocity smoother node. This is a forked version of [yocs_velocity_smooter](https://github.com/yujinrobot/yujin_ocs).
+
+Initial version of this software has been modified to add support velocity smoothing in the y axis.
+
+The recent version was fully re-written to support ROS 2 Humble.

config/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:
+  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 - end robot commands
+    robot_feedback: 2

include/yocs_velocity_smoother/velocity_smoother.hpp

@@ -0,0 +1,74 @@
+#ifndef YOCS_VELOCITY_SMOOTHER_HPP_
+#define YOCS_VELOCITY_SMOOTHER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <mutex>
+#include <functional>
+#include <geometry_msgs/msg/twist.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/qos.hpp>
+#include <rclcpp/logger.hpp>
+
+namespace yocs_velocity_smoother
+{
+
+  class VelocitySmoother : public rclcpp::Node
+  {
+  public:
+    VelocitySmoother();
+
+    ~VelocitySmoother() {}
+
+    void spin();
+    void shutdown() { shutdown_req_ = true; }
+    std::mutex locker;
+
+  private:
+    enum RobotFeedbackType
+    {
+      NONE,
+      ODOMETRY,
+      COMMANDS
+    } robot_feedback_;
+
+    bool quiet_;
+    double speed_lim_v_x_, speed_lim_v_y_, accel_lim_v_, decel_lim_v_;
+    double speed_lim_w_, accel_lim_w_, decel_lim_w_;
+    double decel_factor_;
+    double frequency_;
+
+    geometry_msgs::msg::Twist last_cmd_vel_;
+    geometry_msgs::msg::Twist current_vel_;
+    geometry_msgs::msg::Twist target_vel_;
+
+    bool shutdown_req_;
+    bool input_active_;
+    double cb_avg_time_;
+    rclcpp::Time last_cb_time_;
+    std::vector<double> period_record_;
+    unsigned int pr_next_;
+
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr current_vel_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr raw_in_vel_sub_;
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr smooth_vel_pub_;
+
+    void velocityCB(const geometry_msgs::msg::Twist &msg);
+    void robotVelCB(const geometry_msgs::msg::Twist &msg);
+    void odometryCB(const nav_msgs::msg::Odometry &msg);
+
+    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
+      nth_element(values.begin(), values.begin() + values.size() / 2, values.end());
+      return values[values.size() / 2];
+    }
+  };
+} // namespace yocs_velocity_smoother
+
+#endif // YOCS_VELOCITY_SMOOTHER_HPP_