From f9a630269f56ae2d5f2c7da5ad565e20f285bea9 Mon Sep 17 00:00:00 2001
From: sskorol <serhii.s.korol@gmail.com>
Date: Mon, 23 Jan 2023 20:32:21 +0000
Subject: [PATCH] Added velocity_smoother support.

---
 .../velocity_smoother/velocity_smoother.yaml  | 31 ++++++++--------
 champ_bringup/launch/bringup.launch.py        | 14 ++++++++
 .../include/velocity_smoother.launch.py       | 36 +++++++++++++++++++
 champ_bringup/package.xml                     |  2 +-
 4 files changed, 67 insertions(+), 16 deletions(-)
 create mode 100644 champ_bringup/launch/include/velocity_smoother.launch.py

diff --git a/champ_base/config/velocity_smoother/velocity_smoother.yaml b/champ_base/config/velocity_smoother/velocity_smoother.yaml
index 9af7d821..215b95a8 100644
--- a/champ_base/config/velocity_smoother/velocity_smoother.yaml
+++ b/champ_base/config/velocity_smoother/velocity_smoother.yaml
@@ -1,22 +1,23 @@
 # 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_x: 0.5
+    speed_lim_v_y: 0.35
 
-# Mandatory parameters
-speed_lim_v_x: 0.5
-speed_lim_v_y: 0.35
+    speed_lim_w: 0.4
 
-speed_lim_w: 0.4
+    accel_lim_v: 1.0
+    accel_lim_w: 2.0
 
-accel_lim_v: 1.0
-accel_lim_w: 2.0
+    # Optional parameters
+    frequency: 10.0
+    decel_factor: 1.0
 
-# Optional parameters
-frequency: 10.0
-decel_factor: 1.0
-
-# Robot velocity feedback type:
-#  0 - none
-#  1 - odometry
-#  2 - end robot commands
-robot_feedback: 2
\ No newline at end of file
+    # Robot velocity feedback type:
+    #  0 - none
+    #  1 - odometry
+    #  2 - end robot commands
+    robot_feedback: 2
diff --git a/champ_bringup/launch/bringup.launch.py b/champ_bringup/launch/bringup.launch.py
index c1a9487c..93444d71 100644
--- a/champ_bringup/launch/bringup.launch.py
+++ b/champ_bringup/launch/bringup.launch.py
@@ -139,6 +139,17 @@ def generate_launch_description():
     declare_close_loop_odom = DeclareLaunchArgument(
         "close_loop_odom", default_value="false", description=""
     )
+    
+    velocity_smoother_ld = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("champ_bringup"),
+                "launch",
+                "include",
+                "velocity_smoother.launch.py",
+            )
+        )
+    )
 
     description_ld = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -154,6 +165,7 @@ def generate_launch_description():
         }.items(),
     )
 
+    # ToDo: publish_joint_states requires conditional arg based on joint_hardware_connected (ROS2 doesn't support such style yet)
     quadruped_controller_node = Node(
         package="champ_base",
         executable="quadruped_controller_node",
@@ -173,6 +185,7 @@ def generate_launch_description():
         remappings=[("/cmd_vel/smooth", "/cmd_vel")],
     )
 
+    # ToDo: orientation_from_imu requires conditional arg based on joint_hardware_connected (ROS2 doesn't support such style yet)
     state_estimator_node = Node(
         package="champ_base",
         executable="state_estimation_node",
@@ -255,6 +268,7 @@ def generate_launch_description():
             declare_publish_foot_contacts,
             declare_publish_odom_tf,
             declare_close_loop_odom,
+            velocity_smoother_ld,
             description_ld,
             quadruped_controller_node,
             state_estimator_node,
diff --git a/champ_bringup/launch/include/velocity_smoother.launch.py b/champ_bringup/launch/include/velocity_smoother.launch.py
new file mode 100644
index 00000000..4907c813
--- /dev/null
+++ b/champ_bringup/launch/include/velocity_smoother.launch.py
@@ -0,0 +1,36 @@
+
+from os import path
+
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import IncludeLaunchDescription
+
+
+def generate_launch_description():
+    config_path = path.join(
+        get_package_share_directory('champ_base'),
+        'config',
+        'velocity_smoother',
+        'velocity_smoother.yaml'
+    )
+
+    velocity_smoother_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            path.join(
+                get_package_share_directory("yocs_velocity_smoother"),
+                "launch",
+                "velocity_smoother.launch.py"
+            )
+        ),
+        launch_arguments={
+            "config_file": LaunchConfiguration("config_file", default=config_path),
+            "raw_cmd_vel_topic": LaunchConfiguration("raw_cmd_vel_topic", default="cmd_vel"),
+            "smooth_cmd_vel_topic": LaunchConfiguration("smooth_cmd_vel_topic", default="cmd_vel/smooth"),
+            "robot_cmd_vel_topic": LaunchConfiguration("robot_cmd_vel_topic", default="cmd_vel"),
+            "odom_topic": LaunchConfiguration("odom_topic", default="odom")
+        }.items()
+    )
+
+    return LaunchDescription([velocity_smoother_launch])
diff --git a/champ_bringup/package.xml b/champ_bringup/package.xml
index 2e7d2694..88c68aaf 100644
--- a/champ_bringup/package.xml
+++ b/champ_bringup/package.xml
@@ -16,12 +16,12 @@
   <exec_depend>champ_base</exec_depend>
   <exec_depend>robot_localization</exec_depend>
   <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>yocs_velocity_smoother</exec_depend>
 
   <build_export_depend>rclcpp</build_export_depend>
   <build_export_depend>pluginlib</build_export_depend>
   <build_export_depend>geometry_msgs</build_export_depend>
   <build_export_depend>nav_msgs</build_export_depend>
-  <build_export_depend>ecl_threads</build_export_depend>
 
   <export>
     <build_type>ament_cmake</build_type>