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 7e7fd25a0f4..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,18 +58,18 @@ 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 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 - 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 f3c976d5e5d..ddfa93f93af 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -19,4 +19,10 @@ 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 + id: precommit + - name: Upload pre-commit changes + if: failure() && steps.precommit.outcome == 'failure' + uses: rhaschke/upload-git-patch-action@main + with: + name: pre-commit diff --git a/moveit2.repos b/moveit2.repos index 6c9272be900..afa6e8a0f17 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.1.0 + ros2_controllers: + type: git + url: https://github.com/ros-controls/ros2_controllers + version: 1.1.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 diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 8cc6e8140a0..ceb8b528650 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 + 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..6cfdfbbd1b2 --- /dev/null +++ b/moveit_core/online_signal_smoothing/CMakeLists.txt @@ -0,0 +1,54 @@ +# Base class +set(SMOOTHING_BASE_LIB moveit_smoothing_base) +add_library(${SMOOTHING_BASE_LIB} SHARED + src/smoothing_base_class.cpp +) +include(GenerateExportHeader) +generate_export_header(${SMOOTHING_BASE_LIB}) +target_include_directories(${SMOOTHING_BASE_LIB} PUBLIC + $ +) +set_target_properties(${SMOOTHING_BASE_LIB} PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}" +) +ament_target_dependencies(${SMOOTHING_BASE_LIB} + rclcpp +) + +# Plugin implementations +set(BUTTERWORTH_FILTER_LIB moveit_butterworth_filter) +add_library(${BUTTERWORTH_FILTER_LIB} SHARED + src/butterworth_filter.cpp +) +generate_export_header(${BUTTERWORTH_FILTER_LIB}) +target_include_directories(${BUTTERWORTH_FILTER_LIB} PUBLIC + $ +) +set_target_properties(${BUTTERWORTH_FILTER_LIB} PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}" +) +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}/${SMOOTHING_BASE_LIB}_export.h DESTINATION include) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${BUTTERWORTH_FILTER_LIB}_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 ${BUTTERWORTH_FILTER_LIB}) +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..95632245043 --- /dev/null +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h @@ -0,0 +1,112 @@ +/********************************************************************* + * 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); + ButterworthFilter() = delete; + + 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: + /** + * 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..4621849be72 --- /dev/null +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/smoothing_base_class.h @@ -0,0 +1,86 @@ +/********************************************************************* + * 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 "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(); + virtual ~SmoothingBaseClass(); + + /** + * 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..ca3d2b45e2f --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/butterworth_filter.cpp @@ -0,0 +1,141 @@ +/********************************************************************* + * 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 +{ +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..f39984f3d42 --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/smoothing_base_class.cpp @@ -0,0 +1,45 @@ +/********************************************************************* + * 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 +{ +SmoothingBaseClass::SmoothingBaseClass() = default; +SmoothingBaseClass::~SmoothingBaseClass() = default; +} // namespace online_signal_smoothing 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_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_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_); 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/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/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.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_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_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/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/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 f9bf5ad4e7f..640412e0ce8 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", @@ -88,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/package.xml b/moveit_ros/moveit_servo/package.xml index ff15ed32392..23558a575a3 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,15 +39,15 @@ joint_state_broadcaster joint_trajectory_controller joy - tf2_ros robot_state_publisher + tf2_ros - moveit_resources_panda_moveit_config - ros_testing ament_cmake_gtest - ament_lint_auto ament_lint_common + controller_manager + moveit_resources_panda_moveit_config + ros_testing ament_cmake 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.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_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_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/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' 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..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", ) @@ -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, 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/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(); 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", )