diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 0c5196e..fe4ae99 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,10 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package yocs_velocity_smoother -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package velocity_smoother +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Refactored for ROS 2 and renamed to velocity_smoother 0.6.3 (2014-12-05) ------------------ diff --git a/CMakeLists.txt b/CMakeLists.txt index d92c1af..3127f08 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,41 +1,52 @@ -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) - -# Dynamic reconfigure support -generate_dynamic_reconfigure_options(cfg/params.cfg) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME}_nodelet - CATKIN_DEPENDS roscpp pluginlib nodelet ecl_threads dynamic_reconfigure geometry_msgs nav_msgs +cmake_minimum_required(VERSION 3.5) +project(velocity_smoother) + +find_package(ament_cmake_ros REQUIRED) +find_package(ecl_build REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) + +ecl_enable_cxx14_compiler() +ecl_enable_cxx_warnings() + +include_directories(include) + +add_library(${PROJECT_NAME} SHARED src/velocity_smoother.cpp) +ament_target_dependencies(${PROJECT_NAME} + "geometry_msgs" + "nav_msgs" + "rcl_interfaces" + "rclcpp" + "rclcpp_components" ) -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) +add_executable(velocity_smoother_node src/velocity_smoother_node.cpp) +ament_target_dependencies(velocity_smoother_node + "rclcpp" +) +target_link_libraries(velocity_smoother_node + ${PROJECT_NAME} +) +rclcpp_components_register_nodes(${PROJECT_NAME} + "velocity_smoother::VelocitySmoother") -install(TARGETS ${PROJECT_NAME}_nodelet - DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) -install(DIRECTORY plugins - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(TARGETS velocity_smoother_node + DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(DIRECTORY param - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} ) + +ament_package() 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/velocity_smoother_params.yaml b/config/velocity_smoother_params.yaml new file mode 100644 index 0000000..189e761 --- /dev/null +++ b/config/velocity_smoother_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_node: + 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 - robot commands (cmd_vel) + robot_feedback: 2 diff --git a/include/velocity_smoother/velocity_smoother.hpp b/include/velocity_smoother/velocity_smoother.hpp new file mode 100644 index 0000000..c17cfd7 --- /dev/null +++ b/include/velocity_smoother/velocity_smoother.hpp @@ -0,0 +1,121 @@ +/* + * Copyright (c) 2012, Yujin Robot. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of Yujin Robot nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/***************************************************************************** + ** Ifdefs + *****************************************************************************/ + +#ifndef VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_ +#define VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_ + +/***************************************************************************** + ** Includes + *****************************************************************************/ + +#include +#include +#include + +#include +#include +#include +#include + +/***************************************************************************** +** Namespaces +*****************************************************************************/ + +namespace velocity_smoother { + +/***************************************************************************** +** VelocitySmoother +*****************************************************************************/ + +class VelocitySmoother final : public rclcpp::Node +{ +public: + explicit VelocitySmoother(const rclcpp::NodeOptions & options); + + ~VelocitySmoother() override; + VelocitySmoother(VelocitySmoother && c) = delete; + VelocitySmoother & operator=(VelocitySmoother && c) = delete; + VelocitySmoother(const VelocitySmoother & c) = delete; + VelocitySmoother & operator=(const VelocitySmoother & c) = delete; + +private: + enum RobotFeedbackType + { + NONE, + ODOMETRY, + COMMANDS + } robot_feedback; /**< What source to use as robot velocity feedback */ + + bool quiet_; /**< Quieten some warnings that are unavoidable because of velocity multiplexing. **/ + double speed_lim_v_, accel_lim_v_, decel_lim_v_; + double speed_lim_w_, accel_lim_w_, decel_lim_w_; + + geometry_msgs::msg::Twist current_vel_; + geometry_msgs::msg::Twist target_vel_; + double last_cmd_vel_linear_x_; + double last_cmd_vel_angular_z_; + + double period_; + double decel_factor_; + bool input_active_; + double cb_avg_time_; + rclcpp::Time last_velocity_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 */ + + rclcpp::Subscription::SharedPtr odometry_sub_; /**< Current velocity from odometry */ + rclcpp::Subscription::SharedPtr current_vel_sub_; /**< Current velocity from commands sent to the robot, not necessarily by this node */ + rclcpp::Subscription::SharedPtr raw_in_vel_sub_; /**< Incoming raw velocity commands */ + rclcpp::Publisher::SharedPtr smooth_vel_pub_; /**< Outgoing smoothed velocity commands */ + rclcpp::TimerBase::SharedPtr timer_; + + void timerCB(); + void velocityCB(const geometry_msgs::msg::Twist::SharedPtr msg); + void robotVelCB(const geometry_msgs::msg::Twist::SharedPtr msg); + void odometryCB(const nav_msgs::msg::Odometry::SharedPtr msg); + + rcl_interfaces::msg::SetParametersResult parameterUpdate( + const std::vector & parameters); + + 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 + std::nth_element(values.begin(), values.begin() + values.size()/2, values.end()); + return values[values.size()/2]; + } +}; + +} // namespace velocity_smoother + +#endif /* VELOCITY_SMOOTHER__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 1c28b26..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, 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-composed-launch.py b/launch/velocity_smoother-composed-launch.py new file mode 100644 index 0000000..af895f6 --- /dev/null +++ b/launch/velocity_smoother-composed-launch.py @@ -0,0 +1,70 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Launch the velocity smoother as a composed node with default configuration.""" + +import os + +import ament_index_python.packages + +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +import yaml + + +def generate_launch_description(): + share_dir = ament_index_python.packages.get_package_share_directory('velocity_smoother') + + # Passing parameters to a composed node must be done via a dictionary of + # key -> value pairs. Here we read in the data from the configuration file + # and create a dictionary of it that the ComposableNode will accept. + params_file = os.path.join(share_dir, 'config', 'velocity_smoother_params.yaml') + with open(params_file, 'r') as f: + params = yaml.safe_load(f)['velocity_smoother_node']['ros__parameters'] + container = ComposableNodeContainer( + node_name='velocity_smoother_container', + node_namespace='', + package='rclcpp_components', + node_executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='velocity_smoother', + node_plugin='velocity_smoother::VelocitySmoother', + node_name='velocity_smoother_node', + parameters=[params]), + ], + output='both', + ) + + return LaunchDescription([container]) diff --git a/launch/velocity_smoother-launch.py b/launch/velocity_smoother-launch.py new file mode 100644 index 0000000..a5d6b32 --- /dev/null +++ b/launch/velocity_smoother-launch.py @@ -0,0 +1,61 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Launch the velocity smoother node with default configuration.""" + +import os + +import ament_index_python.packages +import launch +import launch_ros.actions + +import yaml + + +def generate_launch_description(): + share_dir = ament_index_python.packages.get_package_share_directory('velocity_smoother') + + # There are two different ways to pass parameters to a non-composed node; + # either by specifying the path to the file containing the parameters, or by + # passing a dictionary containing the key -> value pairs of the parameters. + # When starting a *composed* node on the other hand, only the dictionary + # style is supported. To keep the code between the non-composed and + # composed launch file similar, we use that style here as well. + params_file = os.path.join(share_dir, 'config', 'velocity_smoother_params.yaml') + with open(params_file, 'r') as f: + params = yaml.safe_load(f)['velocity_smoother_node']['ros__parameters'] + velocity_smoother_node = launch_ros.actions.Node(package='velocity_smoother', + node_executable='velocity_smoother_node', + output='both', + parameters=[params]) + + return launch.LaunchDescription([velocity_smoother_node]) diff --git a/launch/velocity_smoother.launch b/launch/velocity_smoother.launch deleted file mode 100644 index 28ccaac..0000000 --- a/launch/velocity_smoother.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/package.xml b/package.xml index e004ffa..6553ccd 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,5 @@ - - yocs_velocity_smoother + + velocity_smoother 0.12.1 Bound incoming velocity messages according to robot velocity and acceleration limits. @@ -7,29 +7,26 @@ Jorge Santos Simon Jihoon Lee BSD - http://ros.org/wiki/yocs_velocity_smoother - https://github.com/yujinrobot/yujin_ocs - https://github.com/yujinrobot/yujin_ocs/issues - - catkin + https://index.ros.org/v/velocity_smoother/github-kobuki-base-velocity_smoother + https://github.com/kobuki-base/velocity_smoother + https://github.com/kobuki-base/velocity_smoother/issues - roscpp - pluginlib - nodelet + ament_cmake_ros + + ecl_build geometry_msgs nav_msgs - ecl_threads - dynamic_reconfigure + rcl_interfaces + rclcpp + rclcpp_components + + geometry_msgs + nav_msgs + rcl_interfaces + rclcpp + rclcpp_components - 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..39cee40 --- /dev/null +++ b/src/velocity_smoother.cpp @@ -0,0 +1,413 @@ +/* + * Copyright (c) 2012, Yujin Robot. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of Yujin Robot nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/***************************************************************************** + ** Includes + *****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "velocity_smoother/velocity_smoother.hpp" + +/***************************************************************************** + ** Preprocessing + *****************************************************************************/ + +#define PERIOD_RECORD_SIZE 5 + +/***************************************************************************** +** Namespaces +*****************************************************************************/ + +namespace velocity_smoother { + +/********************* +** Implementation +**********************/ + +VelocitySmoother::VelocitySmoother(const rclcpp::NodeOptions & options) : rclcpp::Node("velocity_smoother_node", options) +, input_active_(false) +, pr_next_(0) +{ + double frequency = this->declare_parameter("frequency", 20.0); + quiet_ = this->declare_parameter("quiet", false); + decel_factor_ = this->declare_parameter("decel_factor", 1.0); + int feedback = this->declare_parameter("robot_feedback", static_cast(NONE)); + + if ((static_cast(feedback) < NONE) || (static_cast(feedback) > COMMANDS)) + { + throw std::runtime_error("Invalid robot feedback type. Valid options are 0 (NONE, default), 1 (ODOMETRY) and 2 (COMMANDS)"); + } + + robot_feedback = static_cast(feedback); + + // Mandatory parameters + rclcpp::ParameterValue speed_v = this->declare_parameter("speed_lim_v"); + if (speed_v.get_type() != rclcpp::ParameterType::PARAMETER_DOUBLE) { + throw std::runtime_error("speed_lim_v must be specified as a double"); + } + speed_lim_v_ = speed_v.get(); + + rclcpp::ParameterValue speed_w = this->declare_parameter("speed_lim_w"); + if (speed_w.get_type() != rclcpp::ParameterType::PARAMETER_DOUBLE) { + throw std::runtime_error("speed_lim_w must be specified as a double"); + } + speed_lim_w_ = speed_w.get(); + + rclcpp::ParameterValue accel_v = this->declare_parameter("accel_lim_v"); + if (accel_v.get_type() != rclcpp::ParameterType::PARAMETER_DOUBLE) { + throw std::runtime_error("accel_lim_v must be specified as a double"); + } + accel_lim_v_ = accel_v.get(); + + rclcpp::ParameterValue accel_w = this->declare_parameter("accel_lim_w"); + if (accel_w.get_type() != rclcpp::ParameterType::PARAMETER_DOUBLE) { + throw std::runtime_error("accel_lim_w must be specified as a double"); + } + accel_lim_w_ = accel_w.get(); + + // 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_ = this->create_subscription("odometry", rclcpp::QoS(1), std::bind(&VelocitySmoother::odometryCB, this, std::placeholders::_1)); + current_vel_sub_ = this->create_subscription("robot_cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::robotVelCB, this, std::placeholders::_1)); + raw_in_vel_sub_ = this->create_subscription("raw_cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::velocityCB, this, std::placeholders::_1)); + smooth_vel_pub_ = this->create_publisher("smooth_cmd_vel", 1); + + period_ = 1.0 / frequency; + timer_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(period_ * 1000.0)), std::bind(&VelocitySmoother::timerCB, this)); +} + +VelocitySmoother::~VelocitySmoother() +{ +} + +void VelocitySmoother::velocityCB(const geometry_msgs::msg::Twist::SharedPtr 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((this->get_clock()->now() - last_velocity_cb_time_).seconds()); + } + else + { + period_record_[pr_next_] = (this->get_clock()->now() - last_velocity_cb_time_).seconds(); + } + + pr_next_++; + pr_next_ %= period_record_.size(); + last_velocity_cb_time_ = this->get_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 + target_vel_.linear.x = + msg->linear.x > 0.0 ? std::min(msg->linear.x, speed_lim_v_) : std::max(msg->linear.x, -speed_lim_v_); + 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_); +} + +void VelocitySmoother::odometryCB(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + if (robot_feedback == ODOMETRY) + { + current_vel_ = msg->twist.twist; + } + + // ignore otherwise +} + +void VelocitySmoother::robotVelCB(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + if (robot_feedback == COMMANDS) + { + current_vel_ = *msg; + } + + // ignore otherwise +} + +void VelocitySmoother::timerCB() +{ + if ((input_active_ == true) && (cb_avg_time_ > 0.0) && + ((this->get_clock()->now() - last_velocity_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 (target_vel_.linear.x != 0.0 || target_vel_.angular.z != 0.0) + { + RCLCPP_WARN(get_logger(), "Velocity Smoother : input went inactive leaving us a non-zero target velocity (%d, %d), zeroing...", + target_vel_.linear.x, + target_vel_.angular.z); + target_vel_ = geometry_msgs::msg::Twist(); + } + } + + // 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) && + (((this->get_clock()->now() - last_velocity_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: %f, %f, %f", + std::string(robot_feedback == ODOMETRY ? "odometry" : "end commands").c_str(), + (this->get_clock()->now() - last_velocity_cb_time_).seconds(), + current_vel_.linear.x - last_cmd_vel_linear_x_, + current_vel_.angular.z - last_cmd_vel_angular_z_); + } + last_cmd_vel_linear_x_ = current_vel_.linear.x; + last_cmd_vel_angular_z_ = current_vel_.angular.z; + } + + auto cmd_vel = std::make_unique(); + + if ((target_vel_.linear.x != last_cmd_vel_linear_x_) || + (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->linear = target_vel_.linear; + cmd_vel->angular = target_vel_.angular; + + double v_inc, w_inc, max_v_inc, 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_; + } + + 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 = std::sqrt( v_inc * v_inc + w_inc * w_inc); + double MB = std::sqrt(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 = std::atan2(Bw, Bv) - atan2(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(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; + } + last_cmd_vel_linear_x_ = cmd_vel->linear.x; + last_cmd_vel_angular_z_ = cmd_vel->angular.z; + smooth_vel_pub_->publish(std::move(cmd_vel)); + } + else if (input_active_ == true) + { + // We already reached target velocity; just keep resending last command while input is active + smooth_vel_pub_->publish(std::move(cmd_vel)); + } +} + +rcl_interfaces::msg::SetParametersResult VelocitySmoother::parameterUpdate( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const rclcpp::Parameter & parameter : parameters) + { + if (parameter.get_name() == "frequency") + { + result.successful = false; + result.reason = "frequency cannot be changed on-the-fly"; + break; + } + else if (parameter.get_name() == "quiet") + { + if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) { + result.successful = false; + result.reason = "quiet must be a boolean"; + break; + } + + quiet_ = parameter.get_value(); + } + else if (parameter.get_name() == "decel_factor") + { + if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_DOUBLE) { + result.successful = false; + result.reason = "decel_factor must be a double"; + break; + } + + decel_factor_ = parameter.get_value(); + } + else if (parameter.get_name() == "robot_feedback") + { + result.successful = false; + result.reason = "robot_feedback cannot be changed on-the-fly"; + break; + } + else if (parameter.get_name() == "speed_lim_v") + { + if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_DOUBLE) { + result.successful = false; + result.reason = "speed_lim_v must be a double"; + break; + } + + speed_lim_v_ = parameter.get_value(); + } + else if (parameter.get_name() == "speed_lim_w") + { + if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_DOUBLE) { + result.successful = false; + result.reason = "speed_lim_w must be a double"; + break; + } + + speed_lim_w_ = parameter.get_value(); + } + else if (parameter.get_name() == "accel_lim_v") + { + if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_DOUBLE) { + result.successful = false; + result.reason = "accel_lim_v must be a double"; + break; + } + + accel_lim_v_ = parameter.get_value(); + } + else if (parameter.get_name() == "accel_lim_w") + { + if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_DOUBLE) { + result.successful = false; + result.reason = "accel_lim_w must be a double"; + break; + } + + accel_lim_w_ = parameter.get_value(); + } + else + { + result.successful = false; + result.reason = "unknown parameter"; + break; + } + } + + if (result.successful) + { + // Since these rely on more than one parameter, update at the end in case + // multiple change at once. + decel_lim_v_ = decel_factor_*accel_lim_v_; + decel_lim_w_ = decel_factor_*accel_lim_w_; + } + + return result; +} + +} + +RCLCPP_COMPONENTS_REGISTER_NODE(velocity_smoother::VelocitySmoother) diff --git a/src/velocity_smoother_node.cpp b/src/velocity_smoother_node.cpp new file mode 100644 index 0000000..420b6be --- /dev/null +++ b/src/velocity_smoother_node.cpp @@ -0,0 +1,49 @@ +/* + * Copyright (c) 2020, Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of Yujin Robot nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#include + +#include "velocity_smoother/velocity_smoother.hpp" + +int main(int argc, char** argv) +{ + // Force flush of the stdout buffer. + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + + rclcpp::init(argc, argv); + + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); + + rclcpp::shutdown(); + + return 0; +} diff --git a/src/velocity_smoother_nodelet.cpp b/src/velocity_smoother_nodelet.cpp deleted file mode 100644 index 204fc49..0000000 --- a/src/velocity_smoother_nodelet.cpp +++ /dev/null @@ -1,373 +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.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 = config.speed_lim_v; - 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) : std::max(msg->linear.x, -speed_lim_v); - 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) - { - ROS_WARN_STREAM("Velocity Smoother : input got inactive leaving us a non-zero target velocity (" - << target_vel.linear.x << ", " << 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.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, w_inc, max_v_inc, 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; - } - - 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 = sqrt( v_inc * v_inc + w_inc * w_inc); - double MB = sqrt(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 = atan2(Bw, Bv) - atan2(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(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", speed_lim_v) == 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 index 7996291..92e1717 100755 --- a/test/test_translational_input.py +++ b/test/test_translational_input.py @@ -1,6 +1,6 @@ #!/usr/bin/env python import roslib -roslib.load_manifest('yocs_velocity_smoother') +roslib.load_manifest('velocity_smoother') import rospy import os