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 @@ champ_base robot_localization joint_state_publisher_gui + yocs_velocity_smoother rclcpp pluginlib geometry_msgs nav_msgs - ecl_threads ament_cmake