diff --git a/.docker/ci-testing/Dockerfile b/.docker/ci-testing/Dockerfile index 9a872d92c6..f3ca5f6e6d 100644 --- a/.docker/ci-testing/Dockerfile +++ b/.docker/ci-testing/Dockerfile @@ -1,10 +1,8 @@ # ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci-testing # CI image using the ROS testing repository -ARG ROS_DISTRO=foxy +ARG ROS_DISTRO=rolling FROM ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci - -MAINTAINER Dave Coleman dave@picknik.ai MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de # Switch to ros-testing diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index da4d0f8700..a9be862e96 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -1,34 +1,34 @@ # ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci # ROS base image augmented with all MoveIt dependencies to use for CI -ARG ROS_DISTRO=foxy +ARG ROS_DISTRO=rolling FROM ros:${ROS_DISTRO}-ros-base-focal - MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de -MAINTAINER Dave Coleman dave@picknik.ai ENV TERM xterm +# Setup (temporary) ROS workspace WORKDIR /root/ws_moveit +# Copy MoveIt sources from docker context +COPY . src/moveit2 + # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers RUN \ # Update apt package list as previous containers clear the cache - apt-get -qq update && \ - apt-get -qq dist-upgrade && \ + apt-get -q update && \ + apt-get -q -y dist-upgrade && \ # # Install some base dependencies - apt-get -qq install --no-install-recommends -y \ + apt-get -q install --no-install-recommends -y \ # Some basic requirements wget git sudo curl \ # Preferred build tools clang clang-format-10 clang-tidy clang-tools \ ccache && \ # - # Download MoveIt source, so that we can fetch all necessary dependencies - mkdir src && \ - git clone https://github.com/ros-planning/moveit2.git src/moveit2 -b main && \ + # Fetch all dependencies from moveit2.repos vcs import src < src/moveit2/moveit2.repos && \ if [ -r src/moveit2/moveit2_${ROS_DISTRO}.repos ] ; then vcs import src < src/moveit2/moveit2_${ROS_DISTRO}.repos ; fi && \ # diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index f49f32d1bc..bc8e7b2796 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -1,11 +1,12 @@ # ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-release # Full debian-based install of MoveIt using apt-get -ARG ROS_DISTRO=foxy +ARG ROS_DISTRO=rolling FROM ros:${ROS_DISTRO}-ros-base-focal -MAINTAINER Dave Coleman dave@picknik.ai +MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size -RUN apt-get update && \ +RUN apt-get update -q && \ + apt-get dist-upgrade -q -y && \ apt-get install -y ros-${ROS_DISTRO}-moveit-* && \ rm -rf /var/lib/apt/lists/* diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index e6993ccbf3..153878f351 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -1,35 +1,34 @@ +# syntax = docker/dockerfile:1.3 + # ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-source # Downloads the moveit source code and install remaining debian dependencies -ARG ROS_DISTRO=foxy +ARG ROS_DISTRO=rolling FROM ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci-testing - MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de -MAINTAINER Dave Coleman dave@picknik.ai +# Export ROS_UNDERLAY for downstream docker containers ENV ROS_UNDERLAY /root/ws_moveit/install -WORKDIR $ROS_UNDERLAY/../src +WORKDIR $ROS_UNDERLAY/.. + +# Copy MoveIt sources from docker context +COPY . src/moveit2 # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers -RUN \ - # Download moveit source so that we can get necessary dependencies - git clone https://github.com/ros-planning/moveit2.git -b main && \ - vcs import < moveit2/moveit2.repos && \ - if [ -r moveit2/moveit2_${ROS_DISTRO}.repos ] ; then vcs import < moveit2/moveit2_${ROS_DISTRO}.repos ; fi && \ +RUN --mount=type=cache,target=/root/.ccache/ \ + # Enable ccache + PATH=/usr/lib/ccache:$PATH && \ + # Fetch required upstream sources for building + vcs import src < src/moveit2/moveit2.repos && \ + if [ -r src/moveit2/moveit2_${ROS_DISTRO}.repos ] ; then vcs import src < src/moveit2/moveit2_${ROS_DISTRO}.repos ; fi && \ # - # Update apt package list as cache is cleared in previous container - # Usually upgrading involves a few packages only (if container builds became out-of-sync) - apt-get -qq update && \ - apt-get -qq dist-upgrade && \ - # - rosdep update && \ - rosdep install -y --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \ - rm -rf /var/lib/apt/lists/* - -# Build the workspace -RUN cd $ROS_UNDERLAY/.. && . /opt/ros/${ROS_DISTRO}/setup.sh &&\ + . /opt/ros/${ROS_DISTRO}/setup.sh &&\ colcon build \ --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF -DCMAKE_EXPORT_COMPILE_COMMANDS=ON \ - --ament-cmake-args -DCMAKE_BUILD_TYPE=Release \ - --event-handlers desktop_notification- status- + --ament-cmake-args -DCMAKE_BUILD_TYPE=Release \ + --event-handlers desktop_notification- status- && \ + ccache -s && \ + # + # Update /ros_entrypoint.sh to source our new workspace + sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.sh#g" /ros_entrypoint.sh diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000000..8cf86e3030 --- /dev/null +++ b/.dockerignore @@ -0,0 +1,41 @@ +# ignore everything +* +# but include these: +!moveit2*.repos +!**/package.xml +!**/COLCON_IGNORE + +# https://github.com/moby/moby/issues/42788 +!moveit_plugins/moveit_plugins/package.xml +!moveit_plugins/moveit_ros_control_interface/package.xml +!moveit_plugins/moveit_simple_controller_manager/package.xml +!moveit_kinematics/package.xml +!moveit_common/package.xml +!moveit_setup_assistant/package.xml +!moveit_core/package.xml +!moveit_commander/package.xml +!moveit_demo_nodes/run_move_group/package.xml +!moveit_demo_nodes/run_ompl_constrained_planning/package.xml +!moveit_demo_nodes/run_moveit_cpp/package.xml +!moveit_planners/ompl/package.xml +!moveit_planners/chomp/chomp_motion_planner/package.xml +!moveit_planners/chomp/chomp_interface/package.xml +!moveit_planners/chomp/chomp_optimizer_adapter/package.xml +!moveit_planners/pilz_industrial_motion_planner_testutils/package.xml +!moveit_planners/pilz_industrial_motion_planner/package.xml +!moveit_planners/moveit_planners/package.xml +!moveit_planners/trajopt/package.xml +!moveit_runtime/package.xml +!moveit/package.xml +!moveit_ros/warehouse/package.xml +!moveit_ros/moveit_servo/package.xml +!moveit_ros/occupancy_map_monitor/package.xml +!moveit_ros/perception/package.xml +!moveit_ros/move_group/package.xml +!moveit_ros/robot_interaction/package.xml +!moveit_ros/visualization/package.xml +!moveit_ros/manipulation/package.xml +!moveit_ros/planning/package.xml +!moveit_ros/planning_interface/package.xml +!moveit_ros/benchmarks/package.xml +!moveit_ros/moveit_ros/package.xml diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index 83bfa2f1ee..407be89214 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -26,6 +26,7 @@ jobs: steps: - name: Check for apt updates uses: addnab/docker-run-action@v3 + continue-on-error: true id: apt with: image: ${{ env.GH_IMAGE }} @@ -77,6 +78,7 @@ jobs: steps: - name: Check for apt updates uses: addnab/docker-run-action@v3 + continue-on-error: true id: apt with: image: ${{ env.GH_IMAGE }} @@ -129,6 +131,7 @@ jobs: steps: - name: Check for apt updates uses: addnab/docker-run-action@v3 + continue-on-error: true id: apt with: image: ${{ env.GH_IMAGE }} @@ -179,6 +182,7 @@ jobs: DH_IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} steps: + - uses: actions/checkout@v2 - name: Set up Docker Buildx uses: docker/setup-buildx-action@v1 - name: Login to Github Containter Registry @@ -192,15 +196,17 @@ jobs: with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: "Remove .dockerignore" + run: rm .dockerignore # enforce full source context - name: Build and Push uses: docker/build-push-action@v2 with: + context: . file: .docker/${{ github.job }}/Dockerfile build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} push: true cache-from: type=registry,ref=${{ env.GH_IMAGE }} cache-to: type=inline - no-cache: true tags: | ${{ env.GH_IMAGE }} ${{ env.DH_IMAGE }} diff --git a/MIGRATION.md b/MIGRATION.md index a352b12029..c3d287c424 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -2,6 +2,9 @@ API changes in MoveIt releases +## ROS Rolling +- ServoServer was renamed to ServoNode + ## ROS Noetic - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link. - Planned trajectories are *slow* by default. diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index 1918466bac..3952247b10 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -48,12 +48,12 @@ target_link_libraries(collision_detector_bullet_plugin install(DIRECTORY include/ DESTINATION include) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${MOVEIT_LIB_NAME}_export.h DESTINATION include) -install(TARGETS ${MOVEIT_LIB_NAME} EXPORT ${MOVEIT_LIB_NAME} - TARGETS collision_detector_bullet_plugin EXPORT collision_detector_bullet_plugin +install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_bullet_plugin EXPORT export_${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin ) +ament_export_targets(export_${MOVEIT_LIB_NAME} HAS_LIBRARY_TARGET) if(BUILD_TESTING) if(WIN32) diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index 2034910bda..71a65870b1 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -160,7 +160,8 @@ class PosedDistanceField : public distance_field::PropagationDistanceField double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z, bool& in_bounds) const { - Eigen::Vector3d rel_pos = pose_.inverse() * Eigen::Vector3d(x, y, z); + // Transpose of a rotation matrix equals its inverse, but computationally cheaper + Eigen::Vector3d rel_pos = pose_.linear().transpose() * Eigen::Vector3d(x, y, z); double gx, gy, gz; double res = distance_field::PropagationDistanceField::getDistanceGradient(rel_pos.x(), rel_pos.y(), rel_pos.z(), gx, gy, gz, in_bounds); diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index e1f3b9f75c..28d8d61e33 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -273,6 +273,21 @@ class RobotTrajectory */ bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const; + /** @brief Print information about the trajectory + * @param out Stream to print to + * @param variable_indexes The indexes of the variables to print. + * If empty/not specified and the group is defined, then uses the indexes for the group + * If empty and the group is not defined, uses ALL variables in robot_model + * + * e.g. + * Trajectory has 13 points over 2.965 seconds + * waypoint 0 time 0.000 pos 0.000 vel 0.000 acc 0.000 + * waypoint 1 time 0.067 pos 0.001 vel 0.033 acc 1.000 + * waypoint 2 time 0.665 pos 0.200 vel 0.632 acc 1.000 + * ... + */ + void print(std::ostream& out, std::vector variable_indexes = std::vector()) const; + private: moveit::core::RobotModelConstPtr robot_model_; const moveit::core::JointModelGroup* group_; @@ -280,4 +295,8 @@ class RobotTrajectory std::deque duration_from_previous_; rclcpp::Clock clock_ros_; }; + +/** @brief Operator overload for printing trajectory to a stream */ +std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory); + } // namespace robot_trajectory diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index bf312f94a8..0a3e6eadfe 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -504,4 +504,81 @@ bool RobotTrajectory::getStateAtDurationFromStart(const double request_duration, return true; } +void RobotTrajectory::print(std::ostream& out, std::vector variable_indexes) const +{ + size_t num_points = getWayPointCount(); + if (num_points == 0) + { + out << "Empty trajectory."; + return; + } + + std::ios::fmtflags old_settings = out.flags(); + int old_precision = out.precision(); + out << std::fixed << std::setprecision(3); + + out << "Trajectory has " << num_points << " points over " << getDuration() << " seconds\n"; + + if (variable_indexes.empty()) + { + if (group_) + { + variable_indexes = group_->getVariableIndexList(); + } + else + { + // use all variables + variable_indexes.resize(robot_model_->getVariableCount()); + std::iota(variable_indexes.begin(), variable_indexes.end(), 0); + } + } + + for (size_t p_i = 0; p_i < num_points; ++p_i) + { + const moveit::core::RobotState& point = getWayPoint(p_i); + out << " waypoint " << std::setw(3) << p_i; + out << " time " << std::setw(5) << getWayPointDurationFromStart(p_i); + out << " pos "; + for (int index : variable_indexes) + { + out << std::setw(6) << point.getVariablePosition(index) << " "; + } + if (point.hasVelocities()) + { + out << "vel "; + for (int index : variable_indexes) + { + out << std::setw(6) << point.getVariableVelocity(index) << " "; + } + } + if (point.hasAccelerations()) + { + out << "acc "; + for (int index : variable_indexes) + { + out << std::setw(6) << point.getVariableAcceleration(index) << " "; + } + } + if (point.hasEffort()) + { + out << "eff "; + for (int index : variable_indexes) + { + out << std::setw(6) << point.getVariableEffort(index) << " "; + } + } + out << "\n"; + } + + out.flags(old_settings); + out.precision(old_precision); + out.flush(); +} + +std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory) +{ + trajectory.print(out); + return out; +} + } // end of namespace robot_trajectory diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp index 643a4e82d4..34d8b7a655 100644 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -115,26 +115,7 @@ void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) { const moveit::core::JointModelGroup* group = trajectory.getGroup(); const std::vector& idx = group->getVariableIndexList(); - unsigned int count = trajectory.getWayPointCount(); - - std::cout << "trajectory length is " << trajectory.getWayPointDurationFromStart(count - 1) << " seconds." - << std::endl; - std::cout << " Trajectory Points" << std::endl; - for (unsigned i = 0; i < count; i++) - { - moveit::core::RobotStatePtr point = trajectory.getWayPointPtr(i); - printf(" waypoint %2d time %6.2f pos %6.2f vel %6.2f acc %6.2f ", i, trajectory.getWayPointDurationFromStart(i), - point->getVariablePosition(idx[0]), point->getVariableVelocity(idx[0]), - point->getVariableAcceleration(idx[0])); - if (i > 0) - { - moveit::core::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1); - printf("jrk %6.2f", - (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) / - (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1))); - } - printf("\n"); - } + trajectory.print(std::cout, { idx[0] }); } TEST(TestTimeParameterization, TestIterativeParabolic) 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 807ef04c5a..aa87242cf8 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 @@ -138,7 +138,8 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll managed_controllers_.clear(); active_controllers_.clear(); - for (const controller_manager_msgs::msg::ControllerState& controller : result_future.get()->controller) + const auto& result = result_future.get(); + for (const controller_manager_msgs::msg::ControllerState& controller : result->controller) { if (isActive(controller)) { diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index 29343fefa0..e80411247e 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -16,11 +16,11 @@ set(SERVO_PARAM_LIB_NAME ${SERVO_LIB_NAME}_parameters) set(POSE_TRACKING pose_tracking) # Component Nodes (Shared libraries) ############################ -set(SERVO_COMPONENT_NODE servo_server) +set(SERVO_COMPONENT_NODE servo_node) set(SERVO_CONTROLLER_INPUT servo_controller_input) # Executable Nodes ############################################## -set(SERVO_SERVER_NODE_NAME servo_server_node) +set(SERVO_NODE_MAIN_NAME servo_node_main) set(POSE_TRACKING_DEMO_NAME servo_pose_tracking_demo) set(FAKE_SERVO_CMDS_NAME fake_command_publisher) @@ -96,10 +96,10 @@ target_link_libraries(${POSE_TRACKING} ${SERVO_LIB_NAME}) ##################### # Add and export library to run as a ROS node component, and receive commands via topics -add_library(${SERVO_COMPONENT_NODE} SHARED src/servo_server.cpp) +add_library(${SERVO_COMPONENT_NODE} SHARED src/servo_node.cpp) ament_target_dependencies(${SERVO_COMPONENT_NODE} ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(${SERVO_COMPONENT_NODE} ${SERVO_LIB_NAME}) -rclcpp_components_register_nodes(${SERVO_COMPONENT_NODE} "moveit_servo::ServoServer") +rclcpp_components_register_nodes(${SERVO_COMPONENT_NODE} "moveit_servo::ServoNode") # Add executable for using a controller add_library(${SERVO_CONTROLLER_INPUT} SHARED src/teleop_demo/joystick_servo_example.cpp) @@ -111,9 +111,9 @@ rclcpp_components_register_nodes(${SERVO_CONTROLLER_INPUT} "moveit_servo::JoyToS ###################### # An executable node for the servo server -add_executable(${SERVO_SERVER_NODE_NAME} src/servo_server_node.cpp) -target_link_libraries(${SERVO_SERVER_NODE_NAME} ${SERVO_COMPONENT_NODE}) -ament_target_dependencies(${SERVO_SERVER_NODE_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_executable(${SERVO_NODE_MAIN_NAME} src/servo_node_main.cpp) +target_link_libraries(${SERVO_NODE_MAIN_NAME} ${SERVO_COMPONENT_NODE}) +ament_target_dependencies(${SERVO_NODE_MAIN_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) # An example of pose tracking add_executable(${POSE_TRACKING_DEMO_NAME} src/cpp_interface_demo/pose_tracking_demo.cpp) @@ -150,7 +150,7 @@ install( # Install Binaries install( TARGETS - ${SERVO_SERVER_NODE_NAME} + ${SERVO_NODE_MAIN_NAME} ${CPP_DEMO_NAME} ${POSE_TRACKING_DEMO_NAME} ${FAKE_SERVO_CMDS_NAME} 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 608c005b95..d5b72b4f7d 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -309,6 +309,7 @@ class ServoCalcs rclcpp::Publisher::SharedPtr worst_case_stop_time_pub_; rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub_; rclcpp::Publisher::SharedPtr multiarray_outgoing_cmd_pub_; + rclcpp::Publisher::SharedPtr condition_pub_; rclcpp::Service::SharedPtr control_dimensions_server_; rclcpp::Service::SharedPtr drift_dimensions_server_; rclcpp::Service::SharedPtr reset_servo_status_; diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h new file mode 100644 index 0000000000..3ffeb8fde3 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h @@ -0,0 +1,88 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, 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. + *********************************************************************/ + +/* Title : servo_node.h + * Project : moveit_servo + * Created : 07/13/2020 + * Author : Adam Pettinger + */ + +#pragma once + +#include +#include + +namespace moveit_servo +{ +class ServoNode : public rclcpp::Node +{ +public: + ServoNode(const rclcpp::NodeOptions& options); + +private: + bool init(); + + void reset(); + + rclcpp::TimerBase::SharedPtr initialization_timer_; + + std::unique_ptr servo_; + std::shared_ptr tf_buffer_; + std::shared_ptr planning_scene_monitor_; + + /** \brief Start the servo loop. Must be called once to begin Servoing. */ + void startCB(const std::shared_ptr request, + std::shared_ptr response); + rclcpp::Service::SharedPtr start_servo_service_; + + /** \brief Stop the servo loop. This involves more overhead than pauseCB, e.g. it clears the planning scene monitor. + * We recommend using pauseCB/unpauseCB if you will resume the Servo loop soon. + */ + void stopCB(const std::shared_ptr request, + std::shared_ptr response); + rclcpp::Service::SharedPtr stop_servo_service_; + + /** \brief Pause the servo loop but continue monitoring joint state so we can resume easily. */ + void pauseCB(const std::shared_ptr request, + std::shared_ptr response); + rclcpp::Service::SharedPtr pause_servo_service_; + + /** \brief Resume the servo loop after pauseCB has been called. */ + void unpauseCB(const std::shared_ptr request, + std::shared_ptr response); + rclcpp::Service::SharedPtr unpause_servo_service_; + + bool is_initialized_; +}; +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_server.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_server.h index 3642398a05..f16e132039 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_server.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_server.h @@ -40,49 +40,12 @@ #pragma once -#include -#include +#pragma message("Header `servo_server.h` is deprecated, please use `servo_node.h`") + +#include namespace moveit_servo { -class ServoServer : public rclcpp::Node -{ -public: - ServoServer(const rclcpp::NodeOptions& options); - -private: - bool init(); - - void reset(); - - rclcpp::TimerBase::SharedPtr initialization_timer_; - - std::unique_ptr servo_; - std::shared_ptr tf_buffer_; - std::shared_ptr planning_scene_monitor_; - - /** \brief Start the servo loop. Must be called once to begin Servoing. */ - void startCB(const std::shared_ptr request, - std::shared_ptr response); - rclcpp::Service::SharedPtr start_servo_service_; - - /** \brief Stop the servo loop. This involves more overhead than pauseCB, e.g. it clears the planning scene monitor. - * We recommend using pauseCB/unpauseCB if you will resume the Servo loop soon. - */ - void stopCB(const std::shared_ptr request, - std::shared_ptr response); - rclcpp::Service::SharedPtr stop_servo_service_; - - /** \brief Pause the servo loop but continue monitoring joint state so we can resume easily. */ - void pauseCB(const std::shared_ptr request, - std::shared_ptr response); - rclcpp::Service::SharedPtr pause_servo_service_; - - /** \brief Resume the servo loop after pauseCB has been called. */ - void unpauseCB(const std::shared_ptr request, - std::shared_ptr response); - rclcpp::Service::SharedPtr unpause_servo_service_; +using ServoServer [[deprecated("use ServoNode from servo_node.h")]] = ServoNode; - bool is_initialized_; -}; } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/servo_example.launch.py index 97891020ff..5cc5a173ba 100644 --- a/moveit_ros/moveit_servo/launch/servo_example.launch.py +++ b/moveit_ros/moveit_servo/launch/servo_example.launch.py @@ -102,6 +102,7 @@ def generate_launch_description(): executable="component_container", composable_node_descriptions=[ # Example of launching Servo as a node component + # Assuming ROS2 intraprocess communications works well, this is a more efficient way. # ComposableNode( # package="moveit_servo", # plugin="moveit_servo::ServoServer", @@ -125,17 +126,6 @@ def generate_launch_description(): name="static_tf2_broadcaster", parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], ), - ComposableNode( - package="moveit_servo", - plugin="moveit_servo::ServoServer", - name="servo_server", - parameters=[ - servo_params, - robot_description, - robot_description_semantic, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ), ComposableNode( package="moveit_servo", plugin="moveit_servo::JoyToServoPub", @@ -151,7 +141,8 @@ def generate_launch_description(): ], output="screen", ) - + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC servo_node = Node( package="moveit_servo", executable="servo_server_node", diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index d404adc848..8a3328edda 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -157,6 +157,7 @@ ServoCalcs::ServoCalcs(rclcpp::Node::SharedPtr node, // Publish status status_pub_ = node_->create_publisher(parameters_->status_topic, ROS_QUEUE_SIZE); + condition_pub_ = node_->create_publisher("~/condition", ROS_QUEUE_SIZE); internal_joint_state_.name = joint_model_group_->getActiveJointModelNames(); num_joints_ = internal_joint_state_.name.size(); @@ -383,14 +384,6 @@ void ServoCalcs::calculateSingleIteration() } } - // Print a warning to the user if both are stale - if (twist_command_is_stale_ && joint_command_is_stale_) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "Stale command. Try a larger 'incoming_command_timeout' parameter?"); - } - // If we should halt if (!have_nonzero_command_) { @@ -408,6 +401,14 @@ void ServoCalcs::calculateSingleIteration() rclcpp::Clock& clock = *node_->get_clock(); RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, "All-zero command. Doing nothing."); } + // Skip servoing publication if both types of commands are stale. + else if (twist_command_is_stale_ && joint_command_is_stale_) + { + ok_to_publish_ = false; + rclcpp::Clock& clock = *node_->get_clock(); + RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, + "Skipping publishing because incoming commands are stale."); + } else { ok_to_publish_ = true; @@ -713,6 +714,10 @@ double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& co double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); + auto condition_msg = std::make_unique(); + condition_msg->data = ini_condition; + condition_pub_->publish(std::move(condition_msg)); + // This singular vector tends to flip direction unpredictably. See R. Bro, // "Resolving the Sign Ambiguity in the Singular Value Decomposition". // Look ahead to see if the Jacobian's condition will decrease in this diff --git a/moveit_ros/moveit_servo/src/servo_server.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp similarity index 80% rename from moveit_ros/moveit_servo/src/servo_server.cpp rename to moveit_ros/moveit_servo/src/servo_node.cpp index 9b0bd772fb..8cd8b7cbab 100644 --- a/moveit_ros/moveit_servo/src/servo_server.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -31,20 +31,20 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Title : servo_server.cpp +/* Title : servo_node.cpp * Project : moveit_servo * Created : 12/31/2018 * Author : Andy Zelenak */ -#include +#include #include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_server"); +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_node"); namespace moveit_servo { -ServoServer::ServoServer(const rclcpp::NodeOptions& options) : Node("servo_service", options), is_initialized_(false) +ServoNode::ServoNode(const rclcpp::NodeOptions& options) : Node("servo_node", options), is_initialized_(false) { if (!options.use_intra_process_comms()) { @@ -57,16 +57,16 @@ ServoServer::ServoServer(const rclcpp::NodeOptions& options) : Node("servo_servi using std::placeholders::_1; using std::placeholders::_2; start_servo_service_ = - this->create_service("~/start_servo", std::bind(&ServoServer::startCB, this, _1, _2)); + this->create_service("~/start_servo", std::bind(&ServoNode::startCB, this, _1, _2)); stop_servo_service_ = - this->create_service("~/stop_servo", std::bind(&ServoServer::stopCB, this, _1, _2)); + this->create_service("~/stop_servo", std::bind(&ServoNode::stopCB, this, _1, _2)); pause_servo_service_ = - this->create_service("~/pause_servo", std::bind(&ServoServer::pauseCB, this, _1, _2)); + this->create_service("~/pause_servo", std::bind(&ServoNode::pauseCB, this, _1, _2)); unpause_servo_service_ = - this->create_service("~/unpause_servo", std::bind(&ServoServer::unpauseCB, this, _1, _2)); + this->create_service("~/unpause_servo", std::bind(&ServoNode::unpauseCB, this, _1, _2)); } -bool ServoServer::init() +bool ServoNode::init() { bool performed_initialization = true; @@ -76,9 +76,8 @@ bool ServoServer::init() // Set up planning_scene_monitor auto node_ptr = shared_from_this(); - tf_buffer_ = std::make_shared(this->get_clock()); planning_scene_monitor_ = std::make_shared( - node_ptr, robot_description_name, tf_buffer_, "planning_scene_monitor"); + node_ptr, robot_description_name, "planning_scene_monitor"); // Get the servo parameters auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_ptr, LOGGER); @@ -119,16 +118,15 @@ bool ServoServer::init() } } -void ServoServer::reset() +void ServoNode::reset() { servo_.reset(); - tf_buffer_.reset(); planning_scene_monitor_.reset(); is_initialized_ = false; } -void ServoServer::startCB(const std::shared_ptr request, - std::shared_ptr response) +void ServoNode::startCB(const std::shared_ptr request, + std::shared_ptr response) { // If we already initialized, reset servo before initializing again if (is_initialized_) @@ -137,15 +135,15 @@ void ServoServer::startCB(const std::shared_ptr response->success = init(); } -void ServoServer::stopCB(const std::shared_ptr request, - std::shared_ptr response) +void ServoNode::stopCB(const std::shared_ptr request, + std::shared_ptr response) { reset(); response->success = true; } -void ServoServer::pauseCB(const std::shared_ptr request, - std::shared_ptr response) +void ServoNode::pauseCB(const std::shared_ptr request, + std::shared_ptr response) { if (servo_) servo_->setPaused(true); @@ -153,8 +151,8 @@ void ServoServer::pauseCB(const std::shared_ptr response->success = true; } -void ServoServer::unpauseCB(const std::shared_ptr request, - std::shared_ptr response) +void ServoNode::unpauseCB(const std::shared_ptr request, + std::shared_ptr response) { if (servo_) servo_->setPaused(false); @@ -166,4 +164,4 @@ void ServoServer::unpauseCB(const std::shared_ptr -RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::ServoServer) +RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::ServoNode) diff --git a/moveit_ros/moveit_servo/src/servo_server_node.cpp b/moveit_ros/moveit_servo/src/servo_node_main.cpp similarity index 93% rename from moveit_ros/moveit_servo/src/servo_server_node.cpp rename to moveit_ros/moveit_servo/src/servo_node_main.cpp index 310901169b..a4c2d83280 100644 --- a/moveit_ros/moveit_servo/src/servo_server_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node_main.cpp @@ -31,13 +31,13 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Title : servo_server_node.cpp +/* Title : servo_node_main.cpp * Project : moveit_servo * Created : 08/18/2021 * Author : Joe Schornak */ -#include +#include int main(int argc, char* argv[]) { @@ -46,7 +46,7 @@ int main(int argc, char* argv[]) rclcpp::NodeOptions options; options.automatically_declare_parameters_from_overrides(true); - auto node = std::make_shared(options); + auto node = std::make_shared(options); rclcpp::spin(node); diff --git a/moveit_ros/moveit_servo/src/servo_parameters.cpp b/moveit_ros/moveit_servo/src/servo_parameters.cpp index 64ee044786..7b1271f0c1 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.cpp +++ b/moveit_ros/moveit_servo/src/servo_parameters.cpp @@ -43,8 +43,6 @@ #include #include -using namespace std::placeholders; // for _1, _2 etc. - namespace moveit_servo { rcl_interfaces::msg::SetParametersResult @@ -236,6 +234,7 @@ ServoParameters::SharedConstPtr ServoParameters::makeServoParameters(const rclcp // register parameter change callback if (dynamic_parameters) { + using std::placeholders::_1; parameters->on_set_parameters_callback_handler_ = node->add_on_set_parameters_callback(std::bind(&ServoParameters::setParametersCallback, parameters.get(), _1)); } diff --git a/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp b/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp index 101578ae0c..c944b98bfb 100644 --- a/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp +++ b/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp @@ -49,8 +49,8 @@ // We'll just set up parameters here const std::string JOY_TOPIC = "/joy"; -const std::string TWIST_TOPIC = "/servo_server/delta_twist_cmds"; -const std::string JOINT_TOPIC = "/servo_server/delta_joint_cmds"; +const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds"; +const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds"; const size_t ROS_QUEUE_SIZE = 10; const std::string EEF_FRAME_ID = "panda_hand"; const std::string BASE_FRAME_ID = "panda_link0"; @@ -165,8 +165,8 @@ class JoyToServoPub : public rclcpp::Node joint_pub_ = this->create_publisher(JOINT_TOPIC, ROS_QUEUE_SIZE); collision_pub_ = this->create_publisher("/planning_scene", 10); - // Create a service client to start the ServoServer - servo_start_client_ = this->create_client("/servo_server/start_servo"); + // Create a service client to start the ServoNode + servo_start_client_ = this->create_client("/servo_node/start_servo"); servo_start_client_->wait_for_service(std::chrono::seconds(1)); servo_start_client_->async_send_request(std::make_shared()); diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml index fa36c1e1ee..fef26f046b 100644 --- a/moveit_ros/moveit_servo/test/config/servo_settings.yaml +++ b/moveit_ros/moveit_servo/test/config/servo_settings.yaml @@ -47,11 +47,11 @@ hard_stop_singularity_threshold: 45.0 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names -cartesian_command_in_topic: servo_server/delta_twist_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands +cartesian_command_in_topic: servo_node/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: servo_node/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: joint_states -status_topic: servo_server/status # Publish status to this topic -command_out_topic: servo_server/command # Publish outgoing commands here +status_topic: servo_node/status # Publish status to this topic +command_out_topic: servo_node/command # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? 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 758aaf108c..cf2e60f8c0 100644 --- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py +++ b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py @@ -131,16 +131,16 @@ def generate_servo_test_description( output="screen", ) - servo_server_container = ComposableNodeContainer( - name="servo_server_container", + servo_container = ComposableNodeContainer( + name="servo_container", namespace="/", package="rclcpp_components", executable="component_container", composable_node_descriptions=[ ComposableNode( package="moveit_servo", - plugin="moveit_servo::ServoServer", - name="servo_server", + plugin="moveit_servo::ServoNode", + name="servo_node", parameters=[ servo_params, robot_description, @@ -171,14 +171,14 @@ def generate_servo_test_description( "containing test executables", ), ros2_control_node, - servo_server_container, + servo_container, test_container, TimerAction(period=2.0, actions=[servo_gtest]), launch_testing.actions.ReadyToTest(), ] + load_controllers ), { - "servo_container": servo_server_container, + "servo_container": servo_container, "test_container": test_container, "servo_gtest": servo_gtest, "ros2_control_node": ros2_control_node, diff --git a/moveit_ros/moveit_servo/test/publish_fake_jog_commands.cpp b/moveit_ros/moveit_servo/test/publish_fake_jog_commands.cpp index 261273bb8b..bc7bed6a1d 100644 --- a/moveit_ros/moveit_servo/test/publish_fake_jog_commands.cpp +++ b/moveit_ros/moveit_servo/test/publish_fake_jog_commands.cpp @@ -57,12 +57,12 @@ int main(int argc, char** argv) // Call the start service to init and start the servo component rclcpp::Client::SharedPtr servo_start_client = - node->create_client("/servo_server/start_servo"); + node->create_client("/servo_node/start_servo"); servo_start_client->wait_for_service(1s); servo_start_client->async_send_request(std::make_shared()); // Create a publisher for publishing the jog commands - auto pub = node->create_publisher("/servo_server/delta_twist_cmds", 10); + auto pub = node->create_publisher("/servo_node/delta_twist_cmds", 10); std::weak_ptr::type> captured_pub = pub; std::weak_ptr::type> captured_node = node; auto callback = [captured_pub, captured_node]() -> void { 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 f80e80810e..6ccd4713a9 100644 --- a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp +++ b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp @@ -97,7 +97,7 @@ class ServoFixture : public ::testing::Test test_parameters->publish_hz = 2.0 / servo_parameters_->incoming_command_timeout; test_parameters->publish_period = 1.0 / test_parameters->publish_hz; test_parameters->timeout_iterations = 10 * test_parameters->publish_hz; - test_parameters->servo_node_name = "/servo_server"; + test_parameters->servo_node_name = "/servo_node"; test_parameters_ = test_parameters; } diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 8c9da1760c..b6863f8afb 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -107,9 +107,18 @@ class MoveItCpp }; /** \brief Constructor */ - MoveItCpp(const rclcpp::Node::SharedPtr& node, const std::shared_ptr& tf_buffer = {}); - MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options, - const std::shared_ptr& tf_buffer = {}); + [[deprecated("Passing tf2_ros::Buffer to MoveItCpp's constructor is deprecated")]] MoveItCpp( + const rclcpp::Node::SharedPtr& node, const std::shared_ptr& tf_buffer) + : MoveItCpp(node) + { + } + MoveItCpp(const rclcpp::Node::SharedPtr& node); + [[deprecated("Passing tf2_ros::Buffer to MoveItCpp's constructor is deprecated")]] MoveItCpp( + const rclcpp::Node::SharedPtr& node, const Options& options, const std::shared_ptr& tf_buffer) + : MoveItCpp(node, options) + { + } + MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options); /** * @brief This class owns unique resources (e.g. action clients, threads) and its not very @@ -167,9 +176,6 @@ class MoveItCpp moveit::core::RobotModelConstPtr robot_model_; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - // TF - std::shared_ptr tf_buffer_; - // Planning std::map planning_pipelines_; std::map> groups_pipelines_map_; diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index 75592947c3..5c3f9619b3 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -48,18 +48,12 @@ namespace moveit_cpp static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning_interface.moveit_cpp"); constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin"; -MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const std::shared_ptr& tf_buffer) - : MoveItCpp(node, Options(node), tf_buffer) +MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node) : MoveItCpp(node, Options(node)) { } -MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options, - const std::shared_ptr& tf_buffer) - : node_(node), tf_buffer_(tf_buffer) +MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options) : node_(node) { - if (!tf_buffer_) - tf_buffer_ = std::make_shared(node_->get_clock()); - // Configure planning scene monitor if (!loadPlanningSceneMonitor(options.planning_scene_monitor_options)) { @@ -100,7 +94,7 @@ MoveItCpp::~MoveItCpp() bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& options) { planning_scene_monitor_.reset( - new planning_scene_monitor::PlanningSceneMonitor(node_, options.robot_description, tf_buffer_, options.name)); + new planning_scene_monitor::PlanningSceneMonitor(node_, options.robot_description, options.name)); // Allows us to sycronize to Rviz and also publish collision objects to ourselves RCLCPP_DEBUG(LOGGER, "Configuring Planning Scene Monitor"); if (planning_scene_monitor_->getPlanningScene()) @@ -301,12 +295,11 @@ bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::R const std::shared_ptr& MoveItCpp::getTFBuffer() const { - return tf_buffer_; + return planning_scene_monitor_->getTFClient(); } void MoveItCpp::clearContents() { - tf_buffer_.reset(); planning_scene_monitor_.reset(); robot_model_.reset(); planning_pipelines_.clear(); diff --git a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp index d584956e6c..bbc563d24a 100644 --- a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp +++ b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp @@ -50,8 +50,7 @@ int main(int argc, char** argv) double radius = 0.02; int lifetime = 600; - std::shared_ptr tf_buffer = std::make_shared(node->get_clock()); - planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION, tf_buffer); + planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (psm.getPlanningScene()) { psm.startWorldGeometryMonitor(); diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index c275e40d40..e0ab1c3af4 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -112,8 +112,13 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost: * @param tf_buffer A pointer to a tf2_ros::Buffer * @param name A name identifying this planning scene monitor */ + [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor( + const rclcpp::Node::SharedPtr& node, const std::string& robot_description, + const std::shared_ptr& tf_buffer, const std::string& name = "") + : PlanningSceneMonitor(node, robot_description, name) + { + } PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const std::string& robot_description, - const std::shared_ptr& tf_buffer = std::shared_ptr(), const std::string& name = ""); /** @brief Constructor @@ -121,8 +126,13 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost: * @param tf_buffer A pointer to a tf2_ros::Buffer * @param name A name identifying this planning scene monitor */ + [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor( + const rclcpp::Node::SharedPtr& node, const robot_model_loader::RobotModelLoaderPtr& rml, + const std::shared_ptr& tf_buffer, const std::string& name = "") + : PlanningSceneMonitor(node, rml, name) + { + } PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const robot_model_loader::RobotModelLoaderPtr& rml, - const std::shared_ptr& tf_buffer = std::shared_ptr(), const std::string& name = ""); /** @brief Constructor @@ -131,10 +141,15 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost: * @param tf_buffer A pointer to a tf2_ros::Buffer * @param name A name identifying this planning scene monitor */ + [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor( + const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, + const std::string& robot_description, const std::shared_ptr& tf_buffer, + const std::string& name = "") + : PlanningSceneMonitor(node, scene, robot_description, name) + { + } PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, - const std::string& robot_description, - const std::shared_ptr& tf_buffer = std::shared_ptr(), - const std::string& name = ""); + const std::string& robot_description, const std::string& name = ""); /** @brief Constructor * @param scene The scene instance to maintain up to date with monitored information @@ -142,10 +157,15 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost: * @param tf_buffer A pointer to a tf2_ros::Buffer * @param name A name identifying this planning scene monitor */ + [[deprecated("Passing tf2_ros::Buffer to PlanningSceneMonitor's constructor is deprecated")]] PlanningSceneMonitor( + const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, + const robot_model_loader::RobotModelLoaderPtr& rml, const std::shared_ptr& tf_buffer, + const std::string& name = "") + : PlanningSceneMonitor(node, scene, rml, name) + { + } PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, - const robot_model_loader::RobotModelLoaderPtr& rml, - const std::shared_ptr& tf_buffer = std::shared_ptr(), - const std::string& name = ""); + const robot_model_loader::RobotModelLoaderPtr& rml, const std::string& name = ""); ~PlanningSceneMonitor(); @@ -470,6 +490,7 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost: std::thread private_executor_thread_; std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; std::string robot_description_; diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index b67bb121d5..804bd68318 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -167,15 +167,6 @@ void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topi } if (tf_buffer_ && !robot_model_->getMultiDOFJointModels().empty()) { - // If a dedicated thread is enabled for the buffer this probably means the user is adding them either through - // tf2_ros::TransformListener or themselves, so we print a warning message warning that transformCallback is doing - // the same duplicate operation - if (tf_buffer_->isUsingDedicatedThread()) - { - RCLCPP_WARN(LOGGER, "The tf2_ros::Buffer is attached to tf2_ros::TransformListener and the internal tf " - "subscribers inside CurrentStateMonitor, you may want to remove the transform listener to " - "avoid duplicate addition to the same transforms"); - } tf_buffer_->setUsingDedicatedThread(true); middleware_handle_->createDynamicTfSubscription( std::bind(&CurrentStateMonitor::transformCallback, this, std::placeholders::_1, false)); diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 8f5bc97a93..4ba32721e3 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -75,35 +75,34 @@ const std::string PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE = "get_pl const std::string PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC = "monitored_planning_scene"; PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const std::string& robot_description, - const std::shared_ptr& tf_buffer, const std::string& name) - : PlanningSceneMonitor(node, planning_scene::PlanningScenePtr(), robot_description, tf_buffer, name) + const std::string& name) + : PlanningSceneMonitor(node, planning_scene::PlanningScenePtr(), robot_description, name) { } PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, - const std::string& robot_description, - const std::shared_ptr& tf_buffer, const std::string& name) + const std::string& robot_description, const std::string& name) : PlanningSceneMonitor(node, scene, std::make_shared(node, robot_description), - tf_buffer, name) + name) { } PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const robot_model_loader::RobotModelLoaderPtr& rm_loader, - const std::shared_ptr& tf_buffer, const std::string& name) - : PlanningSceneMonitor(node, planning_scene::PlanningScenePtr(), rm_loader, tf_buffer, name) + const std::string& name) + : PlanningSceneMonitor(node, planning_scene::PlanningScenePtr(), rm_loader, name) { } PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningScenePtr& scene, const robot_model_loader::RobotModelLoaderPtr& rm_loader, - const std::shared_ptr& tf_buffer, const std::string& name) + const std::string& name) : monitor_name_(name) , node_(node) , private_executor_(std::make_shared()) - , tf_buffer_(tf_buffer) + , tf_buffer_(std::make_shared(node->get_clock())) , dt_state_update_(0.0) , shape_transform_cache_lookup_wait_time_(0, 0) , rm_loader_(rm_loader) @@ -116,6 +115,8 @@ PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, std::to_string(reinterpret_cast(this))); new_args.push_back("--"); pnode_ = std::make_shared("_", node_->get_namespace(), rclcpp::NodeOptions().arguments(new_args)); + tf_listener_ = std::make_shared(*tf_buffer_); + tf_buffer_->setUsingDedicatedThread(true); // use same callback queue as root_nh_ // root_nh_.setCallbackQueue(&queue_); // nh_.setCallbackQueue(&queue_); @@ -1100,8 +1101,6 @@ void PlanningSceneMonitor::stopSceneMonitor() bool PlanningSceneMonitor::getShapeTransformCache(const std::string& target_frame, const rclcpp::Time& target_time, occupancy_map_monitor::ShapeTransformCache& cache) const { - if (!tf_buffer_) - return false; try { boost::recursive_mutex::scoped_lock _(shape_handles_lock_); @@ -1456,9 +1455,6 @@ void PlanningSceneMonitor::getUpdatedFrameTransforms(std::vector transforms; diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h index 249c38e934..9911a9dd10 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h +++ b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h @@ -39,6 +39,7 @@ #include #include #include +#include namespace moveit { @@ -46,6 +47,9 @@ namespace planning_interface { std::shared_ptr getSharedTF(); +robot_model_loader::RobotModelLoaderPtr getSharedRobotModelLoader(const rclcpp::Node::SharedPtr& node, + const std::string& robot_description); + moveit::core::RobotModelConstPtr getSharedRobotModel(const rclcpp::Node::SharedPtr& node, const std::string& robot_description); diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp b/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp index c16d868227..7890b95f7f 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp +++ b/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp @@ -35,7 +35,6 @@ /* Author: Ioan Sucan */ #include -#include using namespace planning_scene_monitor; using namespace robot_model_loader; @@ -53,12 +52,14 @@ struct SharedStorage tf_buffer_.reset(); state_monitors_.clear(); models_.clear(); + robot_model_loaders_.clear(); } boost::mutex lock_; std::weak_ptr tf_buffer_; std::map models_; std::map state_monitors_; + std::map robot_model_loaders_; }; SharedStorage& getSharedStorage() @@ -73,22 +74,6 @@ SharedStorage& getSharedStorage() return *storage; #endif } - -// Deleter that, additionally to T*, deletes another object too -template -struct CoupledDeleter -{ - const O* other_; - CoupledDeleter(const O* other = nullptr) : other_(other) - { - } - - void operator()(const T* p) - { - delete other_; - delete p; - } -}; } // namespace namespace moveit @@ -109,6 +94,24 @@ std::shared_ptr getSharedTF() return buffer; } +robot_model_loader::RobotModelLoaderPtr getSharedRobotModelLoader(const rclcpp::Node::SharedPtr& node, + const std::string& robot_description) +{ + SharedStorage& s = getSharedStorage(); + boost::mutex::scoped_lock slock(s.lock_); + auto it = s.robot_model_loaders_ + .insert(std::make_pair(node->get_fully_qualified_name() + robot_description, + robot_model_loader::RobotModelLoaderWeakPtr())) + .first; + auto rml = it->second.lock(); + if (!rml) + { + rml = std::make_shared(node, robot_description); + it->second = rml; + } + return rml; +} + moveit::core::RobotModelConstPtr getSharedRobotModel(const rclcpp::Node::SharedPtr& node, const std::string& robot_description) { @@ -118,9 +121,7 @@ moveit::core::RobotModelConstPtr getSharedRobotModel(const rclcpp::Node::SharedP moveit::core::RobotModelPtr model = it->second.lock(); if (!model) { - RobotModelLoader::Options opt(robot_description); - opt.load_kinematics_solvers_ = true; - RobotModelLoaderPtr loader(new RobotModelLoader(node, opt)); + RobotModelLoaderPtr loader = getSharedRobotModelLoader(node, robot_description); // create an aliasing shared_ptr model = moveit::core::RobotModelPtr(loader, loader->getModel().get()); it->second = model; diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 7d1ba66a8c..b40d9bed84 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -112,8 +112,10 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /** \brief Specification of options to use when constructing the MoveGroupInterface class */ struct Options { - Options(std::string group_name, std::string desc = ROBOT_DESCRIPTION) - : group_name_(std::move(group_name)), robot_description_(std::move(desc)) + Options(std::string group_name, std::string desc = ROBOT_DESCRIPTION, std::string move_group_namespace = "") + : group_name_(std::move(group_name)) + , robot_description_(std::move(desc)) + , move_group_namespace_(std::move(move_group_namespace)) { } @@ -125,6 +127,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /// Optionally, an instance of the RobotModel to use can be also specified moveit::core::RobotModelConstPtr robot_model_; + + /// The namespace for the move group node + std::string move_group_namespace_; }; MOVEIT_STRUCT_FORWARD(Plan); diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 8af1e1b42e..bae3ba9745 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -61,6 +61,7 @@ #include #include #include +#include // TODO(JafarAbdi): Enable once moveit_ros_manipulation is ported // #include // #include @@ -143,15 +144,20 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (joint_model_group_->isChain()) end_effector_link_ = joint_model_group_->getLinkModelNames().back(); pose_reference_frame_ = getRobotModel()->getModelFrame(); - + // Append the slash between two topic components trajectory_event_publisher_ = pnode_->create_publisher( - trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC, 1); + rclcpp::names::append(opt_.move_group_namespace_, + trajectory_execution_manager::TrajectoryExecutionManager::EXECUTION_EVENT_TOPIC), + 1); attached_object_publisher_ = pnode_->create_publisher( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC, 1); + rclcpp::names::append(opt_.move_group_namespace_, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC), + 1); current_state_monitor_ = getSharedStateMonitor(node_, robot_model_, tf_buffer_); - move_action_client_ = rclcpp_action::create_client(pnode_, move_group::MOVE_ACTION); + move_action_client_ = rclcpp_action::create_client( + pnode_, rclcpp::names::append(opt_.move_group_namespace_, move_group::MOVE_ACTION)); move_action_client_->wait_for_action_server(wait_for_servers.to_chrono>()); // TODO(JafarAbdi): Enable once moveit_ros_manipulation is ported // pick_action_client_ = rclcpp_action::create_client( @@ -161,19 +167,19 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // place_action_client_ = rclcpp_action::create_client( // node_, move_group::PLACE_ACTION); // place_action_client_->wait_for_action_server(std::chrono::nanoseconds(timeout_for_servers.nanoseconds())); - execute_action_client_ = - rclcpp_action::create_client(pnode_, move_group::EXECUTE_ACTION_NAME); + execute_action_client_ = rclcpp_action::create_client( + pnode_, rclcpp::names::append(opt_.move_group_namespace_, move_group::EXECUTE_ACTION_NAME)); execute_action_client_->wait_for_action_server(wait_for_servers.to_chrono>()); - query_service_ = - pnode_->create_client(move_group::QUERY_PLANNERS_SERVICE_NAME); - get_params_service_ = - pnode_->create_client(move_group::GET_PLANNER_PARAMS_SERVICE_NAME); - set_params_service_ = - pnode_->create_client(move_group::SET_PLANNER_PARAMS_SERVICE_NAME); + query_service_ = pnode_->create_client( + rclcpp::names::append(opt_.move_group_namespace_, move_group::QUERY_PLANNERS_SERVICE_NAME)); + get_params_service_ = pnode_->create_client( + rclcpp::names::append(opt_.move_group_namespace_, move_group::GET_PLANNER_PARAMS_SERVICE_NAME)); + set_params_service_ = pnode_->create_client( + rclcpp::names::append(opt_.move_group_namespace_, move_group::SET_PLANNER_PARAMS_SERVICE_NAME)); - cartesian_path_service_ = - pnode_->create_client(move_group::CARTESIAN_PATH_SERVICE_NAME); + cartesian_path_service_ = pnode_->create_client( + rclcpp::names::append(opt_.move_group_namespace_, move_group::CARTESIAN_PATH_SERVICE_NAME)); // plan_grasps_service_ = pnode_->create_client(GRASP_PLANNING_SERVICE_NAME); @@ -214,14 +220,15 @@ class MoveGroupInterface::MoveGroupInterfaceImpl bool getInterfaceDescription(moveit_msgs::msg::PlannerInterfaceDescription& desc) { auto req = std::make_shared(); - auto response = query_service_->async_send_request(req); + auto future_response = query_service_->async_send_request(req); // wait until future is done - if (rclcpp::spin_until_future_complete(pnode_, response) == rclcpp::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(pnode_, future_response) == rclcpp::FutureReturnCode::SUCCESS) { - if (!response.get()->planner_interfaces.empty()) + const auto& response = future_response.get(); + if (!response->planner_interfaces.empty()) { - desc = response.get()->planner_interfaces.front(); + desc = response->planner_interfaces.front(); return true; } } @@ -231,12 +238,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl bool getInterfaceDescriptions(std::vector& desc) { auto req = std::make_shared(); - auto res = query_service_->async_send_request(req); - if (rclcpp::spin_until_future_complete(pnode_, res) == rclcpp::FutureReturnCode::SUCCESS) + auto future_response = query_service_->async_send_request(req); + if (rclcpp::spin_until_future_complete(pnode_, future_response) == rclcpp::FutureReturnCode::SUCCESS) { - if (!res.get()->planner_interfaces.empty()) + const auto& response = future_response.get(); + if (!response->planner_interfaces.empty()) { - desc = res.get()->planner_interfaces; + desc = response->planner_interfaces; return true; } } diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index 24bbbc6c0c..31776a76a8 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1140,8 +1140,8 @@ void MotionPlanningDisplay::onRobotModelLoaded() PlanningSceneDisplay::onRobotModelLoaded(); trajectory_visual_->onRobotModelLoaded(getRobotModel()); - robot_interaction_.reset( - new robot_interaction::RobotInteraction(getRobotModel(), node_, "rviz_moveit_motion_planning_display")); + robot_interaction_.reset(new robot_interaction::RobotInteraction( + getRobotModel(), node_, rclcpp::names::append(getMoveGroupNS(), "rviz_moveit_motion_planning_display"))); robot_interaction::KinematicOptions o; o.state_validity_callback_ = boost::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index fa812551f0..ee3613d9d4 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -381,7 +381,8 @@ void MotionPlanningFrame::changePlanningGroupHelper() return; RCLCPP_INFO(LOGGER, "Constructing new MoveGroup connection for group '%s' in namespace '%s'", group.c_str(), planning_display_->getMoveGroupNS().c_str()); - moveit::planning_interface::MoveGroupInterface::Options opt(group); + moveit::planning_interface::MoveGroupInterface::Options opt( + group, moveit::planning_interface::MoveGroupInterface::ROBOT_DESCRIPTION, planning_display_->getMoveGroupNS()); opt.robot_model_ = robot_model; opt.robot_description_.clear(); try diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 49aa813457..fba9b2cff4 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -518,9 +518,9 @@ void PlanningSceneDisplay::unsetLinkColor(rviz_default_plugins::robot::Robot* ro // ****************************************************************************************** planning_scene_monitor::PlanningSceneMonitorPtr PlanningSceneDisplay::createPlanningSceneMonitor() { - std::shared_ptr tf_buffer = moveit::planning_interface::getSharedTF(); - return std::make_shared( - node_, robot_description_property_->getStdString(), tf_buffer, getNameStd() + "_planning_scene_monitor"); + auto rml = moveit::planning_interface::getSharedRobotModelLoader(node_, robot_description_property_->getStdString()); + return std::make_shared(node_, rml, + getNameStd() + "_planning_scene_monitor"); } void PlanningSceneDisplay::clearRobotModel() diff --git a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp index 77ea943120..7712fa0abd 100644 --- a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp @@ -145,11 +145,7 @@ int main(int argc, char** argv) if (!conn->connect()) return 1; - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - std::shared_ptr tf_buffer = std::make_shared(clock); - std::shared_ptr tf_listener = - std::make_shared(*tf_buffer, node); - planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION, tf_buffer); + planning_scene_monitor::PlanningSceneMonitor psm(node, ROBOT_DESCRIPTION); if (!psm.getPlanningScene()) { RCLCPP_ERROR(LOGGER, "Unable to initialize PlanningSceneMonitor");