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",
)