From f68d89cfd18a64a5a35b02013d724ca296fc8f34 Mon Sep 17 00:00:00 2001 From: Sergey Korol Date: Mon, 26 Dec 2022 02:13:27 +0200 Subject: [PATCH 1/3] Migrated sources to ROS 2 Humble --- .cproject | 68 --- .gitignore | 1 - .project | 85 ---- .pydevproject | 7 - .settings/language.settings.xml | 12 - CMakeLists.txt | 72 ++-- README.md | 7 +- cfg/params.cfg | 18 - config/params.yaml | 21 + .../velocity_smoother.hpp | 74 ++++ .../velocity_smoother_nodelet.hpp | 105 ----- launch/standalone.launch | 26 -- launch/test_translational_smoothing.launch | 22 - launch/velocity_smoother.launch | 28 -- launch/velocity_smoother.launch.py | 29 ++ package.xml | 39 +- param/standalone.yaml | 20 - plugins/nodelets.xml | 10 - src/velocity_smoother.cpp | 276 ++++++++++++ src/velocity_smoother_nodelet.cpp | 398 ------------------ test/test_translational_input.py | 147 ------- 21 files changed, 459 insertions(+), 1006 deletions(-) delete mode 100644 .cproject delete mode 100644 .project delete mode 100644 .pydevproject delete mode 100644 .settings/language.settings.xml delete mode 100755 cfg/params.cfg create mode 100644 config/params.yaml create mode 100644 include/yocs_velocity_smoother/velocity_smoother.hpp delete mode 100644 include/yocs_velocity_smoother/velocity_smoother_nodelet.hpp delete mode 100644 launch/standalone.launch delete mode 100644 launch/test_translational_smoothing.launch delete mode 100644 launch/velocity_smoother.launch create mode 100644 launch/velocity_smoother.launch.py delete mode 100644 param/standalone.yaml delete mode 100644 plugins/nodelets.xml create mode 100644 src/velocity_smoother.cpp delete mode 100644 src/velocity_smoother_nodelet.cpp delete mode 100755 test/test_translational_input.py diff --git a/.cproject b/.cproject deleted file mode 100644 index 5e997b0..0000000 --- a/.cproject +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/.gitignore b/.gitignore index 4392efc..638c8d2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,3 @@ bin build lib - diff --git a/.project b/.project deleted file mode 100644 index 1fb5a74..0000000 --- a/.project +++ /dev/null @@ -1,85 +0,0 @@ - - - yocs_velocity_smoother - - - - - - org.python.pydev.PyDevBuilder - - - - - org.eclipse.cdt.managedbuilder.core.genmakebuilder - clean,full,incremental, - - - ?name? - - - - org.eclipse.cdt.make.core.append_environment - true - - - org.eclipse.cdt.make.core.autoBuildTarget - all - - - org.eclipse.cdt.make.core.buildArguments - - - - org.eclipse.cdt.make.core.buildCommand - make - - - org.eclipse.cdt.make.core.cleanBuildTarget - clean - - - org.eclipse.cdt.make.core.contents - org.eclipse.cdt.make.core.activeConfigSettings - - - org.eclipse.cdt.make.core.enableAutoBuild - false - - - org.eclipse.cdt.make.core.enableCleanBuild - true - - - org.eclipse.cdt.make.core.enableFullBuild - true - - - org.eclipse.cdt.make.core.fullBuildTarget - all - - - org.eclipse.cdt.make.core.stopOnError - true - - - org.eclipse.cdt.make.core.useDefaultBuildCmd - true - - - - - org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder - full,incremental, - - - - - - org.eclipse.cdt.core.cnature - org.eclipse.cdt.core.ccnature - org.eclipse.cdt.managedbuilder.core.managedBuildNature - org.eclipse.cdt.managedbuilder.core.ScannerConfigNature - org.python.pydev.pythonNature - - diff --git a/.pydevproject b/.pydevproject deleted file mode 100644 index a9cca03..0000000 --- a/.pydevproject +++ /dev/null @@ -1,7 +0,0 @@ - - - - -Default -python 2.7 - diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml deleted file mode 100644 index 9a935d5..0000000 --- a/.settings/language.settings.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/CMakeLists.txt b/CMakeLists.txt index d92c1af..667f181 100644 --- a/CMakeLists.txt +++ b/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() diff --git a/README.md b/README.md index e2eee2c..574a5ad 100644 --- a/README.md +++ b/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 + +Champ Quadruped Robot's velocity smoother node. This is a forked version of [yocs_velocity_smooter](https://github.com/yujinrobot/yujin_ocs). The 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. diff --git a/cfg/params.cfg b/cfg/params.cfg deleted file mode 100755 index 3374e0c..0000000 --- a/cfg/params.cfg +++ /dev/null @@ -1,18 +0,0 @@ -#!/usr/bin/env python - -PACKAGE = "yocs_velocity_smoother" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("speed_lim_v", double_t, 0, "Maximum linear velocity", 1.0, 0.0, 10.0) -gen.add("speed_lim_w", double_t, 0, "Maximum angular velocity", 5.0, 0.0, 10.0) - -gen.add("accel_lim_v", double_t, 0, "Maximum linear acceleration", 0.5, 0.0, 10.0) -gen.add("accel_lim_w", double_t, 0, "Maximum angular acceleration", 2.5, 0.0, 10.0) - -gen.add("decel_factor", double_t, 0, "Deceleration to acceleration ratio", 1.0, 0.0, 10.0) - -# Second arg is node name it will run in (doc purposes only), third is generated filename prefix -exit(gen.generate(PACKAGE, "velocity_smoother_configure", "params")) diff --git a/config/params.yaml b/config/params.yaml new file mode 100644 index 0000000..07bd588 --- /dev/null +++ b/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 diff --git a/include/yocs_velocity_smoother/velocity_smoother.hpp b/include/yocs_velocity_smoother/velocity_smoother.hpp new file mode 100644 index 0000000..cf609a2 --- /dev/null +++ b/include/yocs_velocity_smoother/velocity_smoother.hpp @@ -0,0 +1,74 @@ +#ifndef YOCS_VELOCITY_SMOOTHER_HPP_ +#define YOCS_VELOCITY_SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 period_record_; + unsigned int pr_next_; + + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr current_vel_sub_; + rclcpp::Subscription::SharedPtr raw_in_vel_sub_; + rclcpp::Publisher::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 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_ diff --git a/include/yocs_velocity_smoother/velocity_smoother_nodelet.hpp b/include/yocs_velocity_smoother/velocity_smoother_nodelet.hpp deleted file mode 100644 index 074d196..0000000 --- a/include/yocs_velocity_smoother/velocity_smoother_nodelet.hpp +++ /dev/null @@ -1,105 +0,0 @@ -/** - * @file /include/yocs_velocity_smoother/velocity_smoother_nodelet.hpp - * - * @brief Velocity smoother interface - * - * License: BSD - * https://raw.github.com/yujinrobot/yujin_ocs/hydro/yocs_velocity_smoother/LICENSE - **/ -/***************************************************************************** - ** Ifdefs - *****************************************************************************/ - -#ifndef YUJIN_OCS_VELOCITY_SMOOTHER_HPP_ -#define YUJIN_OCS_VELOCITY_SMOOTHER_HPP_ - -/***************************************************************************** - ** Includes - *****************************************************************************/ - -#include -#include -#include - -/***************************************************************************** -** Namespaces -*****************************************************************************/ - -namespace yocs_velocity_smoother { - -/***************************************************************************** -** VelocitySmoother -*****************************************************************************/ - -class VelocitySmoother -{ -public: - VelocitySmoother(const std::string &name); - - ~VelocitySmoother() - { - if (dynamic_reconfigure_server != NULL) - delete dynamic_reconfigure_server; - } - - bool init(ros::NodeHandle& nh); - void spin(); - void shutdown() { shutdown_req = true; }; - std::mutex locker; - -private: - enum RobotFeedbackType - { - NONE, - ODOMETRY, - COMMANDS - } robot_feedback; /**< What source to use as robot velocity feedback */ - - std::string name; - bool quiet; /**< Quieten some warnings that are unavoidable because of velocity multiplexing. **/ - 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::Twist last_cmd_vel; - geometry_msgs::Twist current_vel; - geometry_msgs::Twist target_vel; - - bool shutdown_req; /**< Shutdown requested by nodelet; kill worker thread */ - bool input_active; - double cb_avg_time; - ros::Time last_cb_time; - std::vector period_record; /**< Historic of latest periods between velocity commands */ - unsigned int pr_next; /**< Next position to fill in the periods record buffer */ - - ros::Subscriber odometry_sub; /**< Current velocity from odometry */ - ros::Subscriber current_vel_sub; /**< Current velocity from commands sent to the robot, not necessarily by this node */ - ros::Subscriber raw_in_vel_sub; /**< Incoming raw velocity commands */ - ros::Publisher smooth_vel_pub; /**< Outgoing smoothed velocity commands */ - - void velocityCB(const geometry_msgs::Twist::ConstPtr& msg); - void robotVelCB(const geometry_msgs::Twist::ConstPtr& msg); - void odometryCB(const nav_msgs::Odometry::ConstPtr& msg); - - double sign(double x) { return x < 0.0 ? -1.0 : +1.0; }; - - double median(std::vector 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]; - }; - - /********************* - ** Dynamic Reconfigure - **********************/ - dynamic_reconfigure::Server * dynamic_reconfigure_server; - dynamic_reconfigure::Server::CallbackType dynamic_reconfigure_callback; - void reconfigCB(yocs_velocity_smoother::paramsConfig &config, uint32_t unused_level); - -}; - -} // yocs_namespace velocity_smoother - -#endif /* YUJIN_OCS_VELOCITY_SMOOTHER_HPP_ */ diff --git a/launch/standalone.launch b/launch/standalone.launch deleted file mode 100644 index b1d7d55..0000000 --- a/launch/standalone.launch +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/test_translational_smoothing.launch b/launch/test_translational_smoothing.launch deleted file mode 100644 index 203f687..0000000 --- a/launch/test_translational_smoothing.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/velocity_smoother.launch b/launch/velocity_smoother.launch deleted file mode 100644 index 7103600..0000000 --- a/launch/velocity_smoother.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/velocity_smoother.launch.py b/launch/velocity_smoother.launch.py new file mode 100644 index 0000000..6f543a6 --- /dev/null +++ b/launch/velocity_smoother.launch.py @@ -0,0 +1,29 @@ +import os + +import launch +import launch_ros.actions +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('yocs_velocity_smoother'), + 'config', + 'params.yaml' + ) + + velocity_smoother_node = launch_ros.actions.Node( + package='yocs_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + parameters = [config], + output='screen', + remappings=[('velocity_smoother/raw_cmd_vel', '/raw_cmd_vel'), + ('velocity_smoother/smooth_cmd_vel', '/smooth_cmd_vel'), + ('velocity_smoother/robot_cmd_vel', '/robot_cmd_vel'), + ('velocity_smoother/odometry', '/odom')] + ) + + return launch.LaunchDescription([ + velocity_smoother_node + ]) diff --git a/package.xml b/package.xml index e004ffa..6b9ed45 100644 --- a/package.xml +++ b/package.xml @@ -1,35 +1,24 @@ - + + + yocs_velocity_smoother - 0.12.1 + 0.0.1 Bound incoming velocity messages according to robot velocity and acceleration limits. - Jorge Santos Simon - Jihoon Lee + Sergey Korol + Sergey Korol BSD - http://ros.org/wiki/yocs_velocity_smoother - https://github.com/yujinrobot/yujin_ocs - https://github.com/yujinrobot/yujin_ocs/issues - catkin + ament_cmake - roscpp - pluginlib - nodelet - geometry_msgs - nav_msgs - ecl_threads - dynamic_reconfigure + rclcpp + geometry_msgs + nav_msgs - roscpp - pluginlib - nodelet - geometry_msgs - nav_msgs - ecl_threads - dynamic_reconfigure - - - + ament_cmake + diff --git a/param/standalone.yaml b/param/standalone.yaml deleted file mode 100644 index 9acc211..0000000 --- a/param/standalone.yaml +++ /dev/null @@ -1,20 +0,0 @@ -# Example configuration: -# - velocity limits are around a 10% above the physical limits -# - acceleration limits are just low enough to avoid jerking - -# 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 diff --git a/plugins/nodelets.xml b/plugins/nodelets.xml deleted file mode 100644 index 4054325..0000000 --- a/plugins/nodelets.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - Implementation for the command velocity smoother as a nodelet. - - - - diff --git a/src/velocity_smoother.cpp b/src/velocity_smoother.cpp new file mode 100644 index 0000000..716cbda --- /dev/null +++ b/src/velocity_smoother.cpp @@ -0,0 +1,276 @@ +#include + +#define PERIOD_RECORD_SIZE 5 +#define ZERO_VEL_COMMAND geometry_msgs::msg::Twist(); +#define IS_ZERO_VEOCITY(a) ((a.linear.x == 0.0) && (a.linear.y == 0.0) && (a.angular.z == 0.0)) + +using std::placeholders::_1; + +namespace yocs_velocity_smoother +{ + VelocitySmoother::VelocitySmoother() + : Node("velocity_smoother"), + robot_feedback_(RobotFeedbackType::NONE), + quiet_(false), + shutdown_req_(false), + input_active_(false), + cb_avg_time_(0), + pr_next_(0) + { + declare_parameter("speed_lim_v_x", 1.0); + declare_parameter("accel_lim_v", 0.5); + declare_parameter("speed_lim_w", 5.0); + declare_parameter("accel_lim_w", 2.5); + declare_parameter("decel_factor", 1.0); + declare_parameter("frequency", 20.0); + declare_parameter("robot_feedback_", (int)NONE); + + speed_lim_v_x_ = get_parameter("speed_lim_v_x").as_double(); + speed_lim_v_y_ = speed_lim_v_x_ * 0.6; + accel_lim_v_ = get_parameter("accel_lim_v").as_double(); + decel_factor_ = get_parameter("decel_factor").as_double(); + decel_lim_v_ = decel_factor_ * accel_lim_v_; + speed_lim_w_ = get_parameter("speed_lim_w").as_double(); + accel_lim_w_ = get_parameter("accel_lim_w").as_double(); + decel_lim_w_ = decel_factor_ * accel_lim_w_; + frequency_ = get_parameter("frequency").as_double(); + robot_feedback_ = static_cast(get_parameter("robot_feedback_").as_int()); + + smooth_vel_pub_ = create_publisher("cmd_vel", rclcpp::QoS(10)); + raw_in_vel_sub_ = create_subscription( + "raw_cmd_vel", rclcpp::QoS(10), std::bind(&VelocitySmoother::velocityCB, this, _1)); + current_vel_sub_ = create_subscription( + "current_velocity", rclcpp::QoS(10), std::bind(&VelocitySmoother::robotVelCB, this, _1)); + odometry_sub_ = create_subscription( + "odometry", rclcpp::QoS(10), std::bind(&VelocitySmoother::odometryCB, this, _1)); + } + + void VelocitySmoother::velocityCB(const geometry_msgs::msg::Twist &msg) + { + // Estimate commands frequency; we do continuously as it can be very different depending on the + // publisher type, and we don't want to impose extra constraints to keep this package flexible + if (period_record_.size() < PERIOD_RECORD_SIZE) + { + period_record_.push_back((rclcpp::Clock().now() - last_cb_time_).seconds()); + } + else + { + period_record_[pr_next_] = (rclcpp::Clock().now() - last_cb_time_).seconds(); + } + + pr_next_++; + pr_next_ %= period_record_.size(); + last_cb_time_ = rclcpp::Clock().now(); + + if (period_record_.size() <= PERIOD_RECORD_SIZE / 2) + { + // wait until we have some values; make a reasonable assumption (10 Hz) meanwhile + cb_avg_time_ = 0.1; + } + else + { + // enough; recalculate with the latest input + cb_avg_time_ = median(period_record_); + } + + input_active_ = true; + + // Bound speed with the maximum values + locker.lock(); + target_vel_.linear.x = + msg.linear.x > 0.0 ? std::min(msg.linear.x, speed_lim_v_x_) : std::max(msg.linear.x, -speed_lim_v_x_); + target_vel_.linear.y = + msg.linear.y > 0.0 ? std::min(msg.linear.y, speed_lim_v_y_) : std::max(msg.linear.y, -speed_lim_v_y_); + target_vel_.angular.z = + msg.angular.z > 0.0 ? std::min(msg.angular.z, speed_lim_w_) : std::max(msg.angular.z, -speed_lim_w_); + locker.unlock(); + } + + void VelocitySmoother::odometryCB(const nav_msgs::msg::Odometry &msg) + { + if (robot_feedback_ == ODOMETRY) + current_vel_ = msg.twist.twist; + } + + void VelocitySmoother::robotVelCB(const geometry_msgs::msg::Twist &msg) + { + if (robot_feedback_ == COMMANDS) + current_vel_ = msg; + } + + void VelocitySmoother::spin() + { + double period = 1.0 / frequency_; + rclcpp::Rate spin_rate(frequency_); + + while (!shutdown_req_ && rclcpp::ok()) + { + locker.lock(); + double accel_lim_v_(accel_lim_v_); + double accel_lim_w_(accel_lim_w_); + double decel_factor(decel_factor_); + double decel_lim_v_(decel_lim_v_); + double decel_lim_w_(decel_lim_w_); + locker.unlock(); + + if ((input_active_ == true) && (cb_avg_time_ > 0.0) && + ((rclcpp::Clock().now() - last_cb_time_).seconds() > std::min(3.0 * cb_avg_time_, 0.5))) + { + // Velocity input no active anymore; normally last command is a zero-velocity one, but reassure + // this, just in case something went wrong with our input, or he just forgot good manners... + // Issue #2, extra check in case cb_avg_time is very big, for example with several atomic commands + // The cb_avg_time > 0 check is required to deal with low-rate simulated time, that can make that + // several messages arrive with the same time and so lead to a zero median + input_active_ = false; + if (IS_ZERO_VEOCITY(target_vel_) == false) + { + if (!quiet_) + { + RCLCPP_WARN_STREAM( + get_logger(), + "Velocity Smoother : input got inactive leaving us a non-zero target velocity (" << target_vel_.linear.x << ", " << target_vel_.linear.y << ", " << target_vel_.angular.z << "), zeroing...[%s]"); + } + target_vel_ = ZERO_VEL_COMMAND; + } + } + + // check if the feedback is off from what we expect + // don't care about min / max velocities here, just for rough checking + double period_buffer = 2.0; + + double v_deviation_lower_bound = last_cmd_vel_.linear.x - decel_lim_v_ * period * period_buffer; + double v_deviation_upper_bound = last_cmd_vel_.linear.x + accel_lim_v_ * period * period_buffer; + + double w_deviation_lower_bound = last_cmd_vel_.angular.z - decel_lim_w_ * period * period_buffer; + double angular_max_deviation = last_cmd_vel_.angular.z + accel_lim_w_ * period * period_buffer; + + bool v_different_from_feedback = current_vel_.linear.x < v_deviation_lower_bound || current_vel_.linear.x > v_deviation_upper_bound; + bool w_different_from_feedback = current_vel_.angular.z < w_deviation_lower_bound || current_vel_.angular.z > angular_max_deviation; + + if ((robot_feedback_ != NONE) && (input_active_ == true) && (cb_avg_time_ > 0.0) && + (((rclcpp::Clock().now() - last_cb_time_).seconds() > 5.0 * cb_avg_time_) || // 5 missing msgs + v_different_from_feedback || w_different_from_feedback)) + { + // If the publisher has been inactive for a while, or if our current commanding differs a lot + // from robot velocity feedback, we cannot trust the former; relay on robot's feedback instead + // This might not work super well using the odometry if it has a high delay + if (!quiet_) + { + // this condition can be unavoidable due to preemption of current velocity control on + // velocity multiplexer so be quiet if we're instructed to do so + RCLCPP_WARN( + get_logger(), + "Velocity Smoother : using robot velocity feedback %s instead of last command: %s", + std::string(robot_feedback_ == ODOMETRY ? "odometry" : "end commands").c_str(), + ((rclcpp::Clock().now() - last_cb_time_).seconds() > 5.0 * cb_avg_time_) ? "not received for a while" : "deviates a lot from robot feedback"); + } + target_vel_ = current_vel_; + } + + geometry_msgs::msg::Twist cmd_vel; + + if ((target_vel_.linear.x != last_cmd_vel_.linear.x) || + (target_vel_.linear.y != last_cmd_vel_.linear.y) || + (target_vel_.angular.z != last_cmd_vel_.angular.z)) + { + // Try to reach target velocity ensuring that we don't exceed the acceleration limits + cmd_vel = target_vel_; + + double v_inc, v_inc_y, w_inc, max_v_inc, max_v_inc_y, max_w_inc; + + v_inc = target_vel_.linear.x - last_cmd_vel_.linear.x; + if ((robot_feedback_ == ODOMETRY) && (current_vel_.linear.x * target_vel_.linear.x < 0.0)) + { + // countermarch (on robots with significant inertia; requires odometry feedback to be detected) + max_v_inc = decel_lim_v_ * period; + } + else + { + max_v_inc = ((v_inc * target_vel_.linear.x > 0.0) ? accel_lim_v_ : decel_lim_v_) * period; + } + + v_inc_y = target_vel_.linear.y - last_cmd_vel_.linear.y; + if ((robot_feedback_ == ODOMETRY) && (current_vel_.linear.y * target_vel_.linear.y < 0.0)) + { + // countermarch (on robots with significant inertia; requires odometry feedback to be detected) + max_v_inc_y = decel_lim_v_ * period; + } + else + { + max_v_inc_y = ((v_inc_y * target_vel_.linear.y > 0.0) ? accel_lim_v_ : decel_lim_v_) * period; + } + + w_inc = target_vel_.angular.z - last_cmd_vel_.angular.z; + if ((robot_feedback_ == ODOMETRY) && (current_vel_.angular.z * target_vel_.angular.z < 0.0)) + { + // countermarch (on robots with significant inertia; requires odometry feedback to be detected) + max_w_inc = decel_lim_w_ * period; + } + else + { + max_w_inc = ((w_inc * target_vel_.angular.z > 0.0) ? accel_lim_w_ : decel_lim_w_) * period; + } + + // Calculate and normalise vectors A (desired velocity increment) and B (maximum velocity increment), + // where v acts as coordinate x and w as coordinate y; the sign of the angle from A to B determines + // which velocity (v or w) must be overconstrained to keep the direction provided as command + double MA = sqrtf(v_inc * v_inc + w_inc * w_inc); + double MB = sqrtf(max_v_inc * max_v_inc + max_w_inc * max_w_inc); + + double Av = std::abs(v_inc) / MA; + double Aw = std::abs(w_inc) / MA; + double Bv = max_v_inc / MB; + double Bw = max_w_inc / MB; + double theta = atan2f(Bw, Bv) - atan2f(Aw, Av); + + if (theta < 0) + { + // overconstrain linear velocity + max_v_inc = (max_w_inc * std::abs(v_inc)) / std::abs(w_inc); + } + else + { + // overconstrain angular velocity + max_w_inc = (max_v_inc * std::abs(w_inc)) / std::abs(v_inc); + } + + if (std::abs(v_inc) > max_v_inc) + { + // we must limit linear velocity + cmd_vel.linear.x = last_cmd_vel_.linear.x + sign(v_inc) * max_v_inc; + } + + if (std::abs(v_inc_y) > max_v_inc_y) + { + // we must limit linear velocity + cmd_vel.linear.y = last_cmd_vel_.linear.y + sign(v_inc_y) * max_v_inc_y; + } + + if (std::abs(w_inc) > max_w_inc) + { + // we must limit angular velocity + cmd_vel.angular.z = last_cmd_vel_.angular.z + sign(w_inc) * max_w_inc; + } + smooth_vel_pub_->publish(cmd_vel); + last_cmd_vel_ = cmd_vel; + } + else if (input_active_ == true) + { + // We already reached target velocity; just keep resending last command while input is active + cmd_vel = last_cmd_vel_; + smooth_vel_pub_->publish(cmd_vel); + } + + spin_rate.sleep(); + } + } +} + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto smoother = std::make_shared(); + smoother->spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/velocity_smoother_nodelet.cpp b/src/velocity_smoother_nodelet.cpp deleted file mode 100644 index e46d3cb..0000000 --- a/src/velocity_smoother_nodelet.cpp +++ /dev/null @@ -1,398 +0,0 @@ -/** - * @file /src/velocity_smoother_nodelet.cpp - * - * @brief Velocity smoother implementation. - * - * License: BSD - * https://raw.github.com/yujinrobot/yujin_ocs/hydro/yocs_velocity_smoother/LICENSE - **/ -/***************************************************************************** - ** Includes - *****************************************************************************/ - -#include -#include -#include - -#include -#include - -#include - -#include "yocs_velocity_smoother/velocity_smoother_nodelet.hpp" - -/***************************************************************************** - ** Preprocessing - *****************************************************************************/ - -#define PERIOD_RECORD_SIZE 5 -#define ZERO_VEL_COMMAND geometry_msgs::Twist(); -#define IS_ZERO_VEOCITY(a) ((a.linear.x == 0.0) && (a.linear.y == 0.0) && (a.angular.z == 0.0)) - -/***************************************************************************** -** Namespaces -*****************************************************************************/ - -namespace yocs_velocity_smoother { - -/********************* -** Implementation -**********************/ - -VelocitySmoother::VelocitySmoother(const std::string &name) -: name(name) -, quiet(false) -, shutdown_req(false) -, input_active(false) -, pr_next(0) -, dynamic_reconfigure_server(NULL) -{ -}; - -void VelocitySmoother::reconfigCB(yocs_velocity_smoother::paramsConfig &config, uint32_t level) -{ - ROS_INFO("Reconfigure request : %f %f %f %f %f", - config.speed_lim_v, config.speed_lim_w, config.accel_lim_v, config.accel_lim_w, config.decel_factor); - - locker.lock(); - speed_lim_v_x = config.speed_lim_v; - speed_lim_v_y = speed_lim_v_x * 0.6; - speed_lim_w = config.speed_lim_w; - accel_lim_v = config.accel_lim_v; - accel_lim_w = config.accel_lim_w; - decel_factor = config.decel_factor; - decel_lim_v = decel_factor*accel_lim_v; - decel_lim_w = decel_factor*accel_lim_w; - locker.unlock(); -} - -void VelocitySmoother::velocityCB(const geometry_msgs::Twist::ConstPtr& msg) -{ - // Estimate commands frequency; we do continuously as it can be very different depending on the - // publisher type, and we don't want to impose extra constraints to keep this package flexible - if (period_record.size() < PERIOD_RECORD_SIZE) - { - period_record.push_back((ros::Time::now() - last_cb_time).toSec()); - } - else - { - period_record[pr_next] = (ros::Time::now() - last_cb_time).toSec(); - } - - pr_next++; - pr_next %= period_record.size(); - last_cb_time = ros::Time::now(); - - if (period_record.size() <= PERIOD_RECORD_SIZE/2) - { - // wait until we have some values; make a reasonable assumption (10 Hz) meanwhile - cb_avg_time = 0.1; - } - else - { - // enough; recalculate with the latest input - cb_avg_time = median(period_record); - } - - input_active = true; - - // Bound speed with the maximum values - locker.lock(); - target_vel.linear.x = - msg->linear.x > 0.0 ? std::min(msg->linear.x, speed_lim_v_x) : std::max(msg->linear.x, -speed_lim_v_x); - target_vel.linear.y = - msg->linear.y > 0.0 ? std::min(msg->linear.y, speed_lim_v_y) : std::max(msg->linear.y, -speed_lim_v_y); - target_vel.angular.z = - msg->angular.z > 0.0 ? std::min(msg->angular.z, speed_lim_w) : std::max(msg->angular.z, -speed_lim_w); - locker.unlock(); -} - -void VelocitySmoother::odometryCB(const nav_msgs::Odometry::ConstPtr& msg) -{ - if (robot_feedback == ODOMETRY) - current_vel = msg->twist.twist; - - // ignore otherwise -} - -void VelocitySmoother::robotVelCB(const geometry_msgs::Twist::ConstPtr& msg) -{ - if (robot_feedback == COMMANDS) - current_vel = *msg; - - // ignore otherwise -} - -void VelocitySmoother::spin() -{ - double period = 1.0/frequency; - ros::Rate spin_rate(frequency); - - while (! shutdown_req && ros::ok()) - { - locker.lock(); - double accel_lim_v_(accel_lim_v); - double accel_lim_w_(accel_lim_w); - double decel_factor(decel_factor); - double decel_lim_v_(decel_lim_v); - double decel_lim_w_(decel_lim_w); - locker.unlock(); - - if ((input_active == true) && (cb_avg_time > 0.0) && - ((ros::Time::now() - last_cb_time).toSec() > std::min(3.0*cb_avg_time, 0.5))) - { - // Velocity input no active anymore; normally last command is a zero-velocity one, but reassure - // this, just in case something went wrong with our input, or he just forgot good manners... - // Issue #2, extra check in case cb_avg_time is very big, for example with several atomic commands - // The cb_avg_time > 0 check is required to deal with low-rate simulated time, that can make that - // several messages arrive with the same time and so lead to a zero median - input_active = false; - if (IS_ZERO_VEOCITY(target_vel) == false) - { - if ( !quiet ) - { - ROS_WARN_STREAM("Velocity Smoother : input got inactive leaving us a non-zero target velocity (" - << target_vel.linear.x << ", " << target_vel.linear.y << ", " << target_vel.angular.z << "), zeroing...[" << name << "]"); - } - target_vel = ZERO_VEL_COMMAND; - } - } - - //check if the feedback is off from what we expect - //don't care about min / max velocities here, just for rough checking - double period_buffer = 2.0; - - double v_deviation_lower_bound = last_cmd_vel.linear.x - decel_lim_v_ * period * period_buffer; - double v_deviation_upper_bound = last_cmd_vel.linear.x + accel_lim_v_ * period * period_buffer; - - double w_deviation_lower_bound = last_cmd_vel.angular.z - decel_lim_w_ * period * period_buffer; - double angular_max_deviation = last_cmd_vel.angular.z + accel_lim_w_ * period * period_buffer; - - bool v_different_from_feedback = current_vel.linear.x < v_deviation_lower_bound || current_vel.linear.x > v_deviation_upper_bound; - bool w_different_from_feedback = current_vel.angular.z < w_deviation_lower_bound || current_vel.angular.z > angular_max_deviation; - - if ((robot_feedback != NONE) && (input_active == true) && (cb_avg_time > 0.0) && - (((ros::Time::now() - last_cb_time).toSec() > 5.0*cb_avg_time) || // 5 missing msgs - v_different_from_feedback || w_different_from_feedback)) - { - // If the publisher has been inactive for a while, or if our current commanding differs a lot - // from robot velocity feedback, we cannot trust the former; relay on robot's feedback instead - // This might not work super well using the odometry if it has a high delay - if ( !quiet ) { - // this condition can be unavoidable due to preemption of current velocity control on - // velocity multiplexer so be quiet if we're instructed to do so - ROS_WARN_STREAM("Velocity Smoother : using robot velocity feedback " << - std::string(robot_feedback == ODOMETRY ? "odometry" : "end commands") << - " instead of last command: " << - (ros::Time::now() - last_cb_time).toSec() << ", " << - current_vel.linear.x - last_cmd_vel.linear.x << ", " << - current_vel.angular.z - last_cmd_vel.angular.z << ", [" << name << "]" - ); - } - last_cmd_vel = current_vel; - } - - geometry_msgs::TwistPtr cmd_vel; - - if ((target_vel.linear.x != last_cmd_vel.linear.x) || - (target_vel.linear.y != last_cmd_vel.linear.y) || - (target_vel.angular.z != last_cmd_vel.angular.z)) - { - // Try to reach target velocity ensuring that we don't exceed the acceleration limits - cmd_vel.reset(new geometry_msgs::Twist(target_vel)); - - double v_inc, v_inc_y, w_inc, max_v_inc, max_v_inc_y, max_w_inc; - - v_inc = target_vel.linear.x - last_cmd_vel.linear.x; - if ((robot_feedback == ODOMETRY) && (current_vel.linear.x*target_vel.linear.x < 0.0)) - { - // countermarch (on robots with significant inertia; requires odometry feedback to be detected) - max_v_inc = decel_lim_v_*period; - } - else - { - max_v_inc = ((v_inc*target_vel.linear.x > 0.0)?accel_lim_v:decel_lim_v_)*period; - } - - v_inc_y = target_vel.linear.y - last_cmd_vel.linear.y; - if ((robot_feedback == ODOMETRY) && (current_vel.linear.y*target_vel.linear.y < 0.0)) - { - // countermarch (on robots with significant inertia; requires odometry feedback to be detected) - max_v_inc_y = decel_lim_v_*period; - } - else - { - max_v_inc_y = ((v_inc_y*target_vel.linear.y > 0.0)?accel_lim_v:decel_lim_v_)*period; - } - - w_inc = target_vel.angular.z - last_cmd_vel.angular.z; - if ((robot_feedback == ODOMETRY) && (current_vel.angular.z*target_vel.angular.z < 0.0)) - { - // countermarch (on robots with significant inertia; requires odometry feedback to be detected) - max_w_inc = decel_lim_w_*period; - } - else - { - max_w_inc = ((w_inc*target_vel.angular.z > 0.0)?accel_lim_w_:decel_lim_w_)*period; - } - - // Calculate and normalise vectors A (desired velocity increment) and B (maximum velocity increment), - // where v acts as coordinate x and w as coordinate y; the sign of the angle from A to B determines - // which velocity (v or w) must be overconstrained to keep the direction provided as command - double MA = sqrtf( v_inc * v_inc + w_inc * w_inc); - double MB = sqrtf(max_v_inc * max_v_inc + max_w_inc * max_w_inc); - - double Av = std::abs(v_inc) / MA; - double Aw = std::abs(w_inc) / MA; - double Bv = max_v_inc / MB; - double Bw = max_w_inc / MB; - double theta = atan2f(Bw, Bv) - atan2f(Aw, Av); - - if (theta < 0) - { - // overconstrain linear velocity - max_v_inc = (max_w_inc*std::abs(v_inc))/std::abs(w_inc); - } - else - { - // overconstrain angular velocity - max_w_inc = (max_v_inc*std::abs(w_inc))/std::abs(v_inc); - } - - if (std::abs(v_inc) > max_v_inc) - { - // we must limit linear velocity - cmd_vel->linear.x = last_cmd_vel.linear.x + sign(v_inc)*max_v_inc; - } - - if (std::abs(v_inc_y) > max_v_inc_y) - { - // we must limit linear velocity - cmd_vel->linear.y = last_cmd_vel.linear.y + sign(v_inc_y)*max_v_inc_y; - } - - if (std::abs(w_inc) > max_w_inc) - { - // we must limit angular velocity - cmd_vel->angular.z = last_cmd_vel.angular.z + sign(w_inc)*max_w_inc; - } - smooth_vel_pub.publish(cmd_vel); - last_cmd_vel = *cmd_vel; - } - else if (input_active == true) - { - // We already reached target velocity; just keep resending last command while input is active - cmd_vel.reset(new geometry_msgs::Twist(last_cmd_vel)); - smooth_vel_pub.publish(cmd_vel); - } - - spin_rate.sleep(); - } -} - -/** - * Initialise from a nodelet's private nodehandle. - * @param nh : private nodehandle - * @return bool : success or failure - */ -bool VelocitySmoother::init(ros::NodeHandle& nh) -{ - // Dynamic Reconfigure - dynamic_reconfigure_callback = boost::bind(&VelocitySmoother::reconfigCB, this, _1, _2); - - dynamic_reconfigure_server = new dynamic_reconfigure::Server(nh); - dynamic_reconfigure_server->setCallback(dynamic_reconfigure_callback); - - // Optional parameters - int feedback; - nh.param("frequency", frequency, 20.0); - nh.param("quiet", quiet, quiet); - nh.param("decel_factor", decel_factor, 1.0); - nh.param("robot_feedback", feedback, (int)NONE); - - if ((int(feedback) < NONE) || (int(feedback) > COMMANDS)) - { - ROS_WARN("Invalid robot feedback type (%d). Valid options are 0 (NONE, default), 1 (ODOMETRY) and 2 (COMMANDS)", - feedback); - feedback = NONE; - } - - robot_feedback = static_cast(feedback); - - // Mandatory parameters - if ((nh.getParam("speed_lim_v_x", speed_lim_v_x) == false) || - (nh.getParam("speed_lim_v_y", speed_lim_v_y) == false) || - (nh.getParam("speed_lim_w", speed_lim_w) == false)) - { - ROS_ERROR("Missing velocity limit parameter(s)"); - return false; - } - - if ((nh.getParam("accel_lim_v", accel_lim_v) == false) || - (nh.getParam("accel_lim_w", accel_lim_w) == false)) - { - ROS_ERROR("Missing acceleration limit parameter(s)"); - return false; - } - - // Deceleration can be more aggressive, if necessary - decel_lim_v = decel_factor*accel_lim_v; - decel_lim_w = decel_factor*accel_lim_w; - - // Publishers and subscribers - odometry_sub = nh.subscribe("odometry", 1, &VelocitySmoother::odometryCB, this); - current_vel_sub = nh.subscribe("robot_cmd_vel", 1, &VelocitySmoother::robotVelCB, this); - raw_in_vel_sub = nh.subscribe("raw_cmd_vel", 1, &VelocitySmoother::velocityCB, this); - smooth_vel_pub = nh.advertise ("smooth_cmd_vel", 1); - - return true; -} - - -/********************* -** Nodelet -**********************/ - -class VelocitySmootherNodelet : public nodelet::Nodelet -{ -public: - VelocitySmootherNodelet() { } - ~VelocitySmootherNodelet() - { - NODELET_DEBUG("Velocity Smoother : waiting for worker thread to finish..."); - vel_smoother_->shutdown(); - worker_thread_.join(); - } - - std::string unresolvedName(const std::string &name) const { - size_t pos = name.find_last_of('/'); - return name.substr(pos + 1); - } - - - virtual void onInit() - { - ros::NodeHandle ph = getPrivateNodeHandle(); - std::string resolved_name = ph.getUnresolvedNamespace(); // this always returns like /robosem/goo_arm - why not unresolved? - std::string name = unresolvedName(resolved_name); // unresolve it ourselves - NODELET_DEBUG_STREAM("Velocity Smoother : initialising nodelet...[" << name << "]"); - vel_smoother_.reset(new VelocitySmoother(name)); - if (vel_smoother_->init(ph)) - { - NODELET_DEBUG_STREAM("Velocity Smoother : nodelet initialised [" << name << "]"); - worker_thread_.start(&VelocitySmoother::spin, *vel_smoother_); - } - else - { - NODELET_ERROR_STREAM("Velocity Smoother : nodelet initialisation failed [" << name << "]"); - } - } - -private: - boost::shared_ptr vel_smoother_; - ecl::Thread worker_thread_; -}; - -} // namespace yocs_velocity_smoother - -PLUGINLIB_EXPORT_CLASS(yocs_velocity_smoother::VelocitySmootherNodelet, nodelet::Nodelet); diff --git a/test/test_translational_input.py b/test/test_translational_input.py deleted file mode 100755 index 7996291..0000000 --- a/test/test_translational_input.py +++ /dev/null @@ -1,147 +0,0 @@ -#!/usr/bin/env python -import roslib -roslib.load_manifest('yocs_velocity_smoother') -import rospy - -import os -import sys -import time -from geometry_msgs.msg import Twist -from nav_msgs.msg import Odometry -''' - Varied translational input for a velocity smoother test. -''' - -STATE_RAMP_UP = 0 -STATE_RAMP_LEVEL = 1 -STATE_RAMP_DOWN = 2 -STATE_ZERO = 3 -STATE_UP = 4 -STATE_DOWN = 5 -STATE_UP_AGAIN = 6 -STATE_NOTHING = 7 - -def main(): - rospy.init_node("test_velocity_smoother_input") - cmd_vel_publisher = rospy.Publisher("~cmd_vel", Twist) - odom_publisher = rospy.Publisher("~odom", Odometry) - param = {} - param['velocity_maximum'] = rospy.get_param("~velocity_maximum", 0.50) - param['ramp_increment'] = rospy.get_param("~ramp_increment", 0.02) - rospy.loginfo("Test Input : ramp increment [%f]",param['ramp_increment']) - param['ramp_decrement'] = rospy.get_param("~ramp_decrement", 0.02) - rospy.loginfo("Test Input : ramp decrement [%f]",param['ramp_decrement']) - cmd_vel = Twist() - cmd_vel.linear.x = 0 - cmd_vel.linear.y = 0 - cmd_vel.linear.z = 0 - cmd_vel.angular.x = 0 - cmd_vel.angular.y = 0 - cmd_vel.angular.z = 0 - odom = Odometry() - odom.header.frame_id = "base_link" - odom.pose.pose.position.x = 0.0 - odom.pose.pose.position.y = 0.0 - odom.pose.pose.position.z = 0.0 - odom.pose.pose.orientation.x = 0.0 - odom.pose.pose.orientation.y = 0.0 - odom.pose.pose.orientation.z = 0.0 - odom.pose.pose.orientation.w = 1.0 - - odom.pose.covariance[0] = 0.1 - odom.pose.covariance[7] = 0.1 - odom.pose.covariance[35] = 0.2 - odom.pose.covariance[14] = 10.0 - odom.pose.covariance[21] = 10.0 - odom.pose.covariance[28] = 10.0 - - odom.twist.twist.linear.x = 0.0 - odom.twist.twist.linear.y = 0.0 - odom.twist.twist.linear.z = 0.0 - odom.twist.twist.angular.x = 0.0 - odom.twist.twist.angular.y = 0.0 - odom.twist.twist.angular.z = 0.0 - state = STATE_RAMP_UP - count = 0 - count_max = 100 - publish = True - #period = 0.01 - timer = rospy.Rate(100) # 10hz - rospy.loginfo("Test Input : STATE_RAMP_UP") - while not rospy.is_shutdown(): - if state == STATE_RAMP_UP: - cmd_vel.linear.x = cmd_vel.linear.x + param['ramp_increment'] - if cmd_vel.linear.x >= param['velocity_maximum']: - state = STATE_RAMP_LEVEL - count = 0 - rospy.loginfo("Test Input : STATE_RAMP_UP -> STATE_RAMP_LEVEL") - elif state == STATE_RAMP_LEVEL: - if count > count_max: # 0.5s - state = STATE_RAMP_DOWN - count = 0 - rospy.loginfo("Test Input : STATE_RAMP_LEVEL -> STATE_RAMP_DOWN") - else: - count = count + 1 - elif state == STATE_RAMP_DOWN: - cmd_vel.linear.x = cmd_vel.linear.x - param['ramp_decrement'] - if cmd_vel.linear.x <= 0.0: - cmd_vel.linear.x = 0.0 - state = STATE_ZERO - count = 0 - rospy.loginfo("Test Input : STATE_RAMP_DOWN -> STATE_ZERO") - elif state == STATE_ZERO: - if count > count_max: # 0.5s - state = STATE_UP - cmd_vel.linear.x = param['velocity_maximum'] - count = 0 - rospy.loginfo("Test Input : STATE_ZERO -> STATE_UP") - else: - count = count + 1 - elif state == STATE_UP: - if count > count_max: # 0.5s - state = STATE_DOWN - cmd_vel.linear.x = 0.0 - count = 0 - rospy.loginfo("Test Input : STATE_UP -> STATE_DOWN") - else: - count = count + 1 - elif state == STATE_DOWN: - if count > count_max: # 0.5s - #state = STATE_UP_AGAIN - #cmd_vel.linear.x = param['velocity_maximum'] - #rospy.loginfo("Test Input : STATE_DOWN -> STATE_UP_AGAIN") - state = STATE_RAMP_UP - cmd_vel.linear.x = 0.0 - rospy.loginfo("Test Input : STATE_DOWN -> STATE_RAMP_UP") - count = 0 - else: - count = count + 1 - elif state == STATE_UP_AGAIN: - if count > count_max: # 0.5s - state = STATE_NOTHING - count = 0 - publish = False - rospy.loginfo("Test Input : STATE_UP_AGAIN -> STATE_NOTHING") - else: - count = count + 1 - elif state == STATE_NOTHING: - if count > count_max: # 0.5s - state = STATE_RAMP_UP - cmd_vel.linear.x = 0.0 - count = 0 - publish = True - rospy.loginfo("Test Input : STATE_NOTHING -> STATE_RAMP_UP") - else: - count = count + 1 - if publish: - odom.twist.twist.linear.x = cmd_vel.linear.x - cmd_vel_publisher.publish(cmd_vel) - else: - # How to fake it when it's not publishing a cmd velocity? Up to the velocity controller there - odom.twist.twist.linear.x = cmd_vel.linear.x - odom.header.stamp = rospy.Time().now() - odom_publisher.publish(odom) - timer.sleep() - -if __name__ == "__main__": - main() From 7e2f39c5cb7ed59994d99f56cd455018d4e1083f Mon Sep 17 00:00:00 2001 From: sskorol Date: Mon, 23 Jan 2023 20:03:33 +0000 Subject: [PATCH 2/3] Added missing parameters and polished the code to match ROS2 Humble requirements. --- README.md | 2 +- launch/velocity_smoother.launch.py | 43 +++++++++++++++++------------- package.xml | 2 +- src/velocity_smoother.cpp | 35 +++++++++++++++++++----- 4 files changed, 55 insertions(+), 27 deletions(-) diff --git a/README.md b/README.md index 574a5ad..a5a98cd 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,6 @@ Champ Quadruped Robot's velocity smoother node. This is a forked version of [yocs_velocity_smooter](https://github.com/yujinrobot/yujin_ocs). -The software has been modified to add support velocity smoothing in the y axis. +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. diff --git a/launch/velocity_smoother.launch.py b/launch/velocity_smoother.launch.py index 6f543a6..4aab8f5 100644 --- a/launch/velocity_smoother.launch.py +++ b/launch/velocity_smoother.launch.py @@ -1,29 +1,36 @@ -import os +from os import path -import launch -import launch_ros.actions +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - config = os.path.join( + config = path.join( get_package_share_directory('yocs_velocity_smoother'), 'config', 'params.yaml' ) + node_name = LaunchConfiguration("node_name", default="velocity_smoother") + config_file = LaunchConfiguration("config_file", default=config) + 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") - velocity_smoother_node = launch_ros.actions.Node( - package='yocs_velocity_smoother', - executable='velocity_smoother', - name='velocity_smoother', - parameters = [config], - output='screen', - remappings=[('velocity_smoother/raw_cmd_vel', '/raw_cmd_vel'), - ('velocity_smoother/smooth_cmd_vel', '/smooth_cmd_vel'), - ('velocity_smoother/robot_cmd_vel', '/robot_cmd_vel'), - ('velocity_smoother/odometry', '/odom')] - ) - - return launch.LaunchDescription([ - velocity_smoother_node + return LaunchDescription([ + Node( + package='yocs_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + parameters=[config_file], + output='screen', + remappings=[ + ([node_name, '/raw_cmd_vel'], raw_cmd_vel_topic), + ([node_name, '/smooth_cmd_vel'], smooth_cmd_vel_topic), + ([node_name, '/robot_cmd_vel'], robot_cmd_vel_topic), + ([node_name, '/odometry'], odom_topic) + ] + ) ]) diff --git a/package.xml b/package.xml index 6b9ed45..2d701bd 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> yocs_velocity_smoother - 0.0.1 + 1.0.0 Bound incoming velocity messages according to robot velocity and acceleration limits. diff --git a/src/velocity_smoother.cpp b/src/velocity_smoother.cpp index 716cbda..4eabda7 100644 --- a/src/velocity_smoother.cpp +++ b/src/velocity_smoother.cpp @@ -18,15 +18,20 @@ namespace yocs_velocity_smoother pr_next_(0) { declare_parameter("speed_lim_v_x", 1.0); + declare_parameter("speed_lim_v_y", 1.0); declare_parameter("accel_lim_v", 0.5); declare_parameter("speed_lim_w", 5.0); declare_parameter("accel_lim_w", 2.5); declare_parameter("decel_factor", 1.0); declare_parameter("frequency", 20.0); - declare_parameter("robot_feedback_", (int)NONE); + declare_parameter("robot_feedback", (int)NONE); speed_lim_v_x_ = get_parameter("speed_lim_v_x").as_double(); - speed_lim_v_y_ = speed_lim_v_x_ * 0.6; + speed_lim_v_y_ = get_parameter("speed_lim_v_y").as_double(); + // Apply the original logic (ignore v_y arg) if default value is used + if (speed_lim_v_y_ == 1.0) { + speed_lim_v_y_ = speed_lim_v_x_ * 0.6; + } accel_lim_v_ = get_parameter("accel_lim_v").as_double(); decel_factor_ = get_parameter("decel_factor").as_double(); decel_lim_v_ = decel_factor_ * accel_lim_v_; @@ -34,13 +39,30 @@ namespace yocs_velocity_smoother accel_lim_w_ = get_parameter("accel_lim_w").as_double(); decel_lim_w_ = decel_factor_ * accel_lim_w_; frequency_ = get_parameter("frequency").as_double(); - robot_feedback_ = static_cast(get_parameter("robot_feedback_").as_int()); - - smooth_vel_pub_ = create_publisher("cmd_vel", rclcpp::QoS(10)); + robot_feedback_ = static_cast(get_parameter("robot_feedback").as_int()); + + RCLCPP_INFO_STREAM( + get_logger(), + "Speed limits: v_x=" << speed_lim_v_x_ << ", v_y=" << speed_lim_v_y_ << ", w=" << speed_lim_w_ + ); + RCLCPP_INFO_STREAM( + get_logger(), + "Accel limits: v=" << accel_lim_v_ << ", w=" << accel_lim_w_ + ); + RCLCPP_INFO_STREAM( + get_logger(), + "Decel limits: v=" << decel_lim_v_ << ", w=" << decel_lim_w_ << ", factor=" << decel_factor_ + ); + RCLCPP_INFO_STREAM( + get_logger(), + "Frequency: " << frequency_ << ". Feedback: " << robot_feedback_ + ); + + smooth_vel_pub_ = create_publisher("smooth_cmd_vel", rclcpp::QoS(10)); raw_in_vel_sub_ = create_subscription( "raw_cmd_vel", rclcpp::QoS(10), std::bind(&VelocitySmoother::velocityCB, this, _1)); current_vel_sub_ = create_subscription( - "current_velocity", rclcpp::QoS(10), std::bind(&VelocitySmoother::robotVelCB, this, _1)); + "robot_cmd_vel", rclcpp::QoS(10), std::bind(&VelocitySmoother::robotVelCB, this, _1)); odometry_sub_ = create_subscription( "odometry", rclcpp::QoS(10), std::bind(&VelocitySmoother::odometryCB, this, _1)); } @@ -108,7 +130,6 @@ namespace yocs_velocity_smoother locker.lock(); double accel_lim_v_(accel_lim_v_); double accel_lim_w_(accel_lim_w_); - double decel_factor(decel_factor_); double decel_lim_v_(decel_lim_v_); double decel_lim_w_(decel_lim_w_); locker.unlock(); From 28e4a269a0d180c04f528a36e573aa33683d8f63 Mon Sep 17 00:00:00 2001 From: sskorol Date: Mon, 23 Jan 2023 20:09:39 +0000 Subject: [PATCH 3/3] Reverted the author of the lib. --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 2d701bd..79fbf48 100644 --- a/package.xml +++ b/package.xml @@ -8,7 +8,7 @@ Bound incoming velocity messages according to robot velocity and acceleration limits. - Sergey Korol + Jorge Santos Simon Sergey Korol BSD