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..a5a98cd 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
-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.
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..4aab8f5
--- /dev/null
+++ b/launch/velocity_smoother.launch.py
@@ -0,0 +1,36 @@
+from os import path
+
+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 = 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")
+
+ 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 e004ffa..79fbf48 100644
--- a/package.xml
+++ b/package.xml
@@ -1,35 +1,24 @@
-
+
+
+
yocs_velocity_smoother
- 0.12.1
+ 1.0.0
Bound incoming velocity messages according to robot velocity and acceleration limits.
Jorge Santos Simon
- Jihoon Lee
+ 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..4eabda7
--- /dev/null
+++ b/src/velocity_smoother.cpp
@@ -0,0 +1,297 @@
+#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("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);
+
+ speed_lim_v_x_ = get_parameter("speed_lim_v_x").as_double();
+ 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_;
+ 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());
+
+ 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(
+ "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));
+ }
+
+ 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_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()