From f082f0aa171009797b74a116c6ca4bbc02211644 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 20 Oct 2021 08:11:17 -0600 Subject: [PATCH 01/15] Update version of pre-commit action (#733) --- .github/workflows/format.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index f3c976d5e5d..121309898e2 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -19,4 +19,4 @@ jobs: - uses: actions/setup-python@v2 - name: Install clang-format-10 run: sudo apt-get install clang-format-10 - - uses: pre-commit/action@v2.0.0 + - uses: pre-commit/action@v2.0.3 From ed63d6bd8f716078f6b04422e3b4780e4365adc8 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Wed, 20 Oct 2021 18:26:17 +0300 Subject: [PATCH 02/15] Disable upstream_ws cache on changes to .repos files (#674) --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 7e7fd25a0f4..b3c431fdb2f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -67,7 +67,7 @@ jobs: key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }} restore-keys: ${{ env.CACHE_PREFIX }} env: - CACHE_PREFIX: upstream_ws-${{ matrix.env.IMAGE }}-${{ hashFiles(env.UPSTREAM_WORKSPACE, '.github/workflows/ci.yaml') }} + CACHE_PREFIX: upstream_ws-${{ matrix.env.IMAGE }}-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }} # The target directory cache doesn't include the source directory because # that comes from the checkout. See "prepare target_ws for cache" task below - name: cache target_ws From a13f3bcb5bfeda85ca8eee232fc757c01311257a Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 13 Sep 2021 08:52:18 -0500 Subject: [PATCH 03/15] Skeleton of the smoothing plugin Alphabetize in CMakeLists Load filter coefficient in the new plugin Do filtering in the plugin Implement 'reset' Purge the previous filter implementation Fix parameter parsing Update unit tests Clang format Code cleanup. This causes an issue with reset() -- need to look into it Do not use const for size_t Fix logic error Sync config files Alphabetize moveit_core subdirectories Move the smoothing plugin to moveit_core Bug fixes Load the plugin name from yaml Remove commented code. Rename the plugin in a generic way Minor cleanup. Delete unused file. Minor cleanup per Nathan's code review Implement a filteredHalt() function Rename the package from smoothing_plugins->online_signal_smoothing Small comments and efficiency improvement Add Butterworth plugin package export Add default constructor for SmoothingBaseClass Add missing pluginlib dependency to moveit_servo Skeleton of the smoothing plugin Load filter coefficient in the new plugin Do filtering in the plugin Implement 'reset' Purge the previous filter implementation Fix parameter parsing Clang format Code cleanup. This causes an issue with reset() -- need to look into it Do not use const for size_t Fix logic error Alphabetize moveit_core subdirectories Move the smoothing plugin to moveit_core Bug fixes Load the plugin name from yaml Remove commented code. Rename the plugin in a generic way Minor cleanup. Delete unused file. Minor cleanup per Nathan's code review Rename the package from smoothing_plugins->online_signal_smoothing Logic fixups, per Nathan Delete default constructor/destructor Post-rebase cleanup Try moving the base class to a separate library Ensure we don't pulish pos if user did not request it (& vel & accel) Try to fix CI for galactic, in particular Restore default constructor and destructor. --- moveit_core/CMakeLists.txt | 79 +++++---- moveit_core/filter_plugin_butterworth.xml | 7 + .../online_signal_smoothing/CMakeLists.txt | 50 ++++++ .../butterworth_filter.h | 113 ++++++++++++ .../smoothing_base_class.h | 75 ++++++++ .../src/butterworth_filter.cpp | 103 +++++++++++ .../src/smoothing_base_class.cpp | 1 + .../test/test_butterworth_filter.cpp | 16 +- moveit_core/package.xml | 2 - moveit_ros/moveit_servo/CMakeLists.txt | 34 ++-- .../config/panda_simulated_config.yaml | 6 +- .../panda_simulated_config_pose_tracking.yaml | 4 +- .../include/moveit_servo/collision_check.h | 1 - .../include/moveit_servo/low_pass_filter.h | 69 -------- .../include/moveit_servo/servo_calcs.h | 23 ++- .../include/moveit_servo/servo_parameters.h | 4 +- moveit_ros/moveit_servo/package.xml | 6 +- .../moveit_servo/src/low_pass_filter.cpp | 97 ----------- moveit_ros/moveit_servo/src/servo_calcs.cpp | 161 +++++++++++++----- .../moveit_servo/src/servo_parameters.cpp | 8 +- .../test/config/servo_settings.yaml | 4 +- .../config/servo_settings_low_latency.yaml | 4 +- 22 files changed, 568 insertions(+), 299 deletions(-) create mode 100644 moveit_core/filter_plugin_butterworth.xml create mode 100644 moveit_core/online_signal_smoothing/CMakeLists.txt create mode 100644 moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h create mode 100644 moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h create mode 100644 moveit_core/online_signal_smoothing/src/butterworth_filter.cpp create mode 100644 moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp rename moveit_ros/moveit_servo/test/test_low_pass_filter.cpp => moveit_core/online_signal_smoothing/test/test_butterworth_filter.cpp (86%) delete mode 100644 moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h delete mode 100644 moveit_ros/moveit_servo/src/low_pass_filter.cpp diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 8cc6e8140a0..dac0a6481e8 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -75,6 +75,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS robot_trajectory/include kinematic_constraints/include macros/include + online_signal_smoothing/include planning_interface/include planning_request_adapter/include planning_scene/include @@ -87,31 +88,33 @@ set(THIS_PACKAGE_INCLUDE_DIRS ) set(THIS_PACKAGE_LIBRARIES - moveit_exceptions moveit_background_processing - moveit_kinematics_base - moveit_robot_model - moveit_transforms - moveit_robot_state - moveit_robot_trajectory - moveit_planning_interface + moveit_butterworth_filter + moveit_collision_distance_field moveit_collision_detection moveit_collision_detection_fcl moveit_collision_detection_bullet + moveit_dynamics_solver + moveit_constraint_samplers + moveit_distance_field + moveit_exceptions + moveit_kinematics_base moveit_kinematic_constraints + moveit_kinematics_metrics + moveit_planning_interface moveit_planning_scene - moveit_constraint_samplers moveit_planning_request_adapter moveit_profiler # TODO: Port python bindings # moveit_python_tools + moveit_robot_model + moveit_robot_state + moveit_robot_trajectory + moveit_smoothing_base_class + moveit_test_utils moveit_trajectory_processing - moveit_distance_field - moveit_collision_distance_field - moveit_kinematics_metrics - moveit_dynamics_solver + moveit_transforms moveit_utils - moveit_test_utils ) set(THIS_PACKAGE_INCLUDE_DEPENDS @@ -138,9 +141,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ruckig ) -pluginlib_export_plugin_description_file(moveit_core collision_detector_fcl_description.xml) -pluginlib_export_plugin_description_file(moveit_core collision_detector_bullet_description.xml) - include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} ${LIBFCL_INCLUDE_DIRS} ) @@ -158,35 +158,35 @@ message(STATUS " *** Building MoveIt ${MOVEIT_VERSION} ***") configure_file("version/version.h.in" "${VERSION_FILE_PATH}/moveit/version.h") install(FILES "${VERSION_FILE_PATH}/moveit/version.h" DESTINATION include/moveit) -add_subdirectory(version) -add_subdirectory(macros) +add_subdirectory(background_processing) add_subdirectory(backtrace) +add_subdirectory(collision_distance_field) +add_subdirectory(constraint_samplers) +add_subdirectory(controller_manager) +add_subdirectory(distance_field) +add_subdirectory(dynamics_solver) add_subdirectory(exceptions) +add_subdirectory(kinematics_base) +add_subdirectory(kinematic_constraints) +add_subdirectory(kinematics_metrics) +add_subdirectory(macros) +add_subdirectory(online_signal_smoothing) +add_subdirectory(planning_interface) +add_subdirectory(planning_request_adapter) +add_subdirectory(planning_scene) add_subdirectory(profiler) -add_subdirectory(utils) -add_subdirectory(background_processing) add_subdirectory(robot_model) -add_subdirectory(collision_detection) -add_subdirectory(kinematics_base) -add_subdirectory(transforms) add_subdirectory(robot_state) -add_subdirectory(collision_detection_fcl) add_subdirectory(robot_trajectory) -add_subdirectory(kinematic_constraints) -add_subdirectory(trajectory_processing) -add_subdirectory(planning_scene) -add_subdirectory(controller_manager) add_subdirectory(sensor_manager) -add_subdirectory(constraint_samplers) -add_subdirectory(planning_interface) -add_subdirectory(planning_request_adapter) -add_subdirectory(distance_field) -add_subdirectory(collision_distance_field) -add_subdirectory(kinematics_metrics) -add_subdirectory(dynamics_solver) -add_subdirectory(collision_detection_bullet) +add_subdirectory(trajectory_processing) +add_subdirectory(transforms) +add_subdirectory(utils) +add_subdirectory(version) -pluginlib_export_plugin_description_file(moveit_core collision_detector_bullet_description.xml) +add_subdirectory(collision_detection) +add_subdirectory(collision_detection_bullet) +add_subdirectory(collision_detection_fcl) # TODO: Port python bindings # add_subdirectory(python) @@ -228,6 +228,11 @@ install( ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Plugin exports +pluginlib_export_plugin_description_file(moveit_core collision_detector_fcl_description.xml) +pluginlib_export_plugin_description_file(moveit_core collision_detector_bullet_description.xml) +pluginlib_export_plugin_description_file(moveit_core filter_plugin_butterworth.xml) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/moveit_core/filter_plugin_butterworth.xml b/moveit_core/filter_plugin_butterworth.xml new file mode 100644 index 00000000000..e38b3d5558f --- /dev/null +++ b/moveit_core/filter_plugin_butterworth.xml @@ -0,0 +1,7 @@ + + + + Simple Butterworth 1st order lowpass filter that only has one tuneable parameter and does not overshoot. + + + diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/moveit_core/online_signal_smoothing/CMakeLists.txt new file mode 100644 index 00000000000..0025fd24532 --- /dev/null +++ b/moveit_core/online_signal_smoothing/CMakeLists.txt @@ -0,0 +1,50 @@ +# Base class + +add_library(moveit_smoothing_base_class SHARED + src/smoothing_base_class.cpp +) +include(GenerateExportHeader) +generate_export_header(moveit_smoothing_base_class) +target_include_directories(moveit_smoothing_base_class PUBLIC + $ +) +set_target_properties(moveit_smoothing_base_class PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}" +) +target_link_libraries(moveit_smoothing_base_class + moveit_robot_model +) + +# Plugin implementations + +add_library(moveit_butterworth_filter SHARED + src/butterworth_filter.cpp +) +include(GenerateExportHeader) +generate_export_header(moveit_butterworth_filter) +target_include_directories(moveit_butterworth_filter PUBLIC + $ +) +set_target_properties(moveit_butterworth_filter PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}" +) +target_link_libraries(moveit_butterworth_filter + moveit_smoothing_base_class +) + +# Installation + +install(DIRECTORY include/ DESTINATION include) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_butterworth_filter_export.h DESTINATION include) + +# Testing + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(ros_testing REQUIRED) + + # Lowpass filter unit test + ament_add_gtest(test_butterworth_filter test/test_butterworth_filter.cpp) + target_link_libraries(test_butterworth_filter moveit_butterworth_filter) +endif() diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h new file mode 100644 index 00000000000..e0bf59efe98 --- /dev/null +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h @@ -0,0 +1,113 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik 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 PickNik Inc. 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. + *********************************************************************/ + +/* Author: Andy Zelenak + Description: A first-order Butterworth low-pass filter. There is only one parameter to tune. + */ + +#pragma once + +#include + +#include +#include + +namespace online_signal_smoothing +{ +/** + * Class ButterworthFilter - Implementation of a signal filter to soften jerks. + * This is a first-order Butterworth low-pass filter. First-order was chosen for 2 reasons: + * - It doesn't overshoot + * - Computational efficiency + */ +class ButterworthFilter +{ +public: + /** + * Constructor. + * @param low_pass_filter_coeff Larger filter_coeff-> more smoothing of servo commands, but more lag. + * Rough plot, with cutoff frequency on the y-axis: + * https://www.wolframalpha.com/input/?i=plot+arccot(c) + */ + ButterworthFilter(double low_pass_filter_coeff); + + double filter(double new_measurement); + + void reset(const double data); + +private: + static constexpr std::size_t FILTER_LENGTH = 2; + std::array previous_measurements_; + double previous_filtered_measurement_; + // Scale and feedback term are calculated from supplied filter coefficient + double scale_term_; + double feedback_term_; +}; + +// Plugin +class ButterworthFilterPlugin : public SmoothingBaseClass +{ +public: + ButterworthFilterPlugin(){}; + + /** + * Initialize the smoothing algorithm + * @param node ROS node, used for parameter retrieval + * @param robot_model typically used to retrieve vel/accel/jerk limits + * @param num_joints number of actuated joints in the JointGroup Servo controls + * @return True if initialization was successful + */ + bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t num_joints) override; + + /** + * Smooth the command signals for all DOF + * @param position_vector array of joint position commands + * @return True if initialization was successful + */ + bool doSmoothing(std::vector& position_vector) override; + + /** + * Reset to a given joint state + * @param joint_positions reset the filters to these joint positions + * @return True if reset was successful + */ + bool reset(const std::vector& joint_positions) override; + +private: + rclcpp::Node::SharedPtr node_; + std::vector position_filters_; + size_t num_joints_; +}; +} // namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h new file mode 100644 index 00000000000..8c8ff08603d --- /dev/null +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h @@ -0,0 +1,75 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik 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 PickNik Inc. 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. + *********************************************************************/ + +/* Author: Andy Zelenak + Description: Defines a pluginlib interface for smoothing algorithms. + */ + +#pragma once + +#include + +namespace online_signal_smoothing +{ +class SmoothingBaseClass +{ +public: + SmoothingBaseClass() = default; + ~SmoothingBaseClass() = default; + + /** + * Initialize the smoothing algorithm + * @param node ROS node, typically used for parameter retrieval + * @param robot_model typically used to retrieve vel/accel/jerk limits + * @param num_joints number of actuated joints in the JointGroup Servo controls + * @return True if initialization was successful + */ + virtual bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t num_joints) = 0; + + /** + * Smooth an array of joint position deltas + * @param position_vector array of joint position commands + * @return True if initialization was successful + */ + virtual bool doSmoothing(std::vector& position_vector) = 0; + + /** + * Reset to a given joint state + * @param joint_positions reset the filters to these joint positions + * @return True if reset was successful + */ + virtual bool reset(const std::vector& joint_positions) = 0; +}; +} // namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp b/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp new file mode 100644 index 00000000000..1e3dddf02d6 --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp @@ -0,0 +1,103 @@ +#include + +namespace online_signal_smoothing +{ +namespace +{ +constexpr double EPSILON = 1e-9; +} + +ButterworthFilter::ButterworthFilter(double low_pass_filter_coeff) + : previous_measurements_{ 0., 0. } + , previous_filtered_measurement_(0.) + , scale_term_(1. / (1. + low_pass_filter_coeff)) + , feedback_term_(1. - low_pass_filter_coeff) +{ + // guarantee this doesn't change because the logic below depends on this length implicity + static_assert(ButterworthFilter::FILTER_LENGTH == 2, + "online_signal_smoothing::ButterworthFilter::FILTER_LENGTH should be 2"); + + if (std::isinf(feedback_term_)) + throw std::length_error("online_signal_smoothing::ButterworthFilter: infinite feedback_term_"); + + if (std::isinf(scale_term_)) + throw std::length_error("online_signal_smoothing::ButterworthFilter: infinite scale_term_"); + + if (low_pass_filter_coeff < 1) + throw std::length_error( + "online_signal_smoothing::ButterworthFilter: Filter coefficient < 1. makes the lowpass filter unstable"); + + if (std::abs(feedback_term_) < EPSILON) + throw std::length_error( + "online_signal_smoothing::ButterworthFilter: Filter coefficient value resulted in feedback term of 0"); +} + +double ButterworthFilter::filter(double new_measurement) +{ + // Push in the new measurement + previous_measurements_[1] = previous_measurements_[0]; + previous_measurements_[0] = new_measurement; + + previous_filtered_measurement_ = scale_term_ * (previous_measurements_[1] + previous_measurements_[0] - + feedback_term_ * previous_filtered_measurement_); + + return previous_filtered_measurement_; +} + +void ButterworthFilter::reset(const double data) +{ + previous_measurements_[0] = data; + previous_measurements_[1] = data; + previous_filtered_measurement_ = data; +} + +bool ButterworthFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t num_joints) +{ + node_ = node; + num_joints_ = num_joints; + + for (std::size_t i = 0; i < num_joints_; ++i) + { + // Low-pass filters for the joint positions + // TODO(andyz): read a parameter + position_filters_.emplace_back(1.5 /* filter coefficient, should be >1 */); + } + return true; +}; + +bool ButterworthFilterPlugin::doSmoothing(std::vector& position_vector) +{ + if (position_vector.size() != position_filters_.size()) + { + RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, + "Position vector to be smoothed does not have the right length."); + return false; + } + for (size_t i = 0; i < position_vector.size(); ++i) + { + // Lowpass filter the position command + position_vector[i] = position_filters_.at(i).filter(position_vector[i]); + } + return true; +}; + +bool ButterworthFilterPlugin::reset(const std::vector& joint_positions) +{ + if (joint_positions.size() != position_filters_.size()) + { + RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, + "Position vector to be reset does not have the right length."); + return false; + } + for (size_t joint_idx = 0; joint_idx < joint_positions.size(); ++joint_idx) + { + position_filters_.at(joint_idx).reset(joint_positions.at(joint_idx)); + } + return true; +}; + +} // namespace online_signal_smoothing + +#include +PLUGINLIB_EXPORT_CLASS(online_signal_smoothing::ButterworthFilterPlugin, online_signal_smoothing::SmoothingBaseClass) diff --git a/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp b/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp new file mode 100644 index 00000000000..e6b82f56b7b --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp @@ -0,0 +1 @@ +#include diff --git a/moveit_ros/moveit_servo/test/test_low_pass_filter.cpp b/moveit_core/online_signal_smoothing/test/test_butterworth_filter.cpp similarity index 86% rename from moveit_ros/moveit_servo/test/test_low_pass_filter.cpp rename to moveit_core/online_signal_smoothing/test/test_butterworth_filter.cpp index 9663d7fdc76..f2f944082a6 100644 --- a/moveit_ros/moveit_servo/test/test_low_pass_filter.cpp +++ b/moveit_core/online_signal_smoothing/test/test_butterworth_filter.cpp @@ -32,19 +32,19 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Title : test_low_pass_filters.cpp - * Project : moveit_servo +/* Title : test_butterworth_filter.cpp + * Project : moveit_core * Created : 07/21/2020 * Author : Adam Pettinger - * Desc : Unit test for moveit_servo::LowPassFilter + * Desc : Unit test for moveit::ButterworthFilter */ #include -#include +#include -TEST(MOVEIT_SERVO, FilterConverge) +TEST(SMOOTHING_PLUGINS, FilterConverge) { - moveit_servo::LowPassFilter lpf(2.0); + online_signal_smoothing::ButterworthFilter lpf(2.0); EXPECT_DOUBLE_EQ(0.0, lpf.filter(0.0)); double value; for (size_t i = 0; i < 100; ++i) @@ -58,9 +58,9 @@ TEST(MOVEIT_SERVO, FilterConverge) EXPECT_NE(5.0, lpf.filter(100.0)); } -TEST(MOVEIT_SERVO, FilterReset) +TEST(SMOOTHING_PLUGINS, FilterReset) { - moveit_servo::LowPassFilter lpf(2.0); + online_signal_smoothing::ButterworthFilter lpf(2.0); EXPECT_DOUBLE_EQ(0.0, lpf.filter(0.0)); lpf.reset(5.0); double value = lpf.filter(5.0); diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 61473c54024..ea42f81eff2 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -70,7 +70,5 @@ ament_cmake - - diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index e80411247e4..fd5258a453c 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -31,34 +31,39 @@ find_package(moveit_common REQUIRED) moveit_package() find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) +find_package(control_toolbox REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(control_msgs REQUIRED) -find_package(control_toolbox REQUIRED) -find_package(moveit_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(tf2_eigen REQUIRED) find_package(trajectory_msgs REQUIRED) -find_package(moveit_ros_planning REQUIRED) + find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + control_toolbox + geometry_msgs + moveit_core + moveit_msgs + moveit_ros_planning + pluginlib rclcpp rclcpp_components + sensor_msgs std_msgs std_srvs - sensor_msgs - control_msgs - control_toolbox - moveit_msgs - geometry_msgs tf2_eigen trajectory_msgs - moveit_ros_planning ) include_directories( @@ -79,7 +84,6 @@ add_library(${SERVO_LIB_NAME} SHARED # These files are used to produce differential motion src/collision_check.cpp src/enforce_limits.cpp - src/low_pass_filter.cpp src/servo.cpp src/servo_calcs.cpp ) @@ -187,10 +191,6 @@ if(BUILD_TESTING) # Run all lint tests in package.xml except those listed above ament_lint_auto_find_test_dependencies() - # Lowpass filter unit test - ament_add_gtest(test_low_pass_filter test/test_low_pass_filter.cpp) - target_link_libraries(test_low_pass_filter ${SERVO_LIB_NAME}) - # Unit test for ServoCalcs # TODO (vatanaksoytezer): Rewrite this similar to enforce limits test # ament_add_gtest_executable( diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index 64ec20683d3..f7763fc536d 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -22,11 +22,11 @@ command_out_type: trajectory_msgs/JointTrajectory # What to publish? Can save some bandwidth as most robots only require positions or velocities publish_joint_positions: true -publish_joint_velocities: false +publish_joint_velocities: true publish_joint_accelerations: false -## Incoming Joint State properties -low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the measurements less. +## Plugins for smoothing outgoing commands +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" ## MoveIt properties move_group_name: panda_arm # Often 'manipulator' or 'arm' diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml index 652a13c298d..a82a1bc3748 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml @@ -24,8 +24,8 @@ publish_joint_positions: true publish_joint_velocities: true publish_joint_accelerations: false -## Incoming Joint State properties -low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the measurements less. +## Plugins for smoothing outgoing commands +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" ## MoveIt properties move_group_name: panda_arm # Often 'manipulator' or 'arm' diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index 25e2eddc79b..48eb6dd7d0b 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -48,7 +48,6 @@ #include #include -#include namespace moveit_servo { diff --git a/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h deleted file mode 100644 index 8682bde86c7..00000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h +++ /dev/null @@ -1,69 +0,0 @@ -/******************************************************************************* - * Title : low_pass_filter.h - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Andy Zelenak - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 the 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 HOLDER 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. - *******************************************************************************/ - -#pragma once - -#include - -namespace moveit_servo -{ -/** - * Class LowPassFilter - Filter a signal to soften jerks. - * This is a first-order Butterworth low-pass filter. - * - * TODO: Use ROS filters package (http://wiki.ros.org/filters, https://github.com/ros/filters) - */ -class LowPassFilter -{ -public: - // Larger filter_coeff-> more smoothing of servo commands, but more lag. - // Rough plot, with cutoff frequency on the y-axis: - // https://www.wolframalpha.com/input/?i=plot+arccot(c) - explicit LowPassFilter(double low_pass_filter_coeff); - double filter(double new_measurement); - void reset(double data); - -private: - static constexpr std::size_t FILTER_LENGTH = 2; - double previous_measurements_[FILTER_LENGTH]; - double previous_filtered_measurement_; - // Scale and feedback term are calculated from supplied filter coefficient - double scale_term_; - double feedback_term_; -}; -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h index d5b72b4f7d8..9f89ade5fef 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -45,13 +45,14 @@ #include // ROS -#include #include #include #include #include #include #include +#include +#include #include #include #include @@ -67,7 +68,7 @@ // moveit_servo #include #include -#include +#include namespace moveit_servo { @@ -155,10 +156,13 @@ class ServoCalcs */ Eigen::VectorXd scaleJointCommand(const control_msgs::msg::JointJog& command); + /** \brief Come to a halt in a smooth way. Apply a smoothing plugin, if one is configured. + */ + void filteredHalt(trajectory_msgs::msg::JointTrajectory& joint_trajectory); + /** \brief Suddenly halt for a joint limit or other critical issue. * Is handled differently for position vs. velocity control. */ - void suddenHalt(trajectory_msgs::msg::JointTrajectory& joint_trajectory) const; void suddenHalt(sensor_msgs::msg::JointState& joint_state, const std::vector& joints_to_halt) const; @@ -191,7 +195,7 @@ class ServoCalcs const ServoType servo_type); /** \brief Joint-wise update of a sensor_msgs::msg::JointState with given delta's - * Also calculates the previous velocity + * Also filters and calculates the previous velocity * @param delta_theta Eigen vector of joint delta's * @param joint_state The joint state msg being updated * @param previous_vel Eigen vector of previous velocities being updated @@ -296,7 +300,8 @@ class ServoCalcs sensor_msgs::msg::JointState internal_joint_state_, original_joint_state_; std::map joint_state_name_map_; - std::vector position_filters_; + // Smoothing algorithm (loads a plugin) + std::shared_ptr smoother_; trajectory_msgs::msg::JointTrajectory::SharedPtr last_sent_command_; @@ -317,6 +322,7 @@ class ServoCalcs // Main tracking / result publisher loop std::thread thread_; bool stop_requested_; + std::atomic done_stopping_; // Status StatusCode status_ = StatusCode::NO_WARNING; @@ -348,8 +354,8 @@ class ServoCalcs control_msgs::msg::JointJog::ConstSharedPtr latest_joint_cmd_; rclcpp::Time latest_twist_command_stamp_ = rclcpp::Time(0., RCL_ROS_TIME); rclcpp::Time latest_joint_command_stamp_ = rclcpp::Time(0., RCL_ROS_TIME); - bool latest_nonzero_twist_stamped_ = false; - bool latest_nonzero_joint_cmd_ = false; + bool latest_twist_cmd_is_nonzero_ = false; + bool latest_joint_cmd_is_nonzero_ = false; // input condition variable used for low latency mode std::condition_variable input_cv_; @@ -358,5 +364,8 @@ class ServoCalcs // dynamic parameters std::string robot_link_command_frame_; rcl_interfaces::msg::SetParametersResult robotLinkCommandFrameCallback(const rclcpp::Parameter& parameter); + + // Load a smoothing plugin + pluginlib::ClassLoader smoothing_loader_; }; } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h index 072ba9b74f2..307d1431dc8 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h @@ -76,9 +76,9 @@ struct ServoParameters bool publish_joint_positions; bool publish_joint_velocities; bool publish_joint_accelerations; - // Incoming Joint State properties + // Plugins for smoothing outgoing commands std::string joint_topic; - double low_pass_filter_coeff; + std::string smoothing_filter_plugin_name; // MoveIt properties std::string move_group_name; std::string planning_frame; diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index ff15ed32392..c3e2f9ff3fd 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -26,7 +26,9 @@ control_toolbox geometry_msgs moveit_msgs + moveit_core moveit_ros_planning_interface + pluginlib sensor_msgs std_msgs std_srvs @@ -37,12 +39,12 @@ joint_state_broadcaster joint_trajectory_controller joy - tf2_ros robot_state_publisher + tf2_ros + ament_cmake_gtest moveit_resources_panda_moveit_config ros_testing - ament_cmake_gtest ament_lint_auto ament_lint_common diff --git a/moveit_ros/moveit_servo/src/low_pass_filter.cpp b/moveit_ros/moveit_servo/src/low_pass_filter.cpp deleted file mode 100644 index a24733b141b..00000000000 --- a/moveit_ros/moveit_servo/src/low_pass_filter.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 the 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 HOLDER 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. - *******************************************************************************/ - -/* Title : low_pass_filter.cpp - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Andy Zelenak - */ - -#include -#include -#include -#include - -namespace moveit_servo -{ -namespace -{ -constexpr double EPSILON = 1e-9; -} - -LowPassFilter::LowPassFilter(double low_pass_filter_coeff) - : previous_measurements_{ 0., 0. } - , previous_filtered_measurement_(0.) - , scale_term_(1. / (1. + low_pass_filter_coeff)) - , feedback_term_(1. - low_pass_filter_coeff) -{ - // guarantee this doesn't change because the logic below depends on this length implicity - static_assert(LowPassFilter::FILTER_LENGTH == 2, "moveit_servo::LowPassFilter::FILTER_LENGTH should be 2"); - - // Make sure input values are ok - if (std::isinf(feedback_term_)) - throw std::length_error("moveit_servo::LowPassFilter: infinite feedback_term_"); - - if (std::isinf(scale_term_)) - throw std::length_error("moveit_servo::LowPassFilter: infinite scale_term_"); - - if (low_pass_filter_coeff < 1) - throw std::length_error("moveit_servo::LowPassFilter: Filter coefficient < 1. makes the lowpass filter unstable"); - - if (std::abs(feedback_term_) < EPSILON) - throw std::length_error("moveit_servo::LowPassFilter: Filter coefficient value resulted in feedback term of 0"); -} - -void LowPassFilter::reset(double data) -{ - previous_measurements_[0] = data; - previous_measurements_[1] = data; - - previous_filtered_measurement_ = data; -} - -double LowPassFilter::filter(double new_measurement) -{ - // Push in the new measurement - previous_measurements_[1] = previous_measurements_[0]; - previous_measurements_[0] = new_measurement; - - double new_filtered_measurement = scale_term_ * (previous_measurements_[1] + previous_measurements_[0] - - feedback_term_ * previous_filtered_measurement_); - - // Store the new filtered measurement - previous_filtered_measurement_ = new_filtered_measurement; - - return new_filtered_measurement; -} -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 6b11ad295a5..d214418d81f 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -52,6 +52,7 @@ using namespace std::chrono_literals; // for s, ms, etc. static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_calcs"); constexpr auto ROS_LOG_THROTTLE_PERIOD = std::chrono::milliseconds(3000).count(); +static constexpr double STOPPED_VELOCITY_EPS = 1e-4; // rad/s namespace moveit_servo { @@ -96,8 +97,10 @@ ServoCalcs::ServoCalcs(rclcpp::Node::SharedPtr node, , parameters_(parameters) , planning_scene_monitor_(planning_scene_monitor) , stop_requested_(true) + , done_stopping_(false) , paused_(false) , robot_link_command_frame_(parameters->robot_link_command_frame) + , smoothing_loader_("moveit_core", "online_signal_smoothing::SmoothingBaseClass") { // Register callback for changes in robot_link_command_frame parameters_->registerSetParameterCallback(parameters->ns + ".robot_link_command_frame", @@ -172,9 +175,44 @@ ServoCalcs::ServoCalcs(rclcpp::Node::SharedPtr node, { // A map for the indices of incoming joint commands joint_state_name_map_[internal_joint_state_.name[i]] = i; + } + + // Load the smoothing plugin + try + { + smoother_ = smoothing_loader_.createSharedInstance(parameters_->smoothing_filter_plugin_name); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while loading the smoothing plugin '%s': '%s'", + parameters_->smoothing_filter_plugin_name.c_str(), ex.what()); + std::exit(EXIT_FAILURE); + } + + // Initialize the smoothing plugin + if (!smoother_->initialize(node_, planning_scene_monitor_->getRobotModel(), num_joints_)) + { + RCLCPP_ERROR(LOGGER, "Smoothing plugin could not be initialized"); + std::exit(EXIT_FAILURE); + } + + // Load the smoothing plugin + try + { + smoother_ = smoothing_loader_.createSharedInstance(parameters_->smoothing_filter_plugin_name); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while loading the smoothing plugin '%s': '%s'", + parameters_->smoothing_filter_plugin_name.c_str(), ex.what()); + std::exit(EXIT_FAILURE); + } - // Low-pass filters for the joint positions - position_filters_.emplace_back(parameters_->low_pass_filter_coeff); + // Initialize the smoothing plugin + if (!smoother_->initialize(node_, planning_scene_monitor_->getRobotModel(), num_joints_)) + { + RCLCPP_ERROR(LOGGER, "Smoothing plugin could not be initialized"); + std::exit(EXIT_FAILURE); } // A matrix of all zeros is used to check whether matrices have been initialized @@ -321,8 +359,8 @@ void ServoCalcs::calculateSingleIteration() joint_command_is_stale_ = ((node_->now() - latest_joint_command_stamp_) >= rclcpp::Duration::from_seconds(parameters_->incoming_command_timeout)); - have_nonzero_twist_stamped_ = latest_nonzero_twist_stamped_; - have_nonzero_joint_command_ = latest_nonzero_joint_cmd_; + have_nonzero_twist_stamped_ = latest_twist_cmd_is_nonzero_; + have_nonzero_joint_command_ = latest_joint_cmd_is_nonzero_; // Get the transform from MoveIt planning frame to servoing command frame // Calculate this transform to ensure it is available via C++ API @@ -389,16 +427,18 @@ void ServoCalcs::calculateSingleIteration() } // If we should halt - if (!have_nonzero_command_) + if (!have_nonzero_command_ && !done_stopping_) { - suddenHalt(*joint_trajectory); - have_nonzero_twist_stamped_ = false; - have_nonzero_joint_command_ = false; + filteredHalt(*joint_trajectory); + } + else + { + done_stopping_ = false; } // Skip the servoing publication if all inputs have been zero for several cycles in a row. // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. - if (!have_nonzero_command_ && (parameters_->num_outgoing_halt_msgs_to_publish != 0) && + if (!have_nonzero_command_ && done_stopping_ && (parameters_->num_outgoing_halt_msgs_to_publish != 0) && (zero_velocity_count_ > parameters_->num_outgoing_halt_msgs_to_publish)) { ok_to_publish_ = false; @@ -420,7 +460,7 @@ void ServoCalcs::calculateSingleIteration() // Store last zero-velocity message flag to prevent superfluous warnings. // Cartesian and joint commands must both be zero. - if (!have_nonzero_command_) + if (!have_nonzero_command_ && done_stopping_) { // Avoid overflow if (zero_velocity_count_ < std::numeric_limits::max()) @@ -433,6 +473,21 @@ void ServoCalcs::calculateSingleIteration() if (ok_to_publish_ && !paused_) { + // Clear out position commands if user did not request them (can cause interpolation issues) + if (!parameters_->publish_joint_positions) + { + joint_trajectory->points[0].positions.clear(); + } + // Likewise for velocity and acceleration + if (!parameters_->publish_joint_velocities) + { + joint_trajectory->points[0].velocities.clear(); + } + if (!parameters_->publish_joint_accelerations) + { + joint_trajectory->points[0].accelerations.clear(); + } + // Put the outgoing msg in the right format // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). if (parameters_->command_out_type == "trajectory_msgs/JointTrajectory") @@ -635,16 +690,20 @@ bool ServoCalcs::applyJointUpdate(const Eigen::ArrayXd& delta_theta, sensor_msgs { // Increment joint joint_state.position[i] += delta_theta[i]; + } - // Lowpass filter position - joint_state.position[i] = position_filters_[i].filter(joint_state.position[i]); + smoother_->doSmoothing(joint_state.position); + for (std::size_t i = 0; i < joint_state.position.size(); ++i) + { // Calculate joint velocity - joint_state.velocity[i] = delta_theta[i] / parameters_->publish_period; + joint_state.velocity[i] = + (joint_state.position.at(i) - original_joint_state_.position.at(i)) / parameters_->publish_period; // Save this velocity for future accel calculations previous_vel[i] = joint_state.velocity[i]; } + return true; } @@ -668,11 +727,7 @@ void ServoCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::msg::Joint void ServoCalcs::resetLowPassFilters(const sensor_msgs::msg::JointState& joint_state) { - for (std::size_t i = 0; i < position_filters_.size(); ++i) - { - position_filters_[i].reset(joint_state.position[i]); - } - + smoother_->reset(joint_state.position); updated_filters_ = true; } @@ -829,40 +884,55 @@ ServoCalcs::enforcePositionLimits(sensor_msgs::msg::JointState& joint_state) con return joints_to_halt; } -// Suddenly halt for a joint limit or other critical issue. -// Is handled differently for position vs. velocity control. -void ServoCalcs::suddenHalt(trajectory_msgs::msg::JointTrajectory& joint_trajectory) const +void ServoCalcs::filteredHalt(trajectory_msgs::msg::JointTrajectory& joint_trajectory) { // Prepare the joint trajectory message to stop the robot joint_trajectory.points.clear(); joint_trajectory.points.emplace_back(); - trajectory_msgs::msg::JointTrajectoryPoint& point = joint_trajectory.points.front(); - - // When sending out trajectory_msgs/JointTrajectory type messages, the "trajectory" is just a single point. - // That point cannot have the same timestamp as the start of trajectory execution since that would mean the - // arm has to reach the first trajectory point the moment execution begins. To prevent errors about points - // being 0 seconds in the past, the smallest supported timestep is added as time from start to the trajectory point. - point.time_from_start = rclcpp::Duration(0, 1); - if (parameters_->publish_joint_positions) - point.positions.resize(num_joints_); - if (parameters_->publish_joint_velocities) - point.velocities.resize(num_joints_); - - // Assert the following loop is safe to execute + // Deceleration algorithm: + // Set positions to original_joint_state_ + // Filter + // Calculate velocities + // Check if velocities are close to zero. Round to zero, if so. + // Set done_stopping_ flag assert(original_joint_state_.position.size() >= num_joints_); - - // Set the positions and velocities vectors - for (std::size_t i = 0; i < num_joints_; ++i) + joint_trajectory.points[0].positions = original_joint_state_.position; + smoother_->doSmoothing(joint_trajectory.points[0].positions); + done_stopping_ = true; + if (parameters_->publish_joint_velocities) { - // For position-controlled robots, can reset the joints to a known, good state - if (parameters_->publish_joint_positions) - point.positions[i] = original_joint_state_.position[i]; + joint_trajectory.points[0].velocities = std::vector(num_joints_, 0); + for (std::size_t i = 0; i < num_joints_; ++i) + { + joint_trajectory.points[0].velocities.at(i) = + (joint_trajectory.points[0].positions.at(i) - original_joint_state_.position.at(i)) / + parameters_->publish_period; + // If velocity is very close to zero, round to zero + if (joint_trajectory.points[0].velocities.at(i) > STOPPED_VELOCITY_EPS) + { + done_stopping_ = false; + } + } + // If every joint is very close to stopped, round velocity to zero + if (done_stopping_) + { + std::fill(joint_trajectory.points[0].velocities.begin(), joint_trajectory.points[0].velocities.end(), 0); + } + } - // For velocity-controlled robots, stop - if (parameters_->publish_joint_velocities) - point.velocities[i] = 0; + if (parameters_->publish_joint_accelerations) + { + joint_trajectory.points[0].accelerations = std::vector(num_joints_, 0); + for (std::size_t i = 0; i < num_joints_; ++i) + { + joint_trajectory.points[0].accelerations.at(i) = + (joint_trajectory.points[0].velocities.at(i) - original_joint_state_.velocity.at(i)) / + parameters_->publish_period; + } } + + joint_trajectory.points[0].time_from_start = rclcpp::Duration::from_seconds(parameters_->publish_period); } void ServoCalcs::suddenHalt(sensor_msgs::msg::JointState& joint_state, @@ -881,7 +951,6 @@ void ServoCalcs::suddenHalt(sensor_msgs::msg::JointState& joint_state, } } -// Parse the incoming joint msg for the joints of our MoveGroup void ServoCalcs::updateJoints() { // Get the latest joint group positions @@ -1103,7 +1172,7 @@ void ServoCalcs::twistStampedCB(const geometry_msgs::msg::TwistStamped::SharedPt { const std::lock_guard lock(main_loop_mutex_); latest_twist_stamped_ = msg; - latest_nonzero_twist_stamped_ = isNonZero(*latest_twist_stamped_.get()); + latest_twist_cmd_is_nonzero_ = isNonZero(*latest_twist_stamped_.get()); if (msg.get()->header.stamp != rclcpp::Time(0.)) latest_twist_command_stamp_ = msg.get()->header.stamp; @@ -1117,7 +1186,7 @@ void ServoCalcs::jointCmdCB(const control_msgs::msg::JointJog::SharedPtr msg) { const std::lock_guard lock(main_loop_mutex_); latest_joint_cmd_ = msg; - latest_nonzero_joint_cmd_ = isNonZero(*latest_joint_cmd_.get()); + latest_joint_cmd_is_nonzero_ = isNonZero(*latest_joint_cmd_.get()); if (msg.get()->header.stamp != rclcpp::Time(0.)) latest_joint_command_stamp_ = msg.get()->header.stamp; diff --git a/moveit_ros/moveit_servo/src/servo_parameters.cpp b/moveit_ros/moveit_servo/src/servo_parameters.cpp index 7b1271f0c14..e9034fdd2bf 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.cpp +++ b/moveit_ros/moveit_servo/src/servo_parameters.cpp @@ -108,7 +108,8 @@ ServoParameters::SharedConstPtr ServoParameters::makeServoParameters(const rclcp // Incoming Joint State properties declareOrGetParam(parameters->joint_topic, ns + ".joint_topic", node, logger); - declareOrGetParam(parameters->low_pass_filter_coeff, ns + ".low_pass_filter_coeff", node, logger); + declareOrGetParam(parameters->smoothing_filter_plugin_name, ns + ".smoothing_filter_plugin_name", node, + logger); // MoveIt properties declareOrGetParam(parameters->move_group_name, ns + ".move_group_name", node, logger); @@ -164,10 +165,9 @@ ServoParameters::SharedConstPtr ServoParameters::makeServoParameters(const rclcp "greater than zero. Check yaml file."); return nullptr; } - if (parameters->low_pass_filter_coeff <= 0.) + if (parameters->smoothing_filter_plugin_name.empty()) { - RCLCPP_WARN(logger, "Parameter 'low_pass_filter_coeff' should be " - "greater than zero. Check yaml file."); + RCLCPP_WARN(logger, "A smoothing plugin is required."); return nullptr; } if (parameters->joint_limit_margin < 0.) diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml index fef26f046bf..d84c18b7437 100644 --- a/moveit_ros/moveit_servo/test/config/servo_settings.yaml +++ b/moveit_ros/moveit_servo/test/config/servo_settings.yaml @@ -11,7 +11,6 @@ scale: rotational: 0.006 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. joint: 0.01 -low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the measurements less. ## Properties of outgoing commands low_latency_mode: false # Set this to true to tie the output rate to the input rate @@ -27,6 +26,9 @@ publish_joint_positions: true publish_joint_velocities: false publish_joint_accelerations: false +## Plugins for smoothing outgoing commands +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + ## MoveIt properties move_group_name: panda_arm # Often 'manipulator' or 'arm' planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' diff --git a/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml b/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml index 4342c9e29cc..14e006fd5c9 100644 --- a/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml +++ b/moveit_ros/moveit_servo/test/config/servo_settings_low_latency.yaml @@ -11,7 +11,6 @@ scale: rotational: 0.006 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. joint: 0.01 -low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the measurements less. ## Properties of outgoing commands low_latency_mode: true # Set this to true to tie the output rate to the input rate @@ -27,6 +26,9 @@ publish_joint_positions: true publish_joint_velocities: false publish_joint_accelerations: false +## Plugins for smoothing outgoing commands +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + ## MoveIt properties move_group_name: panda_arm # Often 'manipulator' or 'arm' planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' From e030a42ac819262b9bf033f4352346dc2e306313 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Oct 2021 21:20:01 +0000 Subject: [PATCH 04/15] Remove default constructor of ButterworthFilter --- .../moveit/online_signal_smoothing/butterworth_filter.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h index e0bf59efe98..95632245043 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h @@ -61,6 +61,7 @@ class ButterworthFilter * https://www.wolframalpha.com/input/?i=plot+arccot(c) */ ButterworthFilter(double low_pass_filter_coeff); + ButterworthFilter() = delete; double filter(double new_measurement); @@ -79,8 +80,6 @@ class ButterworthFilter class ButterworthFilterPlugin : public SmoothingBaseClass { public: - ButterworthFilterPlugin(){}; - /** * Initialize the smoothing algorithm * @param node ROS node, used for parameter retrieval From 6f8c7f7fd608ff4cd56b9300923505509461698b Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Oct 2021 21:22:03 +0000 Subject: [PATCH 05/15] Fix SmoothingBaseClass constructor/destructor * Make default destructor virtual * Forward declare RobotModel dependency * CMakeLists.txt touch-ups --- moveit_core/CMakeLists.txt | 2 +- .../online_signal_smoothing/CMakeLists.txt | 38 ++++++++++--------- .../smoothing_base_class.h | 17 +++++++-- .../src/smoothing_base_class.cpp | 6 +++ 4 files changed, 42 insertions(+), 21 deletions(-) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index dac0a6481e8..ceb8b528650 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -110,7 +110,7 @@ set(THIS_PACKAGE_LIBRARIES moveit_robot_model moveit_robot_state moveit_robot_trajectory - moveit_smoothing_base_class + moveit_smoothing_base moveit_test_utils moveit_trajectory_processing moveit_transforms diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/moveit_core/online_signal_smoothing/CMakeLists.txt index 0025fd24532..6cfdfbbd1b2 100644 --- a/moveit_core/online_signal_smoothing/CMakeLists.txt +++ b/moveit_core/online_signal_smoothing/CMakeLists.txt @@ -1,41 +1,45 @@ # Base class - -add_library(moveit_smoothing_base_class SHARED +set(SMOOTHING_BASE_LIB moveit_smoothing_base) +add_library(${SMOOTHING_BASE_LIB} SHARED src/smoothing_base_class.cpp ) include(GenerateExportHeader) -generate_export_header(moveit_smoothing_base_class) -target_include_directories(moveit_smoothing_base_class PUBLIC +generate_export_header(${SMOOTHING_BASE_LIB}) +target_include_directories(${SMOOTHING_BASE_LIB} PUBLIC $ ) -set_target_properties(moveit_smoothing_base_class PROPERTIES VERSION +set_target_properties(${SMOOTHING_BASE_LIB} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}" ) -target_link_libraries(moveit_smoothing_base_class - moveit_robot_model +ament_target_dependencies(${SMOOTHING_BASE_LIB} + rclcpp ) # Plugin implementations - -add_library(moveit_butterworth_filter SHARED +set(BUTTERWORTH_FILTER_LIB moveit_butterworth_filter) +add_library(${BUTTERWORTH_FILTER_LIB} SHARED src/butterworth_filter.cpp ) -include(GenerateExportHeader) -generate_export_header(moveit_butterworth_filter) -target_include_directories(moveit_butterworth_filter PUBLIC +generate_export_header(${BUTTERWORTH_FILTER_LIB}) +target_include_directories(${BUTTERWORTH_FILTER_LIB} PUBLIC $ ) -set_target_properties(moveit_butterworth_filter PROPERTIES VERSION +set_target_properties(${BUTTERWORTH_FILTER_LIB} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}" ) -target_link_libraries(moveit_butterworth_filter - moveit_smoothing_base_class +target_link_libraries(${BUTTERWORTH_FILTER_LIB} + ${SMOOTHING_BASE_LIB} + moveit_robot_model +) +ament_target_dependencies(${BUTTERWORTH_FILTER_LIB} + srdfdom # include dependency from moveit_robot_model ) # Installation install(DIRECTORY include/ DESTINATION include) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_butterworth_filter_export.h DESTINATION include) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${SMOOTHING_BASE_LIB}_export.h DESTINATION include) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${BUTTERWORTH_FILTER_LIB}_export.h DESTINATION include) # Testing @@ -46,5 +50,5 @@ if(BUILD_TESTING) # Lowpass filter unit test ament_add_gtest(test_butterworth_filter test/test_butterworth_filter.cpp) - target_link_libraries(test_butterworth_filter moveit_butterworth_filter) + target_link_libraries(test_butterworth_filter ${BUTTERWORTH_FILTER_LIB}) endif() diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h index 8c8ff08603d..4621849be72 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h @@ -38,15 +38,26 @@ #pragma once -#include +#include "rclcpp/rclcpp.hpp" +#include + +#include "moveit_smoothing_base_export.h" + +namespace moveit +{ +namespace core +{ +MOVEIT_CLASS_FORWARD(RobotModel); +} // namespace core +} // namespace moveit namespace online_signal_smoothing { class SmoothingBaseClass { public: - SmoothingBaseClass() = default; - ~SmoothingBaseClass() = default; + SmoothingBaseClass(); + virtual ~SmoothingBaseClass(); /** * Initialize the smoothing algorithm diff --git a/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp b/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp index e6b82f56b7b..7b7b879fd86 100644 --- a/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp +++ b/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp @@ -1 +1,7 @@ #include + +namespace online_signal_smoothing +{ +SmoothingBaseClass::SmoothingBaseClass() = default; +SmoothingBaseClass::~SmoothingBaseClass() = default; +} // namespace online_signal_smoothing From 0649f4f2f4492d04b42a2e0e317dda98d209408c Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Oct 2021 21:29:56 +0000 Subject: [PATCH 06/15] Add missing license headers --- .../src/butterworth_filter.cpp | 38 +++++++++++++++++++ .../src/smoothing_base_class.cpp | 38 +++++++++++++++++++ 2 files changed, 76 insertions(+) diff --git a/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp b/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp index 1e3dddf02d6..ca3d2b45e2f 100644 --- a/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp +++ b/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp @@ -1,3 +1,41 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik 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 PickNik Inc. 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. + *********************************************************************/ + +/* Author: Andy Zelenak + Description: A first-order Butterworth low-pass filter. There is only one parameter to tune. + */ + #include namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp b/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp index 7b7b879fd86..f39984f3d42 100644 --- a/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp +++ b/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp @@ -1,3 +1,41 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik 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 PickNik Inc. 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. + *********************************************************************/ + +/* Author: Andy Zelenak + Description: Base class implementation for online smoothing plugins + */ + #include namespace online_signal_smoothing From c81e9eff34fdb99387ff38840fd2c465ab6150e4 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Oct 2021 21:32:41 +0000 Subject: [PATCH 07/15] Run Servo tests with standalone Servo node --- .../test/launch/servo_launch_test_common.py | 53 ++++++++++++------- 1 file changed, 33 insertions(+), 20 deletions(-) diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py index d312e67abe7..65331d2e1aa 100644 --- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py +++ b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py @@ -131,27 +131,40 @@ def generate_servo_test_description( output="screen", ) - servo_container = ComposableNodeContainer( - name="servo_container", - namespace="/", - package="rclcpp_components", - executable="component_container_mt", - composable_node_descriptions=[ - ComposableNode( - package="moveit_servo", - plugin="moveit_servo::ServoNode", - name="servo_node", - parameters=[ - servo_params, - robot_description, - robot_description_semantic, - joint_limits_yaml, - ], - ), - ], + servo_node = Node( + package="moveit_servo", + executable="servo_node_main", output="screen", + parameters=[ + servo_params, + robot_description, + robot_description_semantic, + joint_limits_yaml, + ], ) + # NOTE: The smoothing plugin doesn't seem to be accessible from containers + # servo_container = ComposableNodeContainer( + # name="servo_container", + # namespace="/", + # package="rclcpp_components", + # executable="component_container_mt", + # composable_node_descriptions=[ + # ComposableNode( + # package="moveit_servo", + # plugin="moveit_servo::ServoNode", + # name="servo_node", + # parameters=[ + # servo_params, + # robot_description, + # robot_description_semantic, + # joint_limits_yaml, + # ], + # ), + # ], + # output="screen", + # ) + # Unknown how to set timeout # https://github.com/ros2/launch/issues/466 servo_gtest = launch_ros.actions.Node( @@ -170,14 +183,14 @@ def generate_servo_test_description( "containing test executables", ), ros2_control_node, - servo_container, + servo_node, test_container, TimerAction(period=2.0, actions=[servo_gtest]), launch_testing.actions.ReadyToTest(), ] + load_controllers ), { - "servo_container": servo_container, + "servo_node": servo_node, "test_container": test_container, "servo_gtest": servo_gtest, "ros2_control_node": ros2_control_node, From 89fcb040922b0a3d21c2b7b453c69124c5646344 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Wed, 20 Oct 2021 21:34:40 +0000 Subject: [PATCH 08/15] Remove run dependency to moveit2_tutorials --- .../moveit_servo/config/demo_rviz_config.rviz | 261 ++++++++++++++++++ .../launch/servo_example.launch.py | 3 +- 2 files changed, 262 insertions(+), 2 deletions(-) create mode 100644 moveit_ros/moveit_servo/config/demo_rviz_config.rviz diff --git a/moveit_ros/moveit_servo/config/demo_rviz_config.rviz b/moveit_ros/moveit_servo/config/demo_rviz_config.rviz new file mode 100644 index 00000000000..e44c6604e8a --- /dev/null +++ b/moveit_ros/moveit_servo/config/demo_rviz_config.rviz @@ -0,0 +1,261 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 628 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /moveit_servo/publish_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + panda_hand: + Value: true + panda_leftfinger: + Value: false + panda_link0: + Value: false + panda_link1: + Value: false + panda_link2: + Value: false + panda_link3: + Value: false + panda_link4: + Value: false + panda_link5: + Value: false + panda_link6: + Value: false + panda_link7: + Value: false + panda_link8: + Value: false + panda_rightfinger: + Value: false + world: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + panda_hand: + panda_leftfinger: + {} + panda_rightfinger: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.155569553375244 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.08608254045248032 + Y: -0.20677587389945984 + Z: 0.3424459993839264 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4603978991508484 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.8753982782363892 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 857 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000216000002fffc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ff000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f000002d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000416000002ff00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1586 + X: 1179 + Y: 393 diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/servo_example.launch.py index f9bf5ad4e7f..502d6bc9d0b 100644 --- a/moveit_ros/moveit_servo/launch/servo_example.launch.py +++ b/moveit_ros/moveit_servo/launch/servo_example.launch.py @@ -55,8 +55,7 @@ def generate_launch_description(): # RViz rviz_config_file = ( - get_package_share_directory("moveit2_tutorials") - + "/config/demo_rviz_config.rviz" + get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz" ) rviz_node = Node( package="rviz2", From 5a27d2fac6da1b94ca5b536fe6c766615ffbd615 Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Thu, 21 Oct 2021 18:07:58 +0200 Subject: [PATCH 09/15] Bump ros2_control version to 1.0.0 for all distros (#711) --- moveit2.repos | 8 ++++++++ moveit2_galactic.repos | 10 ---------- 2 files changed, 8 insertions(+), 10 deletions(-) delete mode 100644 moveit2_galactic.repos diff --git a/moveit2.repos b/moveit2.repos index 6c9272be900..a3b8f4fa895 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -15,3 +15,11 @@ repositories: type: git url: https://github.com/ros-planning/warehouse_ros_mongo version: ros2 + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control + version: 1.0.0 + ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers + version: 1.0.0 diff --git a/moveit2_galactic.repos b/moveit2_galactic.repos deleted file mode 100644 index bc699350bed..00000000000 --- a/moveit2_galactic.repos +++ /dev/null @@ -1,10 +0,0 @@ -# TODO(henningkayser): Remove once ros2_control is released (https://github.com/ros-controls/ros2_control/issues/421) -repositories: - ros2_control: - type: git - url: https://github.com/ros-controls/ros2_control - version: 0.8.0 - ros2_controllers: - type: git - url: https://github.com/ros-controls/ros2_controllers - version: 0.5.0 From df6dc7c340afc9e79c15f3b92a93db936a561ba9 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 21 Oct 2021 13:19:04 -0500 Subject: [PATCH 10/15] Find/replace deprecated spawner.py (#737) --- .../run_move_group/launch/run_move_group.launch.py | 2 +- .../run_moveit_cpp/launch/run_moveit_cpp.launch.py | 2 +- .../launch/run_move_group.launch.py | 2 +- moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py | 2 +- moveit_ros/moveit_servo/launch/servo_example.launch.py | 2 +- moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py | 2 +- .../moveit_servo/test/launch/test_servo_pose_tracking.test.py | 2 +- .../test/launch/move_group_launch_test_common.py | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/moveit_demo_nodes/run_move_group/launch/run_move_group.launch.py b/moveit_demo_nodes/run_move_group/launch/run_move_group.launch.py index 75370aee156..bf4efd69549 100644 --- a/moveit_demo_nodes/run_move_group/launch/run_move_group.launch.py +++ b/moveit_demo_nodes/run_move_group/launch/run_move_group.launch.py @@ -175,7 +175,7 @@ def generate_launch_description(): ]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="screen", ) diff --git a/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp.launch.py b/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp.launch.py index 41d6908f255..1626bd5fef5 100644 --- a/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp.launch.py +++ b/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp.launch.py @@ -159,7 +159,7 @@ def generate_launch_description(): ]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="screen", ) diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_move_group.launch.py b/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_move_group.launch.py index f4cc0a1317e..0eb1fcfc9ef 100644 --- a/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_move_group.launch.py +++ b/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_move_group.launch.py @@ -171,7 +171,7 @@ def generate_launch_description(): for controller in ["panda_arm_controller", "joint_state_broadcaster"]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="screen", ) diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py index b2ae473efc1..fc7c1676d90 100644 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py @@ -139,7 +139,7 @@ def generate_launch_description(): for controller in ["panda_arm_controller", "joint_state_broadcaster"]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="screen", ) diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/servo_example.launch.py index 502d6bc9d0b..640412e0ce8 100644 --- a/moveit_ros/moveit_servo/launch/servo_example.launch.py +++ b/moveit_ros/moveit_servo/launch/servo_example.launch.py @@ -87,7 +87,7 @@ def generate_launch_description(): for controller in ["panda_arm_controller", "joint_state_broadcaster"]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="screen", ) diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py index 65331d2e1aa..28cb13baf0e 100644 --- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py +++ b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py @@ -102,7 +102,7 @@ def generate_servo_test_description( for controller in ["panda_arm_controller", "joint_state_broadcaster"]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="screen", ) diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py index 906ee12d7a0..a564a3038e6 100644 --- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py +++ b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py @@ -97,7 +97,7 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): for controller in ["panda_arm_controller", "joint_state_broadcaster"]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="screen", ) diff --git a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py index 6baaec27a54..8613e2a3b86 100644 --- a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py +++ b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py @@ -170,7 +170,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp for controller in ["panda_arm_controller", "joint_state_broadcaster"]: load_controllers += [ ExecuteProcess( - cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + cmd=["ros2 run controller_manager spawner {}".format(controller)], shell=True, output="screen", ) From e3d4768541602fca900dacff50aac6374853472d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 26 Oct 2021 09:09:57 -0500 Subject: [PATCH 11/15] moveit_ros_control_interface: Small comment cleanup (#754) --- .../src/controller_manager_plugin.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp index aa87242cf86..30195241a35 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp @@ -413,7 +413,7 @@ class MoveItMultiControllerManager : public moveit_controller_manager::MoveItCon node_ = node; } /** - * \brief Poll ROS master for services and filters all controller_manager/list_controllers instances + * \brief Poll for services and filter all controller_manager/list_controllers instances * Throttled down to 1 Hz, controller_managers_mutex_ must be locked externally */ void discover() @@ -434,7 +434,7 @@ class MoveItMultiControllerManager : public moveit_controller_manager::MoveItCon { std::string ns = service_name.substr(0, found); if (controller_managers_.find(ns) == controller_managers_.end()) - { // create MoveItControllerManager if it does not exists + { // create MoveItControllerManager if it does not exist RCLCPP_INFO_STREAM(LOGGER, "Adding controller_manager interface for node at namespace " << ns); auto controller_manager = std::make_shared(ns); controller_manager->initialize(node_); From 8cc1c4caa42e671c327772450ba59b519f9cda55 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 26 Oct 2021 19:06:53 +0300 Subject: [PATCH 12/15] Bump ros2_control version to 1.1.0 (#757) --- moveit2.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit2.repos b/moveit2.repos index a3b8f4fa895..afa6e8a0f17 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -18,8 +18,8 @@ repositories: ros2_control: type: git url: https://github.com/ros-controls/ros2_control - version: 1.0.0 + version: 1.1.0 ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers - version: 1.0.0 + version: 1.1.0 From 9e1bb08b611f346c17ba10eca31aef6e5cea4596 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Tue, 26 Oct 2021 20:54:03 +0300 Subject: [PATCH 13/15] CI: Sync with MoveIt 1 (#756) * Sync with MoveIt CI * Allow full clang-tidy run via manual trigger * Unify formatting with MoveIt 1 Co-authored-by: Robert Haschke --- .ci.prepare_codecov | 21 ------------- .github/workflows/ci.yaml | 59 +++++++++++++++++++++-------------- .github/workflows/format.yaml | 6 ++++ 3 files changed, 41 insertions(+), 45 deletions(-) delete mode 100755 .ci.prepare_codecov diff --git a/.ci.prepare_codecov b/.ci.prepare_codecov deleted file mode 100755 index 91119f5dc18..00000000000 --- a/.ci.prepare_codecov +++ /dev/null @@ -1,21 +0,0 @@ -#!/bin/bash -pushd $BASEDIR - -BLUE='\033[0;34m' -NOCOLOR='\033[0m' - -apt-get install -qq lcov - -echo -e "${BLUE}Capture coverage info${NOCOLOR}" -lcov --capture --directory target_ws --output-file coverage.info - -echo -e "${BLUE}Extract repository files${NOCOLOR}" -lcov --extract coverage.info "$BASEDIR/target_ws/src/$TARGET_REPO_NAME/*" --output-file coverage.info - -echo -e "${BLUE}Filter out test files${NOCOLOR}" -lcov --remove coverage.info '*/test/*' --output-file coverage.info - -echo -e "${BLUE}Output coverage data for debugging${NOCOLOR}" -lcov --list coverage.info - -popd diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index b3c431fdb2f..f5a57cfd66f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -30,23 +30,21 @@ jobs: # Pull any updates to the upstream workspace (after restoring it from cache) AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src - AFTER_RUN_TARGET_TEST: ${{ matrix.env.CCOV && './.ci.prepare_codecov' || '' }} TARGET_CMAKE_ARGS: > - -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'RelWithDebInfo' || 'Release'}} - -DCMAKE_CXX_FLAGS="$CXXFLAGS ${{ matrix.env.CCOV && '--coverage'}}" - --no-warn-unused-cli + -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}} + -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}" CCACHE_DIR: ${{ github.workspace }}/.ccache BASEDIR: ${{ github.workspace }}/.work - CLANG_TIDY_BASE_REF: ${{ github.base_ref || github.ref }} + CLANG_TIDY_BASE_REF: ${{ github.event_name != 'workflow_dispatch' && (github.base_ref || github.ref) || '' }} BEFORE_CLANG_TIDY_CHECKS: (cd $TARGET_REPO_PATH; clang-tidy --list-checks) CC: ${{ matrix.env.CLANG_TIDY && 'clang' }} CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }} - name: ${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && ' + ccov' || ''}}${{ matrix.env.IKFAST_TEST && ' + ikfast' || ''}}${{ matrix.env.CLANG_TIDY && ' + clang-tidy' || '' }} + name: ${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && ' + ccov' || ''}}${{ matrix.env.IKFAST_TEST && ' + ikfast' || ''}}${{ matrix.env.CLANG_TIDY && (github.event_name != 'workflow_dispatch' && ' + clang-tidy (delta)' || ' + clang-tidy (all)') || '' }} runs-on: ubuntu-latest steps: - name: "Free up disk space" - if: ${{ matrix.env.CCOV }} + if: matrix.env.CCOV run: | sudo apt-get -qq purge build-essential "ghc*" sudo apt-get clean @@ -60,7 +58,7 @@ jobs: sudo rm -rf /usr/local df -h - uses: actions/checkout@v2 - - name: cache upstream_ws + - name: Cache upstream workspace uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.BASEDIR }}/upstream_ws @@ -70,8 +68,8 @@ jobs: CACHE_PREFIX: upstream_ws-${{ matrix.env.IMAGE }}-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }} # The target directory cache doesn't include the source directory because # that comes from the checkout. See "prepare target_ws for cache" task below - - name: cache target_ws - if: ${{ ! matrix.env.CCOV }} + - name: Cache target workspace + if: "!matrix.env.CCOV" uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.BASEDIR }}/target_ws @@ -79,7 +77,7 @@ jobs: restore-keys: ${{ env.CACHE_PREFIX }} env: CACHE_PREFIX: target_ws${{ matrix.env.CCOV && '-ccov' || '' }}-${{ matrix.env.IMAGE }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml', '.github/workflows/ci.yaml') }} - - name: cache ccache + - name: Cache ccache uses: pat-s/always-upload-cache@v2.1.5 with: path: ${{ env.CCACHE_DIR }} @@ -90,27 +88,40 @@ jobs: env: CACHE_PREFIX: ccache-${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && '-ccov' || '' }} - - name: generate ikfast packages - if: ${{ matrix.env.IKFAST_TEST }} - run: | - moveit_kinematics/test/test_ikfast_plugins.sh - - id: industrial_ci + - name: Generate ikfast packages + if: matrix.env.IKFAST_TEST + run: moveit_kinematics/test/test_ikfast_plugins.sh + - id: ici + name: Run industrial_ci uses: ros-industrial/industrial_ci@master env: ${{ matrix.env }} - - name: upload test artifacts (on failure) + - name: Upload test artifacts (on failure) uses: actions/upload-artifact@v2 - if: steps.industrial_ci.outcome != 'success' + if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results) with: name: test-results-${{ matrix.env.IMAGE }} path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml - - name: upload codecov report - uses: codecov/codecov-action@v1 - if: ${{ matrix.env.CCOV }} + - name: Generate codecov report + uses: rhaschke/lcov-action@main + if: matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' + with: + docker: $DOCKER_IMAGE + workdir: ${{ env.BASEDIR }}/target_ws + ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"' + - name: Upload codecov report + uses: codecov/codecov-action@v2 + if: matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' + with: + files: ${{ env.BASEDIR }}/target_ws/coverage.info + - name: Upload clang-tidy changes + uses: rhaschke/upload-git-patch-action@main + if: matrix.env.CLANG_TIDY with: - files: ${{ env.BASEDIR }}/coverage.info - - name: prepare target_ws for cache - if: ${{ always() && ! matrix.env.CCOV }} + name: clang-tidy + path: ${{ env.BASEDIR }}/target_ws/src/$(basename $(pwd)) + - name: Prepare target_ws for cache + if: "!matrix.env.CCOV" run: | du -sh ${{ env.BASEDIR }}/target_ws sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 121309898e2..ddfa93f93af 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -20,3 +20,9 @@ jobs: - name: Install clang-format-10 run: sudo apt-get install clang-format-10 - uses: pre-commit/action@v2.0.3 + id: precommit + - name: Upload pre-commit changes + if: failure() && steps.precommit.outcome == 'failure' + uses: rhaschke/upload-git-patch-action@main + with: + name: pre-commit From 2391195d7c4a951bac0d82129c2d854eb50e92e2 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 27 Oct 2021 08:50:15 -0600 Subject: [PATCH 14/15] Fix missing test depend in servo (#759) * Fix missing test depend in servo * Alphabetize Co-authored-by: AndyZe --- moveit_ros/moveit_servo/package.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index c3e2f9ff3fd..23558a575a3 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -43,11 +43,11 @@ tf2_ros ament_cmake_gtest - moveit_resources_panda_moveit_config - ros_testing - ament_lint_auto ament_lint_common + controller_manager + moveit_resources_panda_moveit_config + ros_testing ament_cmake From 7e3fdcffbfef79d92a665d6ea2cd631befd872aa Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Fri, 29 Oct 2021 10:57:04 -0600 Subject: [PATCH 15/15] Move initialization of ServoNode into constructor (#761) --- .../moveit_servo/include/moveit_servo/servo.h | 4 +- .../include/moveit_servo/servo_node.h | 15 +-- moveit_ros/moveit_servo/src/servo.cpp | 34 +++---- moveit_ros/moveit_servo/src/servo_node.cpp | 94 ++++++------------- .../moveit_servo/src/servo_node_main.cpp | 4 +- .../test/servo_launch_test_common.hpp | 22 ----- .../test/test_servo_interface.cpp | 36 ------- 7 files changed, 53 insertions(+), 156 deletions(-) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h index 68d5467f6f0..3c4bf5669f5 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -98,8 +98,8 @@ class Servo // The stored servo parameters ServoParameters::SharedConstPtr parameters_; - std::unique_ptr servo_calcs_; - std::unique_ptr collision_checker_; + ServoCalcs servo_calcs_; + CollisionCheck collision_checker_; }; // ServoPtr using alias diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h index 3ffeb8fde35..e5040415925 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h @@ -45,18 +45,19 @@ namespace moveit_servo { -class ServoNode : public rclcpp::Node +class ServoNode { public: ServoNode(const rclcpp::NodeOptions& options); -private: - bool init(); - - void reset(); - - rclcpp::TimerBase::SharedPtr initialization_timer_; + // NOLINTNEXTLINE(readability-identifier-naming) + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() + { + return node_->get_node_base_interface(); + } +private: + std::shared_ptr node_; std::unique_ptr servo_; std::shared_ptr tf_buffer_; std::shared_ptr planning_scene_monitor_; diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 0286f874d5e..556572aca84 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -42,25 +42,19 @@ namespace moveit_servo { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo"); namespace { +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo"); constexpr double ROBOT_STATE_WAIT_TIME = 10.0; // seconds } // namespace + Servo::Servo(const rclcpp::Node::SharedPtr& node, ServoParameters::SharedConstPtr parameters, planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor) - : planning_scene_monitor_(planning_scene_monitor), parameters_(parameters) + : planning_scene_monitor_{ planning_scene_monitor } + , parameters_{ parameters } + , servo_calcs_{ node, parameters, planning_scene_monitor_ } + , collision_checker_{ node, parameters, planning_scene_monitor_ } { - // Confirm the planning scene monitor is ready to be used - if (!planning_scene_monitor_->getStateMonitor()) - { - planning_scene_monitor_->startStateMonitor(parameters_->joint_topic); - } - planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true); - - servo_calcs_ = std::make_unique(node, parameters, planning_scene_monitor_); - - collision_checker_ = std::make_unique(node, parameters, planning_scene_monitor_); } void Servo::start() @@ -75,11 +69,11 @@ void Servo::start() setPaused(false); // Crunch the numbers in this timer - servo_calcs_->start(); + servo_calcs_.start(); // Check collisions in this timer if (parameters_->check_collisions) - collision_checker_->start(); + collision_checker_.start(); } Servo::~Servo() @@ -89,28 +83,28 @@ Servo::~Servo() void Servo::setPaused(bool paused) { - servo_calcs_->setPaused(paused); - collision_checker_->setPaused(paused); + servo_calcs_.setPaused(paused); + collision_checker_.setPaused(paused); } bool Servo::getCommandFrameTransform(Eigen::Isometry3d& transform) { - return servo_calcs_->getCommandFrameTransform(transform); + return servo_calcs_.getCommandFrameTransform(transform); } bool Servo::getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform) { - return servo_calcs_->getCommandFrameTransform(transform); + return servo_calcs_.getCommandFrameTransform(transform); } bool Servo::getEEFrameTransform(Eigen::Isometry3d& transform) { - return servo_calcs_->getEEFrameTransform(transform); + return servo_calcs_.getEEFrameTransform(transform); } bool Servo::getEEFrameTransform(geometry_msgs::msg::TransformStamped& transform) { - return servo_calcs_->getEEFrameTransform(transform); + return servo_calcs_.getEEFrameTransform(transform); } const ServoParameters::SharedConstPtr& Servo::getParameters() const diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 8cd8b7cbabc..0c5dc71f346 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -44,7 +44,8 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_node namespace moveit_servo { -ServoNode::ServoNode(const rclcpp::NodeOptions& options) : Node("servo_node", options), is_initialized_(false) +ServoNode::ServoNode(const rclcpp::NodeOptions& options) + : node_{ std::make_shared("servo_node", options) } { if (!options.use_intra_process_comms()) { @@ -57,106 +58,65 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) : Node("servo_node", op using std::placeholders::_1; using std::placeholders::_2; start_servo_service_ = - this->create_service("~/start_servo", std::bind(&ServoNode::startCB, this, _1, _2)); + node_->create_service("~/start_servo", std::bind(&ServoNode::startCB, this, _1, _2)); stop_servo_service_ = - this->create_service("~/stop_servo", std::bind(&ServoNode::stopCB, this, _1, _2)); + node_->create_service("~/stop_servo", std::bind(&ServoNode::stopCB, this, _1, _2)); pause_servo_service_ = - this->create_service("~/pause_servo", std::bind(&ServoNode::pauseCB, this, _1, _2)); + node_->create_service("~/pause_servo", std::bind(&ServoNode::pauseCB, this, _1, _2)); unpause_servo_service_ = - this->create_service("~/unpause_servo", std::bind(&ServoNode::unpauseCB, this, _1, _2)); -} - -bool ServoNode::init() -{ - bool performed_initialization = true; + node_->create_service("~/unpause_servo", std::bind(&ServoNode::unpauseCB, this, _1, _2)); // Can set robot_description name from parameters std::string robot_description_name = "robot_description"; - this->get_parameter_or("robot_description_name", robot_description_name, robot_description_name); - - // Set up planning_scene_monitor - auto node_ptr = shared_from_this(); - planning_scene_monitor_ = std::make_shared( - node_ptr, robot_description_name, "planning_scene_monitor"); + node_->get_parameter_or("robot_description_name", robot_description_name, robot_description_name); // Get the servo parameters - auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_ptr, LOGGER); + auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_, LOGGER); if (servo_parameters == nullptr) { - RCLCPP_FATAL(LOGGER, "Failed to load the servo parameters"); - performed_initialization = false; - } - - // Start the planning scene monitor - performed_initialization &= (planning_scene_monitor_->getPlanningScene() != nullptr); - if (performed_initialization) - { - planning_scene_monitor_->startStateMonitor(servo_parameters->joint_topic); - planning_scene_monitor_->setPlanningScenePublishingFrequency(25); - planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, - "/moveit_servo/publish_planning_scene"); - planning_scene_monitor_->startSceneMonitor(); - } - else - { - RCLCPP_FATAL(LOGGER, "Planning scene not configured"); - } - - // If we initialized properly, go ahead and start everything up - if (performed_initialization) - { - // Create Servo - servo_ = std::make_unique(node_ptr, servo_parameters, planning_scene_monitor_); - is_initialized_ = true; - servo_->start(); - return true; - } - else - { - RCLCPP_FATAL(LOGGER, "Servo Service failed to initialize properly, not starting servoing"); - return false; + RCLCPP_ERROR(LOGGER, "Failed to load the servo parameters"); + throw std::runtime_error("Failed to load the servo parameters"); } -} -void ServoNode::reset() -{ - servo_.reset(); - planning_scene_monitor_.reset(); - is_initialized_ = false; + // Set up planning_scene_monitor + planning_scene_monitor_ = std::make_shared( + node_, robot_description_name, "planning_scene_monitor"); + planning_scene_monitor_->startStateMonitor(servo_parameters->joint_topic); + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->setPlanningScenePublishingFrequency(25); + planning_scene_monitor_->getStateMonitor()->enableCopyDynamics(true); + planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, + "~/publish_planning_scene"); + + // Create Servo + servo_ = std::make_unique(node_, servo_parameters, planning_scene_monitor_); } void ServoNode::startCB(const std::shared_ptr request, std::shared_ptr response) { - // If we already initialized, reset servo before initializing again - if (is_initialized_) - reset(); - - response->success = init(); + servo_->start(); + response->success = true; } void ServoNode::stopCB(const std::shared_ptr request, std::shared_ptr response) { - reset(); + servo_->setPaused(true); response->success = true; } void ServoNode::pauseCB(const std::shared_ptr request, std::shared_ptr response) { - if (servo_) - servo_->setPaused(true); - + servo_->setPaused(true); response->success = true; } void ServoNode::unpauseCB(const std::shared_ptr request, std::shared_ptr response) { - if (servo_) - servo_->setPaused(false); - + servo_->setPaused(false); response->success = true; } diff --git a/moveit_ros/moveit_servo/src/servo_node_main.cpp b/moveit_ros/moveit_servo/src/servo_node_main.cpp index a4c2d832807..4bda15e91c8 100644 --- a/moveit_ros/moveit_servo/src/servo_node_main.cpp +++ b/moveit_ros/moveit_servo/src/servo_node_main.cpp @@ -46,9 +46,9 @@ int main(int argc, char* argv[]) rclcpp::NodeOptions options; options.automatically_declare_parameters_from_overrides(true); - auto node = std::make_shared(options); + auto servo_node = std::make_shared(options); - rclcpp::spin(node); + rclcpp::spin(servo_node->get_node_base_interface()); rclcpp::shutdown(); } diff --git a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp index a0152a020aa..ba15c88d30a 100644 --- a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp +++ b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp @@ -440,28 +440,6 @@ class ServoFixture : public ::testing::Test RCLCPP_ERROR(LOGGER, "Error returned form service call to stop servo"); return false; } - RCLCPP_INFO_STREAM(LOGGER, "Wait for stop servo service: " << (node_->now() - time_start).seconds()); - - // Test that status messages stop - rclcpp::Rate publish_loop_rate(test_parameters_->publish_hz); - time_start = node_->now(); - size_t num_statuses_start = 0; - size_t iterations = 0; - do - { - num_statuses_start = getNumStatus(); - // Wait 4x the loop rate - for (size_t i = 0; i < 4; ++i) - publish_loop_rate.sleep(); - } while (getNumStatus() != num_statuses_start && ++iterations < test_parameters_->timeout_iterations); - RCLCPP_INFO_STREAM(LOGGER, "Wait for status to stop: " << (node_->now() - time_start).seconds()); - - if (iterations >= test_parameters_->timeout_iterations) - { - RCLCPP_ERROR(LOGGER, "Timeout waiting for status num increasing"); - return false; - } - return true; } diff --git a/moveit_ros/moveit_servo/test/test_servo_interface.cpp b/moveit_ros/moveit_servo/test/test_servo_interface.cpp index ca5e5a728ef..6a7a18f856e 100644 --- a/moveit_ros/moveit_servo/test/test_servo_interface.cpp +++ b/moveit_ros/moveit_servo/test/test_servo_interface.cpp @@ -40,42 +40,6 @@ namespace moveit_servo { -TEST_F(ServoFixture, StartStopTest) -{ - // Setup the start/stop clients, and the command callback - auto log_time_start = node_->now(); - ASSERT_TRUE(setupStartClient()); - ASSERT_TRUE(setupJointStateSub()); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Setup time: " << (log_time_end - log_time_start).seconds()); - - // Wait for a joint state message to ensure fake_joint_driver is up - rclcpp::Rate publish_loop_rate(test_parameters_->publish_hz); - log_time_start = node_->now(); - bool got_msg = false; - while (!got_msg) - { - if (getNumJointState() > 0) - got_msg = true; - - publish_loop_rate.sleep(); - } - ASSERT_TRUE(got_msg); - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Wait for joint state message time: " << (log_time_end - log_time_start).seconds()); - - // Try to start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - // Now stop servo - ASSERT_TRUE(stop()); - - // Restart and recheck Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); -} - TEST_F(ServoFixture, SendTwistStampedTest) { auto log_time_start = node_->now();