From 34a9d59e02b46e9d2ca6bcede1b9be120f337f06 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 6 Dec 2023 15:51:17 +0000 Subject: [PATCH 01/17] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20reproducib?= =?UTF-8?q?le-containers/buildkit-cache-dance=20from=202.1.2=20to=202.1.3?= =?UTF-8?q?=20(#2486)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bumps [reproducible-containers/buildkit-cache-dance](https://github.com/reproducible-containers/buildkit-cache-dance) from 2.1.2 to 2.1.3. - [Release notes](https://github.com/reproducible-containers/buildkit-cache-dance/releases) - [Commits](https://github.com/reproducible-containers/buildkit-cache-dance/compare/v2.1.2...v2.1.3) --- updated-dependencies: - dependency-name: reproducible-containers/buildkit-cache-dance dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Sebastian Jahr --- .github/workflows/tutorial_docker.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/tutorial_docker.yaml b/.github/workflows/tutorial_docker.yaml index 4eb2156397..9aa63ddfd2 100644 --- a/.github/workflows/tutorial_docker.yaml +++ b/.github/workflows/tutorial_docker.yaml @@ -50,7 +50,7 @@ jobs: path: .ccache key: docker-tutorial-ccache-${{ matrix.ROS_DISTRO }}-${{ hashFiles( '.docker/tutorial-source/Dockerfile' ) }} - name: inject ccache into docker - uses: reproducible-containers/buildkit-cache-dance@v2.1.2 + uses: reproducible-containers/buildkit-cache-dance@v2.1.3 with: cache-source: .ccache cache-target: /root/.ccache/ From 6eca49eab05d20301563d93106462c5f1b55cdaa Mon Sep 17 00:00:00 2001 From: Stephanie Eng Date: Wed, 6 Dec 2023 11:51:45 -0500 Subject: [PATCH 02/17] Pass more distance information out from FCL collision check (#2572) * Pass more distance information out from FCL collision check * Update moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h Co-authored-by: Sebastian Jahr * Update moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h --------- Co-authored-by: Sebastian Jahr --- .../collision_detection/collision_common.h | 78 ++++++++++--------- .../src/collision_env_fcl.cpp | 8 ++ 2 files changed, 51 insertions(+), 35 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h index 1b6ff542b0..db246518fd 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h @@ -141,41 +141,7 @@ struct CostSource } }; -/** \brief Representation of a collision checking result */ -struct CollisionResult -{ - using ContactMap = std::map, std::vector >; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** \brief Clear a previously stored result */ - void clear() - { - collision = false; - distance = std::numeric_limits::max(); - contact_count = 0; - contacts.clear(); - cost_sources.clear(); - } - - /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */ - void print() const; - - /** \brief True if collision was found, false otherwise */ - bool collision = false; - - /** \brief Closest distance between two bodies */ - double distance = std::numeric_limits::max(); - - /** \brief Number of contacts returned */ - std::size_t contact_count = 0; - - /** \brief A map returning the pairs of body ids in contact, plus their contact details */ - ContactMap contacts; - - /** \brief These are the individual cost sources when costs are computed */ - std::set cost_sources; -}; +struct CollisionResult; /** \brief Representation of a collision checking request */ struct CollisionRequest @@ -187,6 +153,9 @@ struct CollisionRequest /** \brief If true, compute proximity distance */ bool distance = false; + /** \brief If true, return detailed distance information. Distance must be set to true as well */ + bool detailed_distance = false; + /** \brief If true, a collision cost is computed */ bool cost = false; @@ -354,4 +323,43 @@ struct DistanceResult distances.clear(); } }; + +/** \brief Representation of a collision checking result */ +struct CollisionResult +{ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** \brief Clear a previously stored result */ + void clear() + { + collision = false; + distance = std::numeric_limits::max(); + distance_result.clear(); + contact_count = 0; + contacts.clear(); + cost_sources.clear(); + } + + /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */ + void print() const; + + /** \brief True if collision was found, false otherwise */ + bool collision = false; + + /** \brief Closest distance between two bodies */ + double distance = std::numeric_limits::max(); + + /** \brief Distance data for each link */ + DistanceResult distance_result; + + /** \brief Number of contacts returned */ + std::size_t contact_count = 0; + + /** \brief A map returning the pairs of body ids in contact, plus their contact details */ + using ContactMap = std::map, std::vector >; + ContactMap contacts; + + /** \brief These are the individual cost sources when costs are computed */ + std::set cost_sources; +}; } // namespace collision_detection diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index e5b01de84f..e1406dc499 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -290,6 +290,10 @@ void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, Coll dreq.enableGroup(getRobotModel()); distanceSelf(dreq, dres, state); res.distance = dres.minimum_distance.distance; + if (req.detailed_distance) + { + res.distance_result = dres; + } } } @@ -343,6 +347,10 @@ void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, Col dreq.enableGroup(getRobotModel()); distanceRobot(dreq, dres, state); res.distance = dres.minimum_distance.distance; + if (req.detailed_distance) + { + res.distance_result = dres; + } } } From 1d2bf44449e25e0d7ede28a8aa87107cdcec1b21 Mon Sep 17 00:00:00 2001 From: Peter David Fagan Date: Wed, 6 Dec 2023 20:17:43 +0000 Subject: [PATCH 03/17] init policy class (#2494) update command interfaces Update moveit_py/moveit/policies/policy.py Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Update moveit_py/moveit/policies/policy.py Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Update moveit_py/moveit/policies/policy.py Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Update moveit_py/moveit/policies/policy.py Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Update moveit_py/moveit/policies/policy.py Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Update moveit_py/moveit/policies/policy.py Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> update script description --- moveit_py/moveit/policies/__init__.py | 1 + moveit_py/moveit/policies/policy.py | 151 ++++++++++++++++++++++++++ 2 files changed, 152 insertions(+) create mode 100644 moveit_py/moveit/policies/__init__.py create mode 100644 moveit_py/moveit/policies/policy.py diff --git a/moveit_py/moveit/policies/__init__.py b/moveit_py/moveit/policies/__init__.py new file mode 100644 index 0000000000..53e7dff01b --- /dev/null +++ b/moveit_py/moveit/policies/__init__.py @@ -0,0 +1 @@ +from .policy import Policy diff --git a/moveit_py/moveit/policies/policy.py b/moveit_py/moveit/policies/policy.py new file mode 100644 index 0000000000..24e1f70a5a --- /dev/null +++ b/moveit_py/moveit/policies/policy.py @@ -0,0 +1,151 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2023, Peter David Fagan +# 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 Willow Garage, 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: Peter David Fagan + +""" +Definition of an abstract base class for policy deployment. +For now, this policy only supports moveit_servo command interfaces and Image sensors. +""" + +from abc import ABC, abstractmethod + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile + +from control_msgs.msg import JointJog +from geometry_msgs.msg import PoseStamped, Twist +from sensor_msgs.msg import Image + +from std_srvs.srv import SetBool + +import message_filters + + +class Policy(ABC, Node): + """An abstract base class for deploying learnt policies.""" + + def __init__(self, params, node_name="policy_node"): + """Initialise the policy.""" + super().__init__(node_name) + self.logger = self.get_logger() + + # parse parameters + self.param_listener = params.ParamListener(self) + self.params = self.param_listener.get_params() + + # set policy to inactive by default + self._is_active = False + self.activate_policy = self.create_service( + SetBool, + "activate_policy", + self.activate_policy, + ) + + # register sensor topics + self.register_sensors() + + # register servo command topic + self.register_command() + + @property + def is_active(self): + """Returns True if the policy is active.""" + return self._is_active + + @is_active.setter + def active(self, value): + """Sets the policy to active state via Python API.""" + self._is_active = value + + def activate_policy(self, request, response): + """Sets the policy to active state via ROS interface.""" + self._is_active = request.data + return response + + def get_sensor_msg_type(self, msg_type): + """Returns the ROS 2 message type for a given sensor type.""" + if msg_type == "sensor_msgs/Image": + return Image + else: + raise ValueError(f"Sensor type {sensor_type} not supported.") + + def get_command_msg_type(self, msg_type): + """Returns the ROS 2 message type for a given command type.""" + if msg_type == "geometry_msgs/PoseStamped": + return PoseStamped + elif msg_type == "geometry_msgs/Twist": + return Twist + elif msg_type == "control_msgs/JointJog": + return JointJog + else: + raise ValueError(f"Command type {msg_type} not supported.") + + def register_sensors(self): + """Register the topics to listen to for sensor data.""" + self.sensor_subs = [] + # TODO: refactor this section + # Related Issue: https://github.com/PickNikRobotics/generate_parameter_library/issues/155 + for sensor_idx in range(self.params.num_sensors): + sensor_params = self.get_parameters_by_prefix(f"sensor{sensor_idx + 1}") + self.sensor_subs.append( + message_filters.Subscriber( + self, + self.get_sensor_msg_type(sensor_params["type"].value), + str(sensor_params["topic"].value), + qos_profile=QoSProfile(depth=sensor_params["qos"].value), + ) + ) + + # create a synchronizer for the sensor topics + self.sensor_sync = message_filters.ApproximateTimeSynchronizer( + self.sensor_subs, + self.params.sensor_queue, + self.params.sensor_slop, + ) + + # register model forward pass as callback + self.sensor_sync.registerCallback(self.forward) + + def register_command(self): + """Register the topic to publish actions to.""" + self.command_pub = self.create_publisher( + self.get_command_msg_type(self.params.command.type), + self.params.command.topic, + QoSProfile(depth=self.params.command.qos), + ) + + @abstractmethod + def forward(): + """Perform a forward pass of the policy.""" + pass From f596b9df89c07805f5b911936486ac9f165543b0 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Wed, 6 Dec 2023 15:10:30 -0700 Subject: [PATCH 04/17] [Planning Pipeline Refactoring] #2 Enable chaining planners (#2457) * Enable chaining multiple planners --- MIGRATION.md | 7 + .../default_configs/chomp_planning.yaml | 3 +- .../default_configs/ompl_planning.yaml | 3 +- ...lz_industrial_motion_planner_planning.yaml | 12 +- .../default_configs/stomp_planning.yaml | 4 +- .../planning_interface/planning_interface.h | 4 +- ...est_pilz_industrial_motion_planner.test.py | 6 +- ...nittest_pilz_industrial_motion_planner.cpp | 10 +- .../stomp/src/stomp_moveit_planner_plugin.cpp | 6 - .../prbt_moveit_config/launch/demo.launch.py | 16 +- .../moveit/benchmarks/BenchmarkExecutor.h | 5 +- .../benchmarks/src/BenchmarkExecutor.cpp | 46 +---- .../src/moveit_planning_pipeline.cpp | 2 +- .../test/launch/hybrid_planning_common.py | 14 +- .../query_planners_service_capability.cpp | 178 +++++++++++------- .../query_planners_service_capability.h | 9 +- .../move_group/src/move_group_context.cpp | 8 +- .../planning_pipeline/planning_pipeline.h | 50 +++-- .../res/planning_pipeline_parameters.yaml | 6 +- .../src/planning_pipeline.cpp | 136 +++++++++---- .../test/planning_pipeline_test_plugins.cpp | 4 + .../test/planning_pipeline_tests.cpp | 20 +- .../planning_pipeline_interfaces.hpp | 3 - .../src/planning_pipeline_interfaces.cpp | 2 +- 24 files changed, 336 insertions(+), 218 deletions(-) diff --git a/MIGRATION.md b/MIGRATION.md index 04baab23c5..3d92437e69 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,6 +3,13 @@ API changes in MoveIt releases ## ROS Rolling +- [10/2023] The planning pipeline now has a vector of planner plugins rather than a single one. Please update the planner plugin parameter e.g. like this: +```diff +- planning_plugin: ompl_interface/OMPLPlanner ++ planning_plugins: ++ - ompl_interface/OMPLPlanner +``` + - [10/2023] Planning request adapters are now separated into PlanRequest (preprocessing) and PlanResponse (postprocessing) adapters. The adapters are configured with ROS parameter vectors (vector order corresponds to execution order). Please update your pipeline configurations for example like this: ```diff - request_adapters: >- diff --git a/moveit_configs_utils/default_configs/chomp_planning.yaml b/moveit_configs_utils/default_configs/chomp_planning.yaml index b632d65c05..07c1f32c46 100644 --- a/moveit_configs_utils/default_configs/chomp_planning.yaml +++ b/moveit_configs_utils/default_configs/chomp_planning.yaml @@ -1,4 +1,5 @@ -planning_plugin: chomp_interface/CHOMPPlanner +planning_plugins: + - chomp_interface/CHOMPPlanner enable_failure_recovery: true # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. request_adapters: diff --git a/moveit_configs_utils/default_configs/ompl_planning.yaml b/moveit_configs_utils/default_configs/ompl_planning.yaml index 9c02c44bdd..3bb0acfc8a 100644 --- a/moveit_configs_utils/default_configs/ompl_planning.yaml +++ b/moveit_configs_utils/default_configs/ompl_planning.yaml @@ -1,4 +1,5 @@ -planning_plugin: ompl_interface/OMPLPlanner +planning_plugins: + - ompl_interface/OMPLPlanner # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. request_adapters: - default_planning_request_adapters/ResolveConstraintFrames diff --git a/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml b/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml index 9925a81a8b..d0a07ce4c2 100644 --- a/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml +++ b/moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml @@ -1,6 +1,14 @@ -planning_plugin: pilz_industrial_motion_planner/CommandPlanner -# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +planning_plugins: + - pilz_industrial_motion_planner/CommandPlanner default_planner_config: PTP +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath capabilities: >- pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService diff --git a/moveit_configs_utils/default_configs/stomp_planning.yaml b/moveit_configs_utils/default_configs/stomp_planning.yaml index 38120524cb..e811ac9e91 100644 --- a/moveit_configs_utils/default_configs/stomp_planning.yaml +++ b/moveit_configs_utils/default_configs/stomp_planning.yaml @@ -1,5 +1,5 @@ -planning_plugin: stomp_moveit/StompPlanner -# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +planning_plugins: + - stomp_moveit/StompPlanner request_adapters: - default_planning_request_adapters/ResolveConstraintFrames - default_planning_request_adapters/ValidateWorkspaceBounds diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index c41c8505ab..76bb51bb3e 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -164,8 +164,8 @@ class PlannerManager virtual bool initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace); - /// Get \brief a short string that identifies the planning interface - virtual std::string getDescription() const; + /// \brief Get a short string that identifies the planning interface. + virtual std::string getDescription() const = 0; /// \brief Get the names of the known planning algorithms (values that can be filled as planner_id in the planning /// request) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py index 225fcbc58e..aa2dd03b64 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/launch/unittest_pilz_industrial_motion_planner.test.py @@ -18,8 +18,8 @@ def generate_test_description(): # Load the context test_config = load_moveit_config() - planning_plugin = { - "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner" + planning_plugins = { + "planning_plugins": ["pilz_industrial_motion_planner/CommandPlanner"] } # run test @@ -29,7 +29,7 @@ def generate_test_description(): name="unittest_pilz_industrial_motion_planner", parameters=[ test_config.to_dict(), - planning_plugin, + planning_plugins, ], output="screen", ) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp index aecedafed3..24493d2cdc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp @@ -74,8 +74,8 @@ class CommandPlannerTest : public testing::Test ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; // Load planner name from node parameters - ASSERT_TRUE(node_->has_parameter("planning_plugin")) << "Could not find parameter 'planning_plugin'"; - node_->get_parameter("planning_plugin", planner_plugin_name_); + ASSERT_TRUE(node_->has_parameter("planning_plugins")) << "Could not find parameter 'planning_plugins'"; + node_->get_parameter>("planning_plugins", planner_plugin_names_); // Load the plugin try @@ -92,7 +92,7 @@ class CommandPlannerTest : public testing::Test // Create planner try { - planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_name_)); + planner_instance_.reset(planner_plugin_loader_->createUnmanagedInstance(planner_plugin_names_.at(0))); ASSERT_TRUE(planner_instance_->initialize(robot_model_, node_, "")) << "Initializing the planner instance failed."; } @@ -104,7 +104,7 @@ class CommandPlannerTest : public testing::Test void TearDown() override { - planner_plugin_loader_->unloadLibraryForClass(planner_plugin_name_); + planner_plugin_loader_->unloadLibraryForClass(planner_plugin_names_.at(0)); robot_model_.reset(); } @@ -114,7 +114,7 @@ class CommandPlannerTest : public testing::Test moveit::core::RobotModelConstPtr robot_model_; std::unique_ptr rm_loader_; - std::string planner_plugin_name_; + std::vector planner_plugin_names_; std::unique_ptr> planner_plugin_loader_; planning_interface::PlannerManagerPtr planner_instance_; }; diff --git a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp index e32528f2db..82b964065a 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp @@ -118,12 +118,6 @@ class StompPlannerManager : public PlannerManager RCLCPP_ERROR(LOGGER, "Invalid joint group '%s'", req.group_name.c_str()); return false; } - - if (!req.reference_trajectories.empty()) - { - RCLCPP_WARN(LOGGER, "Ignoring reference trajectories - not implemented!"); - } - return true; } diff --git a/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py b/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py index d9a053f1e8..a71023ecc5 100644 --- a/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py +++ b/moveit_planners/test_configs/prbt_moveit_config/launch/demo.launch.py @@ -81,12 +81,20 @@ def generate_launch_description(): "default_planning_pipeline": "ompl", "planning_pipelines": ["pilz", "ompl"], "pilz": { - "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner", - "request_adapters": "", + "planning_plugins": ["pilz_industrial_motion_planner/CommandPlanner"], }, "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision default_planning_request_adapters/FixStartStatePathConstraints""", + "planning_plugins": ["ompl_interface/OMPLPlanner"], + "request_adapters": [ + "default_planning_request_adapters/ResolveConstraintFrames", + "default_planning_request_adapters/ValidateWorkspaceBounds", + "default_planning_request_adapters/CheckStartStateBounds", + "default_planning_request_adapters/CheckStartStateCollision", + ], + "response_adapters": [ + "default_planning_response_adapters/AddTimeOptimalParameterization", + "default_planning_response_adapters/ValidateSolution", + ], }, } ompl_planning_yaml = load_yaml( diff --git a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h index 9820ba4791..6a0be17731 100644 --- a/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h +++ b/moveit_ros/benchmarks/include/moveit/benchmarks/BenchmarkExecutor.h @@ -169,9 +169,8 @@ class BenchmarkExecutor void shiftConstraintsByOffset(moveit_msgs::msg::Constraints& constraints, const std::vector& offset); - /// Check that the desired planner plugins and algorithms exist for the given group - bool plannerConfigurationsExist(const std::map>& planners, - const std::string& group_name); + /// Check that the desired planning pipelines exist + bool pipelinesExist(const std::map>& planners); /// Load the planning scene with the given name from the warehouse bool loadPlanningScene(const std::string& scene_name, moveit_msgs::msg::PlanningScene& scene_msg); diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 09c811d2c9..f108fec0db 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -121,7 +121,7 @@ BenchmarkExecutor::~BenchmarkExecutor() const auto& pipeline = moveit_cpp_->getPlanningPipelines().at(planning_pipeline_name); // Verify the pipeline has successfully initialized a planner - if (!pipeline->getPlannerManager()) + if (!pipeline) { RCLCPP_ERROR(getLogger(), "Failed to initialize planning pipeline '%s'", planning_pipeline_name.c_str()); continue; @@ -139,8 +139,7 @@ BenchmarkExecutor::~BenchmarkExecutor() for (const std::pair& entry : moveit_cpp_->getPlanningPipelines()) { - RCLCPP_INFO_STREAM(getLogger(), - "Pipeline: " << entry.first << ", Planner: " << entry.second->getPlannerPluginName()); + RCLCPP_INFO_STREAM(getLogger(), entry.first); } } return true; @@ -267,7 +266,7 @@ bool BenchmarkExecutor::initializeBenchmarks(const BenchmarkOptions& options, moveit_msgs::msg::PlanningScene& scene_msg, std::vector& requests) { - if (!plannerConfigurationsExist(options.planning_pipelines, options.group_name)) + if (!pipelinesExist(options.planning_pipelines)) { return false; } @@ -528,8 +527,7 @@ void BenchmarkExecutor::createRequestCombinations(const BenchmarkRequest& benchm } } -bool BenchmarkExecutor::plannerConfigurationsExist( - const std::map>& pipeline_configurations, const std::string& group_name) +bool BenchmarkExecutor::pipelinesExist(const std::map>& pipeline_configurations) { // Make sure planner plugins exist for (const std::pair>& pipeline_config_entry : pipeline_configurations) @@ -549,42 +547,6 @@ bool BenchmarkExecutor::plannerConfigurationsExist( return false; } } - - // Make sure planners exist within those pipelines - auto planning_pipelines = moveit_cpp_->getPlanningPipelines(); - for (const std::pair>& entry : pipeline_configurations) - { - planning_interface::PlannerManagerPtr pm = planning_pipelines[entry.first]->getPlannerManager(); - const planning_interface::PlannerConfigurationMap& config_map = pm->getPlannerConfigurations(); - - // if the planner is chomp or stomp skip this function and return true for checking planner configurations for the - // planning group otherwise an error occurs, because for OMPL a specific planning algorithm needs to be defined for - // a planning group, whereas with STOMP and CHOMP this is not necessary - if (pm->getDescription().compare("stomp") || pm->getDescription().compare("chomp")) - continue; - - for (std::size_t i = 0; i < entry.second.size(); ++i) - { - bool planner_exists = false; - for (const std::pair& config_entry : - config_map) - { - std::string planner_name = group_name + "[" + entry.second[i] + "]"; - planner_exists = (config_entry.second.group == group_name && config_entry.second.name == planner_name); - } - - if (!planner_exists) - { - RCLCPP_ERROR(getLogger(), "Planner '%s' does NOT exist for group '%s' in pipeline '%s'", - entry.second[i].c_str(), group_name.c_str(), entry.first.c_str()); - std::cout << "There are " << config_map.size() << " planner entries: " << '\n'; - for (const auto& config_map_entry : config_map) - std::cout << config_map_entry.second.name << '\n'; - return false; - } - } - } - return true; } diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp index 5a487e8708..d93bfde272 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/src/moveit_planning_pipeline.cpp @@ -60,7 +60,7 @@ bool MoveItPlanningPipeline::initialize(const rclcpp::Node::SharedPtr& node) node->declare_parameter(PLAN_REQUEST_PARAM_NS + "planning_time", 1.0); node->declare_parameter(PLAN_REQUEST_PARAM_NS + "max_velocity_scaling_factor", 1.0); node->declare_parameter(PLAN_REQUEST_PARAM_NS + "max_acceleration_scaling_factor", 1.0); - node->declare_parameter("ompl.planning_plugin", "ompl_interface/OMPLPlanner"); + node->declare_parameter>("ompl.planning_plugins", { "ompl_interface/OMPLPlanner" }); // Planning Scene options node->declare_parameter(PLANNING_SCENE_MONITOR_NS + "name", UNDEFINED); diff --git a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py index 5beba7753b..f88681dbc3 100644 --- a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py +++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py @@ -70,8 +70,18 @@ def generate_common_hybrid_launch_description(): # The global planner uses the typical OMPL parameters planning_pipelines_config = { "ompl": { - "planning_plugin": "ompl_interface/OMPLPlanner", - "request_adapters": """default_planning_request_adapters/AddTimeOptimalParameterization default_planning_request_adapters/ValidateWorkspaceBounds default_planning_request_adapters/CheckStartStateBounds default_planning_request_adapters/CheckStartStateCollision""", + "planning_plugins": ["ompl_interface/OMPLPlanner"], + "request_adapters": [ + "default_planning_request_adapters/ResolveConstraintFrames", + "default_planning_request_adapters/ValidateWorkspaceBounds", + "default_planning_request_adapters/CheckStartStateBounds", + "default_planning_request_adapters/CheckStartStateCollision", + ], + "response_adapters": [ + "default_planning_response_adapters/AddTimeOptimalParameterization", + "default_planning_response_adapters/ValidateSolution", + "default_planning_response_adapters/DisplayMotionPath", + ], } } ompl_planning_yaml = load_yaml( diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index 76cf27ef1e..1bdc92881e 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp @@ -38,9 +38,17 @@ #include #include #include +#include namespace move_group { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("query_planners_service_capability"); +} +} // namespace MoveGroupQueryPlannersService::MoveGroupQueryPlannersService() : MoveGroupCapability("query_planners_service") { } @@ -49,118 +57,152 @@ void MoveGroupQueryPlannersService::initialize() { query_service_ = context_->moveit_cpp_->getNode()->create_service( QUERY_PLANNERS_SERVICE_NAME, - [this](const std::shared_ptr& request_header, - const std::shared_ptr& req, + [this](const std::shared_ptr& req, const std::shared_ptr& res) { - return queryInterface(request_header, req, res); + queryInterface(req, res); }); get_service_ = context_->moveit_cpp_->getNode()->create_service( - GET_PLANNER_PARAMS_SERVICE_NAME, [this](const std::shared_ptr& request_header, - const std::shared_ptr& req, - const std::shared_ptr& res) { - return getParams(request_header, req, res); - }); + GET_PLANNER_PARAMS_SERVICE_NAME, + [this](const std::shared_ptr& req, + const std::shared_ptr& res) { getParams(req, res); }); set_service_ = context_->moveit_cpp_->getNode()->create_service( - SET_PLANNER_PARAMS_SERVICE_NAME, [this](const std::shared_ptr& request_header, - const std::shared_ptr& req, - const std::shared_ptr& res) { - return setParams(request_header, req, res); - }); + SET_PLANNER_PARAMS_SERVICE_NAME, + [this](const std::shared_ptr& req, + const std::shared_ptr& res) { setParams(req, res); }); } -bool MoveGroupQueryPlannersService::queryInterface( - const std::shared_ptr& /* unused */, - const std::shared_ptr& /*req*/, +void MoveGroupQueryPlannersService::queryInterface( + const std::shared_ptr& /* unused */, const std::shared_ptr& res) { for (const auto& planning_pipelines : context_->moveit_cpp_->getPlanningPipelines()) { - const auto& pipeline_id = planning_pipelines.first; const auto& planning_pipeline = planning_pipelines.second; - const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); - if (planner_interface) + + // TODO(sjahr): Update for multiple planner plugins + const auto& planner_plugin_names = planning_pipeline->getPlannerPluginNames(); + if (planner_plugin_names.empty()) + { + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not have any planner plugins.", planning_pipelines.first.c_str()); + return; + } + const auto planner_interface = planning_pipeline->getPlannerManager(planner_plugin_names.at(0)); + if (!planner_interface) { - std::vector algs; - planner_interface->getPlanningAlgorithms(algs); - moveit_msgs::msg::PlannerInterfaceDescription pi_desc; - pi_desc.name = planner_interface->getDescription(); - pi_desc.pipeline_id = pipeline_id; - planner_interface->getPlanningAlgorithms(pi_desc.planner_ids); - res->planner_interfaces.push_back(pi_desc); + RCLCPP_ERROR(getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", + planner_plugin_names.at(0).c_str(), planning_pipelines.first.c_str()); } + std::vector algs; + planner_interface->getPlanningAlgorithms(algs); + moveit_msgs::msg::PlannerInterfaceDescription pi_desc; + pi_desc.name = planner_interface->getDescription(); + pi_desc.pipeline_id = planning_pipelines.first; + planner_interface->getPlanningAlgorithms(pi_desc.planner_ids); + res->planner_interfaces.push_back(pi_desc); } - return true; } -bool MoveGroupQueryPlannersService::getParams(const std::shared_ptr& /* unused */, - const std::shared_ptr& req, +void MoveGroupQueryPlannersService::getParams(const std::shared_ptr& req, const std::shared_ptr& res) { const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); if (!planning_pipeline) - return false; + { + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not exist.", req->pipeline_id.c_str()); + return; + } - const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); - if (planner_interface) + // TODO(sjahr): Update for multiple planner plugins + const auto& planner_plugin_names = planning_pipeline->getPlannerPluginNames(); + if (planner_plugin_names.empty()) + { + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not have any planner plugins.", req->pipeline_id.c_str()); + return; + } + const auto planner_interface = planning_pipeline->getPlannerManager(planner_plugin_names.at(0)); + if (!planner_interface) { - std::map config; + RCLCPP_ERROR(getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", + planner_plugin_names.at(0).c_str(), req->pipeline_id.c_str()); + } + std::map config; - const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations(); + const planning_interface::PlannerConfigurationMap& configs = planner_interface->getPlannerConfigurations(); - planning_interface::PlannerConfigurationMap::const_iterator it = - configs.find(req->planner_config); // fetch default params first + planning_interface::PlannerConfigurationMap::const_iterator it = + configs.find(req->planner_config); // fetch default params first + if (it != configs.end()) + { + config.insert(it->second.config.begin(), it->second.config.end()); + } + + if (!req->group.empty()) + { // merge in group-specific params + it = configs.find(req->group + "[" + req->planner_config + "]"); if (it != configs.end()) + { config.insert(it->second.config.begin(), it->second.config.end()); - - if (!req->group.empty()) - { // merge in group-specific params - it = configs.find(req->group + "[" + req->planner_config + "]"); - if (it != configs.end()) - config.insert(it->second.config.begin(), it->second.config.end()); } + } - for (const auto& key_value_pair : config) - { - res->params.keys.push_back(key_value_pair.first); - res->params.values.push_back(key_value_pair.second); - } + for (const auto& key_value_pair : config) + { + res->params.keys.push_back(key_value_pair.first); + res->params.values.push_back(key_value_pair.second); } - return true; } -bool MoveGroupQueryPlannersService::setParams( - const std::shared_ptr& /* unused */, +void MoveGroupQueryPlannersService::setParams( const std::shared_ptr& req, const std::shared_ptr& /*res*/) { if (req->params.keys.size() != req->params.values.size()) - return false; + { + RCLCPP_ERROR(getLogger(), "Number of parameter names does not match the number of parameters"); + return; + } const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(req->pipeline_id); if (!planning_pipeline) - return false; + { + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not exist.", req->pipeline_id.c_str()); + return; + } - const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline->getPlannerManager(); + // TODO(sjahr): Update for multiple planner plugins + const auto& planner_plugin_names = planning_pipeline->getPlannerPluginNames(); + if (planner_plugin_names.empty()) + { + RCLCPP_ERROR(getLogger(), "Pipeline '%s' does not have any planner plugins.", req->pipeline_id.c_str()); + return; + } + auto planner_interface = planning_pipeline->getPlannerManager(planner_plugin_names.at(0)); + if (!planner_interface) + { + RCLCPP_ERROR(getLogger(), "Requesting planner '%s' from '%s' returned a null pointer.", + planner_plugin_names.at(0).c_str(), req->pipeline_id.c_str()); + return; + } - if (planner_interface) + planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations(); + const std::string config_name = + req->group.empty() ? req->planner_config : req->group + "[" + req->planner_config + "]"; + + planning_interface::PlannerConfigurationSettings& config = configs[config_name]; + config.group = req->group; + config.name = config_name; + if (req->replace) + { + config.config.clear(); + } + for (unsigned int i = 0, end = req->params.keys.size(); i < end; ++i) { - planning_interface::PlannerConfigurationMap configs = planner_interface->getPlannerConfigurations(); - const std::string config_name = - req->group.empty() ? req->planner_config : req->group + "[" + req->planner_config + "]"; - - planning_interface::PlannerConfigurationSettings& config = configs[config_name]; - config.group = req->group; - config.name = config_name; - if (req->replace) - config.config.clear(); - for (unsigned int i = 0, end = req->params.keys.size(); i < end; ++i) - config.config[req->params.keys[i]] = req->params.values[i]; - - planner_interface->setPlannerConfigurations(configs); + config.config[req->params.keys.at(i)] = req->params.values.at(i); } - return true; + + planner_interface->setPlannerConfigurations(configs); } } // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h index 00103c17b5..8fa02ea1ef 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.h @@ -51,15 +51,12 @@ class MoveGroupQueryPlannersService : public MoveGroupCapability void initialize() override; private: - bool queryInterface(const std::shared_ptr& request_header, - const std::shared_ptr& /*req*/, + void queryInterface(const std::shared_ptr& /*req*/, const std::shared_ptr& res); - bool getParams(const std::shared_ptr& request_header, - const std::shared_ptr& req, + void getParams(const std::shared_ptr& req, const std::shared_ptr& res); - bool setParams(const std::shared_ptr& request_header, - const std::shared_ptr& req, + void setParams(const std::shared_ptr& req, const std::shared_ptr& /*res*/); rclcpp::Service::SharedPtr query_service_; diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index cacc9c86df..df35342e66 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -92,18 +92,16 @@ MoveGroupContext::~MoveGroupContext() bool MoveGroupContext::status() const { - const planning_interface::PlannerManagerPtr& planner_interface = planning_pipeline_->getPlannerManager(); - if (planner_interface) + if (planning_pipeline_) { - RCLCPP_INFO_STREAM(getLogger(), - "MoveGroup context using planning plugin " << planning_pipeline_->getPlannerPluginName()); + RCLCPP_INFO_STREAM(getLogger(), "MoveGroup context using pipeline " << planning_pipeline_->getName().c_str()); RCLCPP_INFO_STREAM(getLogger(), "MoveGroup context initialization complete"); return true; } else { RCLCPP_WARN_STREAM(getLogger(), - "MoveGroup running was unable to load " << planning_pipeline_->getPlannerPluginName()); + "MoveGroup running was unable to load pipeline " << planning_pipeline_->getName().c_str()); return false; } } diff --git a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h index 99f17b42fe..89115bbdb6 100644 --- a/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h +++ b/moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h @@ -118,14 +118,13 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline motion planning pipeline. \param model The robot model for which this pipeline is initialized. \param node The ROS node that should be used for reading parameters needed for configuration - \param parameter_namespace Parameter - namespace where the planner configurations are stored - \param request_adapter_plugin_names Optional vector of - RequestAdapter plugin names + \param parameter_namespace Parameter namespace where the planner configurations are stored + \param planner_plugin_names Names of the planner plugins to use + \param request_adapter_plugin_names Optional vector of RequestAdapter plugin names \param response_adapter_plugin_names Optional vector of ResponseAdapter plugin names */ PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, - const std::string& parameter_namespace, const std::string& planning_plugin_name, + const std::string& parameter_namespace, const std::vector& planner_plugin_names, const std::vector& request_adapter_plugin_names = std::vector(), const std::vector& response_adapter_plugin_names = std::vector()); @@ -163,6 +162,16 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline { return false; } + [[deprecated("Please use getPlannerPluginNames().")]] const std::string& getPlannerPluginName() const + { + return pipeline_parameters_.planning_plugins.at(0); + } + [[deprecated( + "Please use 'getPlannerManager(const std::string& planner_name)'.")]] const planning_interface::PlannerManagerPtr& + getPlannerManager() + { + return planner_map_.at(pipeline_parameters_.planning_plugins.at(0)); + } /* END BLOCK OF DEPRECATED FUNCTIONS */ @@ -180,10 +189,10 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline /** \brief Request termination, if a generatePlan() function is currently computing plans */ void terminate() const; - /** \brief Get the name of the planning plugin used */ - [[nodiscard]] const std::string& getPlannerPluginName() const + /** \brief Get the names of the planning plugins used */ + [[nodiscard]] const std::vector& getPlannerPluginNames() const { - return pipeline_parameters_.planning_plugin; + return pipeline_parameters_.planning_plugins; } /** \brief Get the names of the planning request adapter plugins used */ @@ -198,12 +207,6 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline return pipeline_parameters_.response_adapters; } - /** \brief Get the planner manager for the loaded planning plugin */ - [[nodiscard]] const planning_interface::PlannerManagerPtr& getPlannerManager() - { - return planner_instance_; - } - /** \brief Get the robot model that this pipeline is using */ [[nodiscard]] const moveit::core::RobotModelConstPtr& getRobotModel() const { @@ -216,6 +219,23 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline return active_; } + /** \brief Return the parameter namespace as name of the planning pipeline. */ + [[nodiscard]] std::string getName() const + { + return parameter_namespace_; + } + + /** \brief Get access to planner plugin */ + const planning_interface::PlannerManagerPtr getPlannerManager(const std::string& planner_name) + { + if (planner_map_.find(planner_name) == planner_map_.end()) + { + RCLCPP_ERROR(node_->get_logger(), "Cannot retrieve planner because '%s' does not exist.", planner_name.c_str()); + return nullptr; + } + return planner_map_.at(planner_name); + } + private: /// \brief Helper function that is called by both constructors to configure and initialize a PlanningPipeline instance void configure(); @@ -240,7 +260,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline // Planner plugin std::unique_ptr> planner_plugin_loader_; - planning_interface::PlannerManagerPtr planner_instance_; + std::unordered_map planner_map_; // Plan request adapters std::unique_ptr> request_adapter_plugin_loader_; diff --git a/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml b/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml index 29d81597cd..41ef31d187 100644 --- a/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml +++ b/moveit_ros/planning/planning_pipeline/res/planning_pipeline_parameters.yaml @@ -1,9 +1,9 @@ planning_pipeline_parameters: - planning_plugin: { - type: string, + planning_plugins: { + type: string_array, description: "Name of the planner plugin used by the pipeline", read_only: true, - default_value: "UNKNOWN", + default_value: ["UNKNOWN"], } request_adapters: { type: string_array, diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index 87bd34b979..bac47ee51a 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -38,6 +38,41 @@ #include #include +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning.planning_pipeline"); + +/** + * @brief Transform a joint trajectory into a vector of joint constraints + * + * @param trajectory Reference trajectory from which the joint constraints are created + * @return A vector of joint constraints each corresponding to a waypoint of the reference trajectory. + */ +[[nodiscard]] std::vector +getTrajectoryConstraints(const robot_trajectory::RobotTrajectoryPtr& trajectory) +{ + const std::vector joint_names = + trajectory->getFirstWayPoint().getJointModelGroup(trajectory->getGroupName())->getActiveJointModelNames(); + + std::vector trajectory_constraints; + // Iterate through waypoints and create a joint constraint for each + for (size_t waypoint_index = 0; waypoint_index < trajectory->getWayPointCount(); ++waypoint_index) + { + moveit_msgs::msg::Constraints new_waypoint_constraint; + // Iterate through joints and copy waypoint data to joint constraint + for (const auto& joint_name : joint_names) + { + moveit_msgs::msg::JointConstraint new_joint_constraint; + new_joint_constraint.joint_name = joint_name; + new_joint_constraint.position = trajectory->getWayPoint(waypoint_index).getVariablePosition(joint_name); + new_waypoint_constraint.joint_constraints.push_back(new_joint_constraint); + } + trajectory_constraints.push_back(new_waypoint_constraint); + } + return trajectory_constraints; +} +} // namespace + namespace planning_pipeline { PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, @@ -56,7 +91,7 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr& node, const std::string& parameter_namespace, - const std::string& planner_plugin_name, + const std::vector& planner_plugin_names, const std::vector& request_adapter_plugin_names, const std::vector& response_adapter_plugin_names) : active_{ false } @@ -65,7 +100,7 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model , robot_model_(model) , logger_(moveit::getLogger("planning_pipeline")) { - pipeline_parameters_.planning_plugin = planner_plugin_name; + pipeline_parameters_.planning_plugins = planner_plugin_names; pipeline_parameters_.request_adapters = request_adapter_plugin_names; pipeline_parameters_.response_adapters = response_adapter_plugin_names; configure(); @@ -73,14 +108,6 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model void PlanningPipeline::configure() { - // Check if planning plugin name is available - if (pipeline_parameters_.planning_plugin.empty()) - { - const std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); - throw std::runtime_error("Planning plugin name is empty. Please choose one of the available plugins: " + - classes_str); - } - // If progress topic parameter is not empty, initialize publisher if (!pipeline_parameters_.progress_topic.empty()) { @@ -100,30 +127,46 @@ void PlanningPipeline::configure() throw; } - if (pipeline_parameters_.planning_plugin.empty() || pipeline_parameters_.planning_plugin == "UNKNOWN") + if (pipeline_parameters_.planning_plugins.empty() || pipeline_parameters_.planning_plugins.at(0) == "UNKNOWN") { const std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); throw std::runtime_error("Planning plugin name is empty or not defined in namespace '" + parameter_namespace_ + "'. Please choose one of the available plugins: " + classes_str); } - try + for (const auto& planner_name : pipeline_parameters_.planning_plugins) { - planner_instance_ = planner_plugin_loader_->createUniqueInstance(pipeline_parameters_.planning_plugin); - if (!planner_instance_->initialize(robot_model_, node_, parameter_namespace_)) + planning_interface::PlannerManagerPtr planner_instance; + + // Load plugin + try { - throw std::runtime_error("Unable to initialize planning plugin"); + planner_instance = planner_plugin_loader_->createUniqueInstance(planner_name); } - RCLCPP_INFO(logger_, "Using planning interface '%s'", planner_instance_->getDescription().c_str()); - } - catch (pluginlib::PluginlibException& ex) - { - const std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); - RCLCPP_FATAL(logger_, - "Exception while loading planner '%s': %s" - "Available plugins: %s", - pipeline_parameters_.planning_plugin.c_str(), ex.what(), classes_str.c_str()); - throw; + catch (pluginlib::PluginlibException& ex) + { + std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); + RCLCPP_FATAL(LOGGER, + "Exception while loading planner '%s': %s" + "Available plugins: %s", + planner_name.c_str(), ex.what(), classes_str.c_str()); + throw; + } + + // Check if planner is not NULL + if (!planner_instance) + { + throw std::runtime_error("Unable to initialize planning plugin " + planner_name + + ". Planner instance is nullptr."); + } + + // Initialize planner + if (!planner_instance->initialize(robot_model_, node_, parameter_namespace_)) + { + throw std::runtime_error("Unable to initialize planning plugin " + planner_name); + } + RCLCPP_INFO(LOGGER, "Successfully loaded planner '%s'", planner_instance->getDescription().c_str()); + planner_map_.insert(std::make_pair(planner_name, planner_instance)); } // Load the planner request adapters @@ -204,7 +247,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_interface::MotionPlanResponse& res, const bool publish_received_requests) const { - assert(planner_instance_ != nullptr); + assert(!planner_map_.empty()); // Set planning pipeline active active_ = true; @@ -241,22 +284,38 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& } } - // Call planner - if (res.error_code) + // Call planners + for (const auto& planner_name : pipeline_parameters_.planning_plugins) { - planning_interface::PlanningContextPtr context = - planner_instance_->getPlanningContext(planning_scene, mutable_request, res.error_code); - if (context) + const auto& planner = planner_map_.at(planner_name); + // Update reference trajectory with latest solution (if available) + if (res.trajectory) { - context->solve(res); - publishPipelineState(mutable_request, res, planner_instance_->getDescription()); + mutable_request.trajectory_constraints.constraints = getTrajectoryConstraints(res.trajectory); } - else + + // Try creating a planning context + planning_interface::PlanningContextPtr context = + planner->getPlanningContext(planning_scene, mutable_request, res.error_code); + if (!context) { RCLCPP_ERROR(node_->get_logger(), "Failed to create PlanningContext for planner '%s'. Aborting planning pipeline.", - planner_instance_->getDescription().c_str()); + planner->getDescription().c_str()); res.error_code = moveit::core::MoveItErrorCode::PLANNING_FAILED; + break; + } + + // Run planner + RCLCPP_INFO(node_->get_logger(), "Calling Planner '%s'", planner->getDescription().c_str()); + context->solve(res); + publishPipelineState(mutable_request, res, planner->getDescription()); + + // If planner does not succeed, break chain and return false + if (!res.error_code) + { + RCLCPP_ERROR(node_->get_logger(), "Planner '%s' failed", planner->getDescription().c_str()); + break; } } @@ -305,9 +364,12 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& void PlanningPipeline::terminate() const { - if (planner_instance_) + for (const auto& planner_pair : planner_map_) { - planner_instance_->terminate(); + if (planner_pair.second) + { + planner_pair.second->terminate(); + } } } } // namespace planning_pipeline diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp index 1b2940f981..1b5d8986ac 100644 --- a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_test_plugins.cpp @@ -122,6 +122,10 @@ class DummyPlannerManager : public planning_interface::PlannerManager { return true; } + std::string getDescription() const override + { + return std::string("DummyPlannerManager"); + } }; } // namespace planning_pipeline_test diff --git a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp index 010058554c..7100a69f09 100644 --- a/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp +++ b/moveit_ros/planning/planning_pipeline/test/planning_pipeline_tests.cpp @@ -45,7 +45,8 @@ const std::vector REQUEST_ADAPTERS{ "planning_pipeline_test/AlwaysS "planning_pipeline_test/AlwaysSuccessRequestAdapter" }; const std::vector RESPONSE_ADAPTERS{ "planning_pipeline_test/AlwaysSuccessResponseAdapter", "planning_pipeline_test/AlwaysSuccessResponseAdapter" }; -const std::string PLANNER_PLUGIN = std::string("planning_pipeline_test/DummyPlannerManager"); +const std::vector PLANNER_PLUGINS{ "planning_pipeline_test/DummyPlannerManager", + "planning_pipeline_test/DummyPlannerManager" }; } // namespace class TestPlanningPipeline : public testing::Test { @@ -70,15 +71,13 @@ TEST_F(TestPlanningPipeline, HappyPath) // WHEN the planning pipeline is configured // THEN it is successful EXPECT_NO_THROW(pipeline_ptr_ = std::make_shared( - robot_model_, node_, "", PLANNER_PLUGIN, REQUEST_ADAPTERS, RESPONSE_ADAPTERS)); + robot_model_, node_, "", PLANNER_PLUGINS, REQUEST_ADAPTERS, RESPONSE_ADAPTERS)); // THEN a planning pipeline exists EXPECT_NE(pipeline_ptr_, nullptr); // THEN the pipeline is inactive EXPECT_FALSE(pipeline_ptr_->isActive()); // THEN the pipeline contains a valid robot model EXPECT_NE(pipeline_ptr_->getRobotModel(), nullptr); - // THEN a planner plugin is loaded - EXPECT_NE(pipeline_ptr_->getPlannerManager(), nullptr); // THEN the loaded request adapter names equal the adapter names input vector const auto loaded_req_adapters = pipeline_ptr_->getRequestAdapterPluginNames(); EXPECT_TRUE(std::equal(loaded_req_adapters.begin(), loaded_req_adapters.end(), REQUEST_ADAPTERS.begin())); @@ -86,7 +85,8 @@ TEST_F(TestPlanningPipeline, HappyPath) const auto loaded_res_adapters = pipeline_ptr_->getResponseAdapterPluginNames(); EXPECT_TRUE(std::equal(loaded_res_adapters.begin(), loaded_res_adapters.end(), RESPONSE_ADAPTERS.begin())); // THEN the loaded planner plugin name equals the planner name input argument - EXPECT_EQ(pipeline_ptr_->getPlannerPluginName(), PLANNER_PLUGIN); + const auto loaded_planners = pipeline_ptr_->getPlannerPluginNames(); + EXPECT_TRUE(std::equal(loaded_planners.begin(), loaded_planners.end(), PLANNER_PLUGINS.begin())); // WHEN generatePlan is called // THEN A successful response is created @@ -103,7 +103,15 @@ TEST_F(TestPlanningPipeline, NoPlannerPluginConfigured) // WHEN the pipeline is configured // THEN an exception is thrown EXPECT_THROW(pipeline_ptr_ = std::make_shared( - robot_model_, node_, "", "NoExistingPlannerPlugin", REQUEST_ADAPTERS, RESPONSE_ADAPTERS), + robot_model_, node_, "", std::vector(), REQUEST_ADAPTERS, RESPONSE_ADAPTERS), + std::runtime_error); + + // GIVEN a configuration with planner plugin called UNKNOWN + // WHEN the pipeline is configured + // THEN an exception is thrown + EXPECT_THROW(pipeline_ptr_ = std::make_shared( + robot_model_, node_, "", std::vector({ "UNKNOWN" }), REQUEST_ADAPTERS, + RESPONSE_ADAPTERS), std::runtime_error); } diff --git a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp index 21e3763351..9c9760e2e4 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/include/moveit/planning_pipeline_interfaces/planning_pipeline_interfaces.hpp @@ -103,9 +103,6 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe * \param [in] node Node used to load parameters * \param [in] parameter_namespace Optional prefix for the pipeline parameter * namespace. Empty by default, so only the pipeline name is used as namespace - * \param [in] planning_plugin_param_name - * Optional name of the planning plugin namespace - * \param [in] adapter_plugins_param_name Optional name of the adapter plugin namespace * \return Map of PlanningPipelinePtr's associated with a name for faster look-up */ std::unordered_map diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp index 23d6bd89f8..73abd5fe4c 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -181,7 +181,7 @@ createPlanningPipelineMap(const std::vector& pipeline_names, planning_pipeline::PlanningPipelinePtr pipeline = std::make_shared( robot_model, node, parameter_namespace + planning_pipeline_name); - if (!pipeline->getPlannerManager()) + if (!pipeline) { RCLCPP_ERROR(getLogger(), "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); continue; From 3df6b28136c76fc63ffb3f845da2509aabef4611 Mon Sep 17 00:00:00 2001 From: Chris Thrasher Date: Wed, 6 Dec 2023 16:13:36 -0700 Subject: [PATCH 05/17] Depend on `rsl::rsl` as a non-Ament dependency (#2578) --- moveit_core/utils/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index a5d0a884f0..3bd09ef841 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -8,7 +8,8 @@ target_include_directories(moveit_utils PUBLIC $ $ ) -ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp rsl fmt) +ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp fmt) +target_link_libraries(moveit_utils rsl::rsl) set_target_properties(moveit_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") install(DIRECTORY include/ DESTINATION include/moveit_core) @@ -20,7 +21,7 @@ target_include_directories(moveit_test_utils PUBLIC $ $ ) -target_link_libraries(moveit_test_utils moveit_robot_model moveit_kinematics_base) +target_link_libraries(moveit_test_utils moveit_robot_model moveit_kinematics_base rsl::rsl) ament_target_dependencies(moveit_test_utils ament_index_cpp Boost @@ -30,7 +31,6 @@ ament_target_dependencies(moveit_test_utils urdfdom urdfdom_headers rclcpp - rsl fmt ) set_target_properties(moveit_test_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") From 912997071e6958b644bbb0703bf36f5e3162b6bd Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Thu, 7 Dec 2023 09:29:41 -0700 Subject: [PATCH 06/17] Fix moveit_py Policy docs build (#2584) --- moveit_py/docs/source/index.rst | 1 + moveit_py/moveit/policies/__init__.py | 5 +++++ moveit_py/moveit/policies/__init__.pyi | 0 moveit_py/moveit/policies/policy.py | 5 ++--- moveit_py/moveit/policies/policy.pyi | 21 +++++++++++++++++++++ 5 files changed, 29 insertions(+), 3 deletions(-) create mode 100644 moveit_py/moveit/policies/__init__.pyi create mode 100644 moveit_py/moveit/policies/policy.pyi diff --git a/moveit_py/docs/source/index.rst b/moveit_py/docs/source/index.rst index 104580cc28..9d598f9ae2 100644 --- a/moveit_py/docs/source/index.rst +++ b/moveit_py/docs/source/index.rst @@ -9,4 +9,5 @@ MoveItPy API Documentation moveit_py.core moveit_py.planning + moveit_py.policies moveit_py.servo_client diff --git a/moveit_py/moveit/policies/__init__.py b/moveit_py/moveit/policies/__init__.py index 53e7dff01b..d6e57cf6f9 100644 --- a/moveit_py/moveit/policies/__init__.py +++ b/moveit_py/moveit/policies/__init__.py @@ -1 +1,6 @@ +"""Utilities for learned policy deployment. + +This module can be used to configure moveit_servo for learning based applications. +""" + from .policy import Policy diff --git a/moveit_py/moveit/policies/__init__.pyi b/moveit_py/moveit/policies/__init__.pyi new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_py/moveit/policies/policy.py b/moveit_py/moveit/policies/policy.py index 24e1f70a5a..907308ce0a 100644 --- a/moveit_py/moveit/policies/policy.py +++ b/moveit_py/moveit/policies/policy.py @@ -39,7 +39,6 @@ from abc import ABC, abstractmethod -import rclpy from rclpy.node import Node from rclpy.qos import QoSProfile @@ -66,7 +65,7 @@ def __init__(self, params, node_name="policy_node"): # set policy to inactive by default self._is_active = False - self.activate_policy = self.create_service( + self.activate_policy_service = self.create_service( SetBool, "activate_policy", self.activate_policy, @@ -98,7 +97,7 @@ def get_sensor_msg_type(self, msg_type): if msg_type == "sensor_msgs/Image": return Image else: - raise ValueError(f"Sensor type {sensor_type} not supported.") + raise ValueError(f"Sensor type {msg_type} not supported.") def get_command_msg_type(self, msg_type): """Returns the ROS 2 message type for a given command type.""" diff --git a/moveit_py/moveit/policies/policy.pyi b/moveit_py/moveit/policies/policy.pyi new file mode 100644 index 0000000000..c26c12fc38 --- /dev/null +++ b/moveit_py/moveit/policies/policy.pyi @@ -0,0 +1,21 @@ +import abc +from abc import ABC, abstractmethod +from rclpy.node import Node +from typing import Any + +class Policy(ABC, Node, metaclass=abc.ABCMeta): + logger: Any + param_listener: Any + params: Any + activate_policy_service: Any + _is_active: bool + def __init__(self, params, node_name) -> None: ... + @property + def is_active(self) -> Any: ... + def activate_policy(self, request: Any, response: Any) -> Any: ... + def get_sensor_msg_type(self, msg_type: str) -> Any: ... + def get_command_msg_type(self, msg_type: str) -> Any: ... + def register_sensors(self): ... + def register_command(self): ... + @abstractmethod + def forward(self): ... From 517898f94c715b5db8757cb1452ff84079e64346 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 7 Dec 2023 16:42:34 -0700 Subject: [PATCH 07/17] Remove unported trajopt package (#2582) --- .github/CODEOWNERS.disabled | 1 - moveit_planners/trajopt/CATKIN_IGNORE | 0 moveit_planners/trajopt/CMakeLists.txt | 108 ---- moveit_planners/trajopt/COLCON_IGNORE | 0 moveit_planners/trajopt/README.md | 1 - .../trajopt_interface/kinematic_terms.h | 93 --- .../trajopt_interface/problem_description.h | 369 ----------- .../trajopt_interface/trajopt_interface.h | 77 --- .../trajopt_planning_context.h | 32 - moveit_planners/trajopt/package.xml | 34 - .../trajopt/src/kinematic_terms.cpp | 64 -- .../trajopt/src/problem_description.cpp | 601 ------------------ .../trajopt/src/trajopt_interface.cpp | 463 -------------- .../trajopt/src/trajopt_planner_manager.cpp | 140 ---- .../trajopt/src/trajopt_planning_context.cpp | 75 --- .../trajopt/test/trajectory_test.cpp | 212 ------ .../trajopt/test/trajectory_test.test | 10 - .../trajopt_interface_plugin_description.xml | 7 - .../unported_templates/README.md | 1 - .../run_benchmark_trajopt.launch | 22 - 20 files changed, 2310 deletions(-) delete mode 100644 moveit_planners/trajopt/CATKIN_IGNORE delete mode 100644 moveit_planners/trajopt/CMakeLists.txt delete mode 100644 moveit_planners/trajopt/COLCON_IGNORE delete mode 100644 moveit_planners/trajopt/README.md delete mode 100644 moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h delete mode 100644 moveit_planners/trajopt/include/trajopt_interface/problem_description.h delete mode 100644 moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h delete mode 100644 moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h delete mode 100644 moveit_planners/trajopt/package.xml delete mode 100644 moveit_planners/trajopt/src/kinematic_terms.cpp delete mode 100644 moveit_planners/trajopt/src/problem_description.cpp delete mode 100644 moveit_planners/trajopt/src/trajopt_interface.cpp delete mode 100644 moveit_planners/trajopt/src/trajopt_planner_manager.cpp delete mode 100644 moveit_planners/trajopt/src/trajopt_planning_context.cpp delete mode 100644 moveit_planners/trajopt/test/trajectory_test.cpp delete mode 100644 moveit_planners/trajopt/test/trajectory_test.test delete mode 100644 moveit_planners/trajopt/trajopt_interface_plugin_description.xml delete mode 100644 moveit_setup_assistant/unported_templates/run_benchmark_trajopt.launch diff --git a/.github/CODEOWNERS.disabled b/.github/CODEOWNERS.disabled index 059134e4bd..960b877e64 100644 --- a/.github/CODEOWNERS.disabled +++ b/.github/CODEOWNERS.disabled @@ -93,6 +93,5 @@ /moveit_planners/chomp/chomp_interface/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/chomp/chomp_optimizer_adapter/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/chomp/chomp_motion_planner/ @raghavendersahdev @knorth55 @bmagyar -/moveit_planners/trajopt @ommmid @mamoll /moveit_planners/pilz_industrial_motion_planner @jschleicher @ct2034 /moveit_planners/pilz_industrial_motion_planner_testutils/ @jschleicher @ct2034 diff --git a/moveit_planners/trajopt/CATKIN_IGNORE b/moveit_planners/trajopt/CATKIN_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_planners/trajopt/CMakeLists.txt b/moveit_planners/trajopt/CMakeLists.txt deleted file mode 100644 index 53f006669e..0000000000 --- a/moveit_planners/trajopt/CMakeLists.txt +++ /dev/null @@ -1,108 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(moveit_planners_trajopt LANGUAGES CXX) - -find_package(catkin REQUIRED - COMPONENTS - moveit_core - moveit_visual_tools - moveit_ros_planning - moveit_ros_planning_interface - pluginlib - roscpp - rosparam_shortcuts -) -find_package(trajopt REQUIRED) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES - moveit_planners_trajopt - INCLUDE_DIRS - CATKIN_DEPENDS - roscpp - pluginlib - moveit_core - moveit_visual_tools - moveit_ros_planning_interface - rosparam_shortcuts -) - -# The following include_directory should have include folder of the new planner. -include_directories( - include - ${catkin_INCLUDE_DIRS} - SYSTEM - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -add_library( - moveit_planners_trajopt - src/trajopt_interface.cpp - src/problem_description.cpp - src/kinematic_terms.cpp - src/trajopt_planning_context.cpp -) - -target_link_libraries( - moveit_planners_trajopt - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - trajopt::trajopt -) - -set_target_properties( - moveit_planners_trajopt - PROPERTIES - VERSION - "${moveit_planners_trajopt_VERSION}" -) - -# TrajOpt planning plugin -add_library(moveit_trajopt_planner_plugin src/trajopt_planner_manager.cpp) -set_target_properties(moveit_trajopt_planner_plugin PROPERTIES VERSION "${moveit_planners_trajopt_VERSION}") -target_link_libraries(moveit_trajopt_planner_plugin moveit_planners_trajopt ${catkin_LIBRARIES}) - -############# -## Install ## -############# - -# Mark executables and/or libraries for installation -install( - TARGETS - moveit_trajopt_planner_plugin - ARCHIVE DESTINATION - ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION - ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION - ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Mark cpp header files for installation -install( - DIRECTORY - include/trajopt_interface/ - DESTINATION - ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install(FILES trajopt_interface_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -############# -## Testing ## -############# - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - - add_rostest_gtest(trajectory_test test/trajectory_test.test test/trajectory_test.cpp) - target_link_libraries( - trajectory_test - ${LIBRARY_NAME} - ${catkin_LIBRARIES} - moveit_planners_trajopt - ) - -endif() diff --git a/moveit_planners/trajopt/COLCON_IGNORE b/moveit_planners/trajopt/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_planners/trajopt/README.md b/moveit_planners/trajopt/README.md deleted file mode 100644 index 84e0ce1d44..0000000000 --- a/moveit_planners/trajopt/README.md +++ /dev/null @@ -1 +0,0 @@ -As of August 2019, this is a work in progress towards adding trajopt motion planning algorithm to MoveIt as a planner plugin. diff --git a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h deleted file mode 100644 index 96274cf9c7..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h +++ /dev/null @@ -1,93 +0,0 @@ -#pragma once - -#include - -#include - -namespace trajopt_interface -{ -/** - * @brief Extracts the vector part of quaternion - */ -inline Eigen::Vector3d quaternionRotationVector(const Eigen::Matrix3d& matrix) -{ - Eigen::Quaterniond quaternion; - quaternion = matrix; - return Eigen::Vector3d(quaternion.x(), quaternion.y(), quaternion.z()); -} - -/** - * @brief Appends b to a of type VectorXd - */ -inline Eigen::VectorXd concatVector(const Eigen::VectorXd& vector_a, const Eigen::VectorXd& vector_b) -{ - Eigen::VectorXd out(vector_a.size() + vector_b.size()); - out.topRows(vector_a.size()) = vector_a; - out.middleRows(vector_a.size(), vector_b.size()) = vector_b; - return out; -} - -/** - * @brief Appends b to a of type T - */ -template -inline std::vector concatVector(const std::vector& vector_a, const std::vector& vector_b) -{ - std::vector out; - out.insert(out.end(), vector_a.begin(), vector_a.end()); - out.insert(out.end(), vector_b.begin(), vector_b.end()); - return out; -} - -/** - * @brief Used to calculate the error for StaticCartPoseTermInfo - * This is converted to a cost or constraint using TrajOptCostFromErrFunc or TrajOptConstraintFromErrFunc - */ -struct CartPoseErrCalculator : public sco::VectorOfVector -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Eigen::Isometry3d target_pose_inv_; - planning_scene::PlanningSceneConstPtr planning_scene_; - std::string link_; - Eigen::Isometry3d tcp_; - - CartPoseErrCalculator(const Eigen::Isometry3d& pose, const planning_scene::PlanningSceneConstPtr planning_scene, - const std::string& link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) - : target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp) - { - } - - Eigen::VectorXd operator()(const Eigen::VectorXd& dof_vals) const override; -}; - -// TODO(omid): The following should be added and adjusted from trajopt -// JointPosEqCost -// JointPosIneqCost -// JointPosEqConstraint -// JointPosIneqConstraint - -struct JointVelErrCalculator : sco::VectorOfVector -{ - /** @brief Velocity target */ - double target_; - /** @brief Upper tolerance */ - double upper_tol_; - /** @brief Lower tolerance */ - double lower_tol_; - JointVelErrCalculator() : target_(0.0), upper_tol_(0.0), lower_tol_(0.0) - { - } - JointVelErrCalculator(double target, double upper_tol, double lower_tol) - : target_(target), upper_tol_(upper_tol), lower_tol_(lower_tol) - { - } - - Eigen::VectorXd operator()(const Eigen::VectorXd& var_vals) const; -}; - -struct JointVelJacobianCalculator : sco::MatrixOfVector -{ - Eigen::MatrixXd operator()(const Eigen::VectorXd& var_vals) const; -}; - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h deleted file mode 100644 index c8a8598856..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h +++ /dev/null @@ -1,369 +0,0 @@ -#pragma once -#include -#include -#include - -#include -#include - -#include - -namespace trajopt_interface -{ -/** -@brief Used to apply cost/constraint to joint-space velocity - -Term is applied to every step between first_step and last_step. It applies two limits, upper_limits/lower_limits, -to the joint velocity subject to the following cases. - -* term_type = TT_COST -** upper_limit = lower_limit = 0 - Cost is applied with a SQUARED error scaled for each joint by coeffs -** upper_limit != lower_limit - 2 hinge costs are applied scaled for each joint by coeffs. If velocity < upper_limit and -velocity > lower_limit, no penalty. - -* term_type = TT_CNT -** upper_limit = lower_limit = 0 - Equality constraint is applied -** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when velocity < -upper_limit and velocity > lower_limit - -Note: coeffs, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints. -If one value is given, this will be broadcast to all joints. - -Note: Velocity is calculated numerically using forward finite difference - -\f{align*}{ - cost = \sum_{t=0}^{T-2} \sum_j c_j (x_{t+1,j} - x_{t,j})^2 -\f} -where j indexes over DOF, and \f$c_j\f$ are the coeffs. -*/ - -struct TermInfo; -MOVEIT_CLASS_FORWARD(TermInfo); // Defines TermInfoPtr, ConstPtr, WeakPtr... etc - -class TrajOptProblem; -MOVEIT_CLASS_FORWARD(TrajOptProblem); // Defines TrajOptProblemPtr, ConstPtr, WeakPtr... etc - -struct JointPoseTermInfo; -MOVEIT_CLASS_FORWARD(JointPoseTermInfo); // Defines JointPoseTermInfoPtr, ConstPtr, WeakPtr... etc - -struct CartPoseTermInfo; -MOVEIT_CLASS_FORWARD(CartPoseTermInfo); // Defines CartPoseTermInfoPtr, ConstPtr, WeakPtr... etc - -struct JointVelTermInfo; -MOVEIT_CLASS_FORWARD(JointVelTermInfo); // Defines JointVelTermInfoPtr, ConstPtr, WeakPtr... etc - -struct ProblemInfo; -TrajOptProblemPtr ConstructProblem(const ProblemInfo&); - -enum TermType -{ - TT_COST = 0x1, // 0000 0001 - TT_CNT = 0x2, // 0000 0010 - TT_USE_TIME = 0x4, // 0000 0100 -}; - -struct BasicInfo -{ - /** @brief If true, first time step is fixed with a joint level constraint - If this is false, the starting point of the trajectory will - not be the current position of the robot. The use case example is: if we are trying to execute a process like - sanding the critical part which is the actual process path not how we get to the start of the process path. So we - plan the - process path first leaving the start free to hopefully get the most optimal and then we plan from the current - location with - start fixed to the start of the process path. It depends on what we want the default behavior to be - */ - bool start_fixed; - /** @brief Number of time steps (rows) in the optimization matrix */ - int n_steps; - sco::IntVec dofs_fixed; // optional - sco::ModelType convex_solver; // which convex solver to use - - /** @brief If true, the last column in the optimization matrix will be 1/dt */ - bool use_time = false; - /** @brief The upper limit of 1/dt values allowed in the optimization*/ - double dt_upper_lim = 1.0; - /** @brief The lower limit of 1/dt values allowed in the optimization*/ - double dt_lower_lim = 1.0; -}; - -/** -Initialization info read from json -*/ -struct InitInfo -{ - /** @brief Methods of initializing the optimization matrix. This defines how robot moves from the current - state to the start state - - STATIONARY: Initializes all joint values to the initial value (the current value in the env) - JOINT_INTERPOLATED: Linearly interpolates between initial value and the joint position specified in InitInfo.data - GIVEN_TRAJ: Initializes the matrix to a given trajectory - - In all cases the dt column (if present) is appended the selected method is defined. - */ - enum Type - { - STATIONARY, - JOINT_INTERPOLATED, - GIVEN_TRAJ, - }; - /** @brief Specifies the type of initialization to use */ - Type type; - /** @brief Data used during initialization. Use depends on the initialization selected. This data will be used - to create initialization matrix. We need to give the goal information to this init info - */ - trajopt::TrajArray data; - // Eigen::VectorXd data_vec; - // trajopt::TrajArray data_trajectory; - /** @brief Default value the final column of the optimization is initialized too if time is being used */ - double dt = 1.0; -}; - -/** -When cost or constraint element of JSON doc is read, one of these guys gets -constructed to hold the parameters. -Then it later gets converted to a Cost object by the addObjectiveTerms method -*/ -struct TermInfo -{ - std::string name; - int term_type; - int getSupportedTypes() - { - return supported_term_types_; - } - // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; - virtual void addObjectiveTerms(TrajOptProblem& prob) = 0; - - static TermInfoPtr fromName(const std::string& type); - - /** - * Registers a user-defined TermInfo so you can use your own cost - * see function RegisterMakers.cpp - */ - using MakerFunc = TermInfoPtr (*)(void); - static void RegisterMaker(const std::string& type, MakerFunc); - - TermInfo() = default; - TermInfo(const TermInfo&) = default; - TermInfo(TermInfo&&) = default; - TermInfo& operator=(const TermInfo&) = default; - TermInfo& operator=(TermInfo&&) = default; - virtual ~TermInfo() = default; - -protected: - TermInfo(int supported_term_types) : supported_term_types_(supported_term_types) - { - } - -private: - static std::map name_to_maker_; - int supported_term_types_; -}; - -struct ProblemInfo -{ -public: - BasicInfo basic_info; - sco::BasicTrustRegionSQPParameters opt_info; - std::vector cost_infos; - std::vector cnt_infos; - InitInfo init_info; - - planning_scene::PlanningSceneConstPtr planning_scene; - std::string planning_group_name; - - ProblemInfo(planning_scene::PlanningSceneConstPtr ps, const std::string& pg) - : planning_scene(ps), planning_group_name(pg) - { - } -}; - -/** - * Holds all the data for a trajectory optimization problem - * so you can modify it programmatically, e.g. add your own costs - */ -class TrajOptProblem : public sco::OptProb -{ -public: - TrajOptProblem(); - TrajOptProblem(const ProblemInfo& problem_info); - virtual ~TrajOptProblem() = default; - /** @brief Returns the values of the specified joints (start_col to num_col) for the specified timestep i.*/ - sco::VarVector GetVarRow(int i, int start_col, int num_col) - { - return matrix_traj_vars.rblock(i, start_col, num_col); - } - /** @brief Returns the values of all joints for the specified timestep i.*/ - sco::VarVector GetVarRow(int i) - { - return matrix_traj_vars.row(i); - } - /** @brief Returns the value of the specified joint j for the specified timestep i.*/ - sco::Var& GetVar(int i, int j) - { - return matrix_traj_vars.at(i, j); - } - trajopt::VarArray& GetVars() - { - return matrix_traj_vars; - } - /** @brief Returns the number of steps in the problem. This is the number of rows in the optimization matrix.*/ - int GetNumSteps() - { - return matrix_traj_vars.rows(); - } - /** @brief Returns the problem DOF. This is the number of columns in the optization matrix. - * Note that this is not necessarily the same as the kinematic DOF.*/ - int GetNumDOF() - { - return matrix_traj_vars.cols(); - } - /** @brief Returns the kinematic DOF of the active joint model group - */ - int GetActiveGroupNumDOF() - { - return dof_; - } - planning_scene::PlanningSceneConstPtr GetPlanningScene() - { - return planning_scene_; - } - void SetInitTraj(const trajopt::TrajArray& x) - { - matrix_init_traj = x; - } - trajopt::TrajArray GetInitTraj() - { - return matrix_init_traj; - } - // friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&); - /** @brief Returns TrajOptProb.has_time */ - bool GetHasTime() - { - return has_time; - } - /** @brief Sets TrajOptProb.has_time */ - void SetHasTime(bool tmp) - { - has_time = tmp; - } - -private: - /** @brief If true, the last column in the optimization matrix will be 1/dt */ - bool has_time; - /** @brief is the matrix holding the joint values of the trajectory for all timesteps */ - trajopt::VarArray matrix_traj_vars; - planning_scene::PlanningSceneConstPtr planning_scene_; - std::string planning_group_; - /** @brief Kinematic DOF of the active joint model group */ - int dof_; - trajopt::TrajArray matrix_init_traj; -}; - -/** @brief This term is used when the goal frame is fixed in cartesian space - - Set term_type == TT_COST or TT_CNT for cost or constraint. -*/ -struct CartPoseTermInfo : public TermInfo -{ - // EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** @brief Timestep at which to apply term */ - int timestep; - /** @brief Cartesian position */ - Eigen::Vector3d xyz; - /** @brief Rotation quaternion */ - Eigen::Vector4d wxyz; - /** @brief coefficients for position and rotation */ - Eigen::Vector3d pos_coeffs, rot_coeffs; - /** @brief Link which should reach desired pose */ - std::string link; - /** @brief Static transform applied to the link */ - Eigen::Isometry3d tcp; - - CartPoseTermInfo(); - - /** @brief Used to add term to pci from json */ - // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; - /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; - - static TermInfoPtr create() - { - return std::make_shared(); - } -}; - -/** - \brief Joint space position cost - Position operates on a single point (unlike velocity, etc). This is b/c the primary usecase is joint-space - position waypoints - - \f{align*}{ - \sum_i c_i (x_i - xtarg_i)^2 - \f} - where \f$i\f$ indexes over dof and \f$c_i\f$ are coeffs - */ -struct JointPoseTermInfo : public TermInfo -{ - /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec coeffs; - /** @brief Vector of position targets. This is a required value. Size should be the DOF of the system */ - trajopt::DblVec targets; - /** @brief Vector of position upper limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec upper_tols; - /** @brief Vector of position lower limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec lower_tols; - /** @brief First time step to which the term is applied. Default: 0 */ - int first_step = 0; - /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ - int last_step = -1; - - /** @brief Initialize term with it's supported types */ - JointPoseTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) - { - } - - /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; - - static TermInfoPtr create() - { - return std::make_shared(); - } -}; - -struct JointVelTermInfo : public TermInfo -{ - /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec coeffs; - /** @brief Vector of velocity targets. This is a required value. Size should be the DOF of the system */ - trajopt::DblVec targets; - /** @brief Vector of velocity upper limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec upper_tols; - /** @brief Vector of velocity lower limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec lower_tols; - /** @brief First time step to which the term is applied. Default: 0*/ - int first_step = 0; - /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ - int last_step = -1; - - /** @brief Initialize term with it's supported types */ - JointVelTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) - { - } - - /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; - - static TermInfoPtr create() - { - return std::make_shared(); - } -}; - -void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, - trajopt::TrajArray& init_traj); - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h deleted file mode 100644 index 706fffd6f9..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h +++ /dev/null @@ -1,77 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, 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: Omid Heidari */ -#pragma once - -#include -#include -#include -#include "problem_description.h" - -namespace trajopt_interface -{ -MOVEIT_CLASS_FORWARD(TrajOptInterface); // Defines TrajOptInterfacePtr, ConstPtr, WeakPtr... etc - -class TrajOptInterface -{ -public: - TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); - - const sco::BasicTrustRegionSQPParameters& getParams() const - { - return params_; - } - - bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, moveit_msgs::MotionPlanDetailedResponse& res); - -protected: - /** @brief Configure everything using the param server */ - void setTrajOptParams(sco::BasicTrustRegionSQPParameters& param); - void setDefaultTrajOPtParams(); - void setProblemInfoParam(ProblemInfo& problem_info); - void setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name); - trajopt::DblVec extractStartJointValues(const planning_interface::MotionPlanRequest& req, - const std::vector& group_joint_names); - - ros::NodeHandle nh_; /// The ROS node handle - sco::BasicTrustRegionSQPParameters params_; - std::vector optimizer_callbacks_; - TrajOptProblemPtr trajopt_problem_; - std::string name_; -}; - -void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res); -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h deleted file mode 100644 index 9c06eb0f5e..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once - -#include - -#include -#include - -namespace trajopt_interface -{ -MOVEIT_CLASS_FORWARD(TrajOptPlanningContext); // Defines TrajOptPlanningContextPtr, ConstPtr, WeakPtr... etc - -class TrajOptPlanningContext : public planning_interface::PlanningContext -{ -public: - TrajOptPlanningContext(const std::string& name, const std::string& group, - const moveit::core::RobotModelConstPtr& model); - ~TrajOptPlanningContext() override - { - } - - bool solve(planning_interface::MotionPlanResponse& res) override; - bool solve(planning_interface::MotionPlanDetailedResponse& res) override; - - bool terminate() override; - void clear() override; - -private: - moveit::core::RobotModelConstPtr robot_model_; - - TrajOptInterfacePtr trajopt_interface_; -}; -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/package.xml b/moveit_planners/trajopt/package.xml deleted file mode 100644 index 7cb48c34d4..0000000000 --- a/moveit_planners/trajopt/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - moveit_planners_trajopt - 1.1.0 - TrajOpt planning plugin, an optimization based motion planner - - Omid Heidari - MoveIt Release Team - - BSD-3-Clause - - http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 - - Omid Heidari - - catkin - - pluginlib - moveit_core - moveit_ros_planning - moveit_ros_planning_interface - moveit_visual_tools - roscpp - rosparam_shortcuts - trajopt - - - - - - diff --git a/moveit_planners/trajopt/src/kinematic_terms.cpp b/moveit_planners/trajopt/src/kinematic_terms.cpp deleted file mode 100644 index 617ab0bbd9..0000000000 --- a/moveit_planners/trajopt/src/kinematic_terms.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include -#include - -#include -#include - -#include - -using namespace std; -using namespace sco; -using namespace Eigen; - -namespace trajopt_interface -{ -VectorXd CartPoseErrCalculator::operator()(const VectorXd& dof_vals) const -{ - // TODO: create the actual error function from information in planning scene - VectorXd err; - return err; -} - -VectorXd JointVelErrCalculator::operator()(const VectorXd& var_vals) const -{ - assert(var_vals.rows() % 2 == 0); - // var_vals = (theta_t1, theta_t2, theta_t3 ... 1/dt_1, 1/dt_2, 1/dt_3 ...) - int half = static_cast(var_vals.rows() / 2); - int num_vels = half - 1; - // (x1-x0)*(1/dt) - VectorXd vel = (var_vals.segment(1, num_vels) - var_vals.segment(0, num_vels)).array() * - var_vals.segment(half + 1, num_vels).array(); - - // Note that for equality terms tols are 0, so error is effectively doubled - VectorXd result(vel.rows() * 2); - result.topRows(vel.rows()) = -(upper_tol_ - (vel.array() - target_)); - result.bottomRows(vel.rows()) = lower_tol_ - (vel.array() - target_); - return result; -} - -MatrixXd JointVelJacobianCalculator::operator()(const VectorXd& var_vals) const -{ - // var_vals = (theta_t1, theta_t2, theta_t3 ... 1/dt_1, 1/dt_2, 1/dt_3 ...) - int num_vals = static_cast(var_vals.rows()); - int half = num_vals / 2; - int num_vels = half - 1; - MatrixXd jac = MatrixXd::Zero(num_vels * 2, num_vals); - - for (int i = 0; i < num_vels; ++i) - { - // v = (j_i+1 - j_i)*(1/dt) - // We calculate v with the dt from the second pt - int time_index = i + half + 1; - jac(i, i) = -1.0 * var_vals(time_index); - jac(i, i + 1) = 1.0 * var_vals(time_index); - jac(i, time_index) = var_vals(i + 1) - var_vals(i); - // All others are 0 - } - - // bottom half is negative velocities - jac.bottomRows(num_vels) = -jac.topRows(num_vels); - - return jac; -} - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/src/problem_description.cpp deleted file mode 100644 index 9fd3803ad0..0000000000 --- a/moveit_planners/trajopt/src/problem_description.cpp +++ /dev/null @@ -1,601 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, John Schulman - * 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. - * - * 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. - * - * http://opensource.org/licenses/BSD-2-Clause - *********************************************************************/ - -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -/** - * @brief Checks the size of the parameter given and throws if incorrect - * @param parameter The vector whose size is getting checked - * @param expected_size The expected size of the vector - * @param name The name to use when printing an error or warning - * @param apply_first If true and only one value is given, broadcast value to length of expected_size - */ -void checkParameterSize(trajopt::DblVec& parameter, unsigned int expected_size, const std::string& name, - bool apply_first = true) -{ - if (apply_first == true && parameter.size() == 1) - { - parameter = trajopt::DblVec(expected_size, parameter[0]); - ROS_INFO("1 %s given. Applying to all %i joints", name.c_str(), expected_size); - } - else if (parameter.size() != expected_size) - { - PRINT_AND_THROW(boost::format("wrong number of %s. expected %i got %i") % name % expected_size % parameter.size()); - } -} - -namespace trajopt_interface -{ -TrajOptProblem::TrajOptProblem() -{ -} - -TrajOptProblem::TrajOptProblem(const ProblemInfo& problem_info) - : OptProb(problem_info.basic_info.convex_solver) - , planning_scene_(problem_info.planning_scene) - , planning_group_(problem_info.planning_group_name) -{ - moveit::core::RobotModelConstPtr robot_model = planning_scene_->getRobotModel(); - moveit::core::RobotState current_state = planning_scene_->getCurrentState(); - const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup(planning_group_); - - moveit::core::JointBoundsVector bounds = joint_model_group->getActiveJointModelsBounds(); - dof_ = joint_model_group->getActiveJointModelNames().size(); // or bounds.size(); - - int n_steps = problem_info.basic_info.n_steps; - - ROS_INFO(" ======================================= problem_description: limits"); - Eigen::MatrixX2d limits(dof_, 2); - for (int k = 0; k < limits.size() / 2; ++k) - { - moveit::core::JointModel::Bounds bound = *bounds[k]; - // In MoveIt, joints are considered to have multiple dofs but we only have single dof joints: - moveit::core::VariableBounds joint_bound = bound.front(); - - limits(k, 0) = joint_bound.min_position_; - limits(k, 1) = joint_bound.max_position_; - - ROS_INFO("joint %i with lower bound: %f, upper bound: %f", k, joint_bound.min_position_, joint_bound.max_position_); - } - - Eigen::VectorXd lower, upper; - lower = limits.col(0); - upper = limits.col(1); - - trajopt::DblVec vlower, vupper; - std::vector names; - for (int i = 0; i < n_steps; ++i) - { - for (int j = 0; j < dof_; ++j) - { - names.push_back((boost::format("j_%i_%i") % i % j).str()); - } - vlower.insert(vlower.end(), lower.data(), lower.data() + lower.size()); - vupper.insert(vupper.end(), upper.data(), upper.data() + upper.size()); - - if (problem_info.basic_info.use_time == true) - { - vlower.insert(vlower.end(), problem_info.basic_info.dt_lower_lim); - vupper.insert(vupper.end(), problem_info.basic_info.dt_upper_lim); - names.push_back((boost::format("dt_%i") % i).str()); - } - } - - sco::VarVector trajvarvec = createVariables(names, vlower, vupper); - matrix_traj_vars = trajopt::VarArray(n_steps, dof_ + (problem_info.basic_info.use_time ? 1 : 0), trajvarvec.data()); - // matrix_traj_vars is essentially a matrix of elements like: - // j_0_0, j_0_1 ... - // j_1_0, j_1_1 ... - // its size is n_steps by n_dof -} - -TrajOptProblemPtr ConstructProblem(const ProblemInfo& pci) -{ - const BasicInfo& bi = pci.basic_info; - int n_steps = bi.n_steps; - - bool use_time = false; - // Check that all costs and constraints support the types that are specified in pci - for (TermInfoPtr cost : pci.cost_infos) - { - if (cost->term_type & TT_CNT) - ROS_WARN("%s is listed as a type TT_CNT but was added to cost_infos", (cost->name).c_str()); - if (!(cost->getSupportedTypes() & TT_COST)) - PRINT_AND_THROW(boost::format("%s is only a constraint, but you listed it as a cost") % cost->name); - if (cost->term_type & TT_USE_TIME) - { - use_time = true; - if (!(cost->getSupportedTypes() & TT_USE_TIME)) - PRINT_AND_THROW(boost::format("%s does not support time, but you listed it as a using time") % cost->name); - } - } - for (TermInfoPtr cnt : pci.cnt_infos) - { - if (cnt->term_type & TT_COST) - ROS_WARN("%s is listed as a type TT_COST but was added to cnt_infos", (cnt->name).c_str()); - if (!(cnt->getSupportedTypes() & TT_CNT)) - PRINT_AND_THROW(boost::format("%s is only a cost, but you listed it as a constraint") % cnt->name); - if (cnt->term_type & TT_USE_TIME) - { - use_time = true; - if (!(cnt->getSupportedTypes() & TT_USE_TIME)) - PRINT_AND_THROW(boost::format("%s does not support time, but you listed it as a using time") % cnt->name); - } - } - - // Check that if a cost or constraint uses time, basic_info is set accordingly - if ((use_time == true) && (pci.basic_info.use_time == false)) - PRINT_AND_THROW("A term is using time and basic_info is not set correctly. Try basic_info.use_time = true"); - - // This could be removed in the future once we are sure that all costs are - if ((use_time == false) && (pci.basic_info.use_time == true)) - PRINT_AND_THROW("No terms use time and basic_info is not set correctly. Try basic_info.use_time = false"); - - auto prob = std::make_shared(pci); - - // Generate initial trajectory and check its size - moveit::core::RobotModelConstPtr robot_model = pci.planning_scene->getRobotModel(); - moveit::core::RobotState current_state = pci.planning_scene->getCurrentState(); - - const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup(pci.planning_group_name); - int n_dof = prob->GetNumDOF(); - - std::vector current_joint_values; - current_state.copyJointGroupPositions(joint_model_group, current_joint_values); - - trajopt::TrajArray init_traj; - generateInitialTrajectory(pci, current_joint_values, init_traj); - - if (pci.basic_info.use_time == true) - { - prob->SetHasTime(true); - if (init_traj.rows() != n_steps || init_traj.cols() != n_dof + 1) - { - PRINT_AND_THROW(boost::format("Initial trajectory is not the right size matrix\n" - "Expected %i rows (time steps) x %i columns (%i dof + 1 time column)\n" - "Got %i rows and %i columns") % - n_steps % (n_dof + 1) % n_dof % init_traj.rows() % init_traj.cols()); - } - } - else - { - prob->SetHasTime(false); - if (init_traj.rows() != n_steps || init_traj.cols() != n_dof) - { - PRINT_AND_THROW(boost::format("Initial trajectory is not the right size matrix\n" - "Expected %i rows (time steps) x %i columns\n" - "Got %i rows and %i columns") % - n_steps % n_dof % init_traj.rows() % init_traj.cols()); - } - } - prob->SetInitTraj(init_traj); - - trajopt::VarArray matrix_traj_vars_temp; - // If start_fixed, constrain the joint values for the first time step to be their initialized values - if (bi.start_fixed) - { - if (init_traj.rows() < 1) - { - PRINT_AND_THROW("Initial trajectory must contain at least the start state."); - } - - if (init_traj.cols() != (n_dof + (use_time ? 1 : 0))) - { - PRINT_AND_THROW("robot dof values don't match initialization. I don't " - "know what you want me to use for the dof values"); - } - - for (int j = 0; j < static_cast(n_dof); ++j) - { - matrix_traj_vars_temp = prob->GetVars(); - prob->addLinearConstraint(sco::exprSub(sco::AffExpr(matrix_traj_vars_temp(0, j)), init_traj(0, j)), sco::EQ); - } - } - - // Apply constraint to each fixed dof to its initial value for all timesteps (freeze that column) - if (!bi.dofs_fixed.empty()) - { - for (int dof_ind : bi.dofs_fixed) - { - for (int i = 1; i < prob->GetNumSteps(); ++i) - { - matrix_traj_vars_temp = prob->GetVars(); - prob->addLinearConstraint(sco::exprSub(sco::AffExpr(matrix_traj_vars_temp(i, dof_ind)), - sco::AffExpr(init_traj(0, dof_ind))), - sco::EQ); - } - } - } - - for (const TermInfoPtr& ci : pci.cost_infos) - { - ci->addObjectiveTerms(*prob); - } - - for (const TermInfoPtr& ci : pci.cnt_infos) - { - ci->addObjectiveTerms(*prob); - } - return prob; -} - -CartPoseTermInfo::CartPoseTermInfo() : TermInfo(TT_COST | TT_CNT) -{ - pos_coeffs = Eigen::Vector3d::Ones(); - rot_coeffs = Eigen::Vector3d::Ones(); - tcp.setIdentity(); -} - -void CartPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob) -{ - unsigned int n_dof = prob.GetActiveGroupNumDOF(); - - Eigen::Isometry3d input_pose; - Eigen::Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); - input_pose.linear() = q.matrix(); - input_pose.translation() = xyz; - - if (term_type == (TT_COST | TT_USE_TIME)) - { - ROS_ERROR("Use time version of this term has not been defined."); - } - else if (term_type == (TT_CNT | TT_USE_TIME)) - { - ROS_ERROR("Use time version of this term has not been defined."); - } - else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) - { - sco::VectorOfVector::Ptr f = - std::make_shared(input_pose, prob.GetPlanningScene(), link, tcp); - prob.addCost(std::make_shared(f, prob.GetVarRow(timestep, 0, n_dof), - concatVector(rot_coeffs, pos_coeffs), sco::ABS, name)); - } - else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) - { - sco::VectorOfVector::Ptr f = - std::make_shared(input_pose, prob.GetPlanningScene(), link, tcp); - prob.addConstraint(std::make_shared( - f, prob.GetVarRow(timestep, 0, n_dof), concatVector(rot_coeffs, pos_coeffs), sco::EQ, name)); - } - else - { - ROS_WARN("CartPoseTermInfo does not have a valid term_type defined. No cost/constraint applied"); - } -} - -void JointPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob) -{ - unsigned int n_dof = prob.GetActiveGroupNumDOF(); - - // If optional parameter not given, set to default - if (coeffs.empty()) - coeffs = trajopt::DblVec(n_dof, 1); - if (upper_tols.empty()) - upper_tols = trajopt::DblVec(n_dof, 0); - if (lower_tols.empty()) - lower_tols = trajopt::DblVec(n_dof, 0); - if (last_step <= -1) - last_step = prob.GetNumSteps() - 1; - - // Check time step is valid - if ((prob.GetNumSteps() - 1) <= first_step) - first_step = prob.GetNumSteps() - 1; - if ((prob.GetNumSteps() - 1) <= last_step) - last_step = prob.GetNumSteps() - 1; - // if (last_step == first_step) - // last_step += 1; - if (last_step < first_step) - { - int tmp = first_step; - first_step = last_step; - last_step = tmp; - ROS_WARN("Last time step for JointPosTerm comes before first step. Reversing them."); - } - if (last_step == -1) // last_step not set - last_step = first_step; - - // Check if parameters are the correct size. - checkParameterSize(coeffs, n_dof, "JointPoseTermInfo coeffs", true); - checkParameterSize(targets, n_dof, "JointPoseTermInfo targets", true); - checkParameterSize(upper_tols, n_dof, "JointPoseTermInfo upper_tols", true); - checkParameterSize(lower_tols, n_dof, "JointPoseTermInfo lower_tols", true); - - // Check if tolerances are all zeros - bool is_upper_zeros = - std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); - bool is_lower_zeros = - std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); - - // Get vars associated with joints - trajopt::VarArray vars = prob.GetVars(); - trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); - if (prob.GetHasTime()) - ROS_INFO("JointPoseTermInfo does not differ based on setting of TT_USE_TIME"); - - if (term_type & TT_COST) - { - // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost - if (is_upper_zeros && is_lower_zeros) - { - prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), first_step, last_step)); - prob.getCosts().back()->setName(name); - } - else - { - prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step)); - prob.getCosts().back()->setName(name); - } - } - else if (term_type & TT_CNT) - { - // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint - if (is_upper_zeros && is_lower_zeros) - { - prob.addConstraint(std::make_shared( - joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step)); - prob.getConstraints().back()->setName(name); - } - else - { - prob.addConstraint(std::make_shared( - joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step)); - prob.getConstraints().back()->setName(name); - } - } - else - { - ROS_WARN("JointPosTermInfo does not have a valid term_type defined. No cost/constraint applied"); - } -} - -void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob) -{ - unsigned int n_dof = prob.GetNumDOF(); - - // If optional parameter not given, set to default - if (coeffs.empty()) - coeffs = trajopt::DblVec(n_dof, 1); - if (upper_tols.empty()) - upper_tols = trajopt::DblVec(n_dof, 0); - if (lower_tols.empty()) - lower_tols = trajopt::DblVec(n_dof, 0); - if (last_step <= -1) - last_step = prob.GetNumSteps() - 1; - - // If only one time step is desired, calculate velocity with next step (2 steps are needed for 1 velocity calculation) - if ((prob.GetNumSteps() - 2) <= first_step) - first_step = prob.GetNumSteps() - 2; - if ((prob.GetNumSteps() - 1) <= last_step) - last_step = prob.GetNumSteps() - 1; - if (last_step == first_step) - last_step += 1; - if (last_step < first_step) - { - int tmp = first_step; - first_step = last_step; - last_step = tmp; - ROS_WARN("Last time step for JointVelTerm comes before first step. Reversing them."); - } - - // Check if parameters are the correct size. - checkParameterSize(coeffs, n_dof, "JointVelTermInfo coeffs", true); - checkParameterSize(targets, n_dof, "JointVelTermInfo targets", true); - checkParameterSize(upper_tols, n_dof, "JointVelTermInfo upper_tols", true); - checkParameterSize(lower_tols, n_dof, "JointVelTermInfo lower_tols", true); - assert(last_step > first_step); - assert(first_step >= 0); - - // Check if tolerances are all zeros - bool is_upper_zeros = - std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); - bool is_lower_zeros = - std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); - - // Get vars associated with joints - trajopt::VarArray vars = prob.GetVars(); - trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); - - if (term_type == (TT_COST | TT_USE_TIME)) - { - unsigned num_vels = last_step - first_step; - - // Apply separate cost to each joint b/c that is how the error function is currently written - for (size_t j = 0; j < n_dof; ++j) - { - // Get a vector of a single column of vars - sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, j, last_step - first_step + 1); - sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); - - // If the tolerances are 0, an equality cost is set - if (is_upper_zeros && is_lower_zeros) - { - trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addCost(std::make_shared( - std::make_shared(targets[j], upper_tols[j], lower_tols[j]), - std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), - util::toVectorXd(single_jnt_coeffs), sco::SQUARED, name + "_j" + std::to_string(j))); - } - // Otherwise it's a hinged "inequality" cost - else - { - trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addCost(std::make_shared( - std::make_shared(targets[j], upper_tols[j], lower_tols[j]), - std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), - util::toVectorXd(single_jnt_coeffs), sco::HINGE, name + "_j" + std::to_string(j))); - } - } - } - else if (term_type == (TT_CNT | TT_USE_TIME)) - { - unsigned num_vels = last_step - first_step; - - // Apply separate cnt to each joint b/c that is how the error function is currently written - for (size_t j = 0; j < n_dof; ++j) - { - // Get a vector of a single column of vars - sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, j, last_step - first_step + 1); - sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); - - // If the tolerances are 0, an equality cnt is set - if (is_upper_zeros && is_lower_zeros) - { - trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addConstraint(std::make_shared( - std::make_shared(targets[j], upper_tols[j], lower_tols[j]), - std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), - util::toVectorXd(single_jnt_coeffs), sco::EQ, name + "_j" + std::to_string(j))); - } - // Otherwise it's a hinged "inequality" constraint - else - { - trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addConstraint(std::make_shared( - std::make_shared(targets[j], upper_tols[j], lower_tols[j]), - std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), - util::toVectorXd(single_jnt_coeffs), sco::INEQ, name + "_j" + std::to_string(j)))); - } - } - } - else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) - { - // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost - if (is_upper_zeros && is_lower_zeros) - { - prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), first_step, last_step)); - prob.getCosts().back()->setName(name); - } - else - { - prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step)); - prob.getCosts().back()->setName(name); - } - } - else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) - { - // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint - if (is_upper_zeros && is_lower_zeros) - { - prob.addConstraint(std::make_shared( - joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step)); - prob.getConstraints().back()->setName(name); - } - else - { - prob.addConstraint(std::make_shared( - joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step)); - prob.getConstraints().back()->setName(name); - } - } - else - { - ROS_WARN("JointVelTermInfo does not have a valid term_type defined. No cost/constraint applied"); - } -} - -void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, - trajopt::TrajArray& init_traj) -{ - Eigen::VectorXd current_pos(current_joint_values.size()); - for (int joint_index = 0; joint_index < current_joint_values.size(); ++joint_index) - { - current_pos(joint_index) = current_joint_values[joint_index]; - } - - InitInfo init_info = pci.init_info; - - // initialize based on type specified - if (init_info.type == InitInfo::STATIONARY) - { - // Initializes all joint values to the initial value (the current value in scene) - init_traj = current_pos.transpose().replicate(pci.basic_info.n_steps, 1); - } - else if (init_info.type == InitInfo::JOINT_INTERPOLATED) - { - // Linearly interpolates between initial value (current values in the scene) and the joint position specified in - // InitInfo.data - Eigen::VectorXd end_pos = init_info.data; - init_traj.resize(pci.basic_info.n_steps, end_pos.rows()); - for (int dof_index = 0; dof_index < current_pos.rows(); ++dof_index) - { - init_traj.col(dof_index) = - Eigen::VectorXd::LinSpaced(pci.basic_info.n_steps, current_pos(dof_index), end_pos(dof_index)); - } - } - else if (init_info.type == InitInfo::GIVEN_TRAJ) - { - // Initializes the matrix to a given trajectory - init_traj = init_info.data; - } - else - { - PRINT_AND_THROW("Init Info did not have a valid type. Valid types are " - "STATIONARY, JOINT_INTERPOLATED, or GIVEN_TRAJ"); - } - - // Currently all trajectories are generated without time then appended here - if (pci.basic_info.use_time) - { - // add on time (default to 1 sec) - init_traj.conservativeResize(Eigen::NoChange_t(), init_traj.cols() + 1); - - init_traj.block(0, init_traj.cols() - 1, init_traj.rows(), 1) = - Eigen::VectorXd::Constant(init_traj.rows(), init_info.dt); - } -} -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/src/trajopt_interface.cpp b/moveit_planners/trajopt/src/trajopt_interface.cpp deleted file mode 100644 index bf7e89387a..0000000000 --- a/moveit_planners/trajopt/src/trajopt_interface.cpp +++ /dev/null @@ -1,463 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, 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: Omid Heidari */ - -#include -#include - -#include - -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include - -namespace trajopt_interface -{ -TrajOptInterface::TrajOptInterface(const ros::NodeHandle& nh) : nh_(nh), name_("TrajOptInterface") -{ - trajopt_problem_ = std::make_shared(); - setDefaultTrajOPtParams(); - - // TODO: callbacks should be defined by the user - optimizer_callbacks_.push_back(callBackFunc); -} - -bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - moveit_msgs::MotionPlanDetailedResponse& res) -{ - ROS_INFO(" ======================================= From trajopt_interface, solve is called"); - setTrajOptParams(params_); - - if (!planning_scene) - { - ROS_ERROR_STREAM_NAMED(name_, "No planning scene initialized."); - res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; - return false; - } - - ROS_INFO(" ======================================= Extract current state information"); - ros::WallTime start_time = ros::WallTime::now(); - moveit::core::RobotModelConstPtr robot_model = planning_scene->getRobotModel(); - bool robot_model_ok = static_cast(robot_model); - if (!robot_model_ok) - ROS_ERROR_STREAM_NAMED(name_, "robot model is not loaded properly"); - auto current_state = std::make_shared(robot_model); - *current_state = planning_scene->getCurrentState(); - const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(req.group_name); - if (joint_model_group == nullptr) - ROS_ERROR_STREAM_NAMED(name_, "joint model group is empty"); - std::vector group_joint_names = joint_model_group->getActiveJointModelNames(); - int dof = group_joint_names.size(); - std::vector current_joint_values; - current_state->copyJointGroupPositions(joint_model_group, current_joint_values); - - // Current state is different from star state in general - ROS_INFO(" ======================================= Extract start state information"); - trajopt::DblVec start_joint_values = extractStartJointValues(req, group_joint_names); - - // check the start state for being empty or joint limit violiation - if (start_joint_values.empty()) - { - ROS_ERROR_STREAM_NAMED(name_, "Start_state is empty"); - res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; - return false; - } - - if (not joint_model_group->satisfiesPositionBounds(start_joint_values.data())) - { - ROS_ERROR_STREAM_NAMED(name_, "Start state violates joint limits"); - res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; - return false; - } - - ROS_INFO(" ======================================= Create ProblemInfo"); - ProblemInfo problem_info(planning_scene, req.group_name); - - setProblemInfoParam(problem_info); - - ROS_INFO(" ======================================= Populate init info, hard-coded"); - // TODO: init info should be defined by user. To this end, we need to add seed trajectories to MotionPlanRequest. - // JOINT_INTERPOLATED: data is the current joint values - // GIVEN_TRAJ: data is the joint values of the current state copied to all timesteps - Eigen::VectorXd current_joint_values_eigen(dof); - for (int joint_index = 0; joint_index < dof; ++joint_index) - { - current_joint_values_eigen(joint_index) = current_joint_values[joint_index]; - } - - if (problem_info.init_info.type == InitInfo::JOINT_INTERPOLATED) - { - problem_info.init_info.data = current_joint_values_eigen; - } - else if (problem_info.init_info.type == InitInfo::GIVEN_TRAJ) - { - problem_info.init_info.data = current_joint_values_eigen.transpose().replicate(problem_info.basic_info.n_steps, 1); - } - - ROS_INFO(" ======================================= Create Constraints"); - if (req.goal_constraints.empty()) - { - ROS_ERROR_STREAM_NAMED("trajopt_planner", "No goal constraints specified!"); - res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; - return false; - } - - ROS_INFO(" ======================================= Cartesian Constraints"); - if (!req.goal_constraints[0].position_constraints.empty() && !req.goal_constraints[0].orientation_constraints.empty()) - { - CartPoseTermInfoPtr cart_goal_pos = std::make_shared(); - - // TODO: Feed cart_goal_pos with request information and the needed param to the setup.yaml file - // TODO: Multiple Cartesian constraints - - // Add the constraint to problem_info - problem_info.cnt_infos.push_back(cart_goal_pos); - } - else if (req.goal_constraints[0].position_constraints.empty() && - !req.goal_constraints[0].orientation_constraints.empty()) - { - ROS_ERROR_STREAM_NAMED("trajopt_planner", "position constraint is not defined"); - res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; - return false; - } - else if (!req.goal_constraints[0].orientation_constraints.empty() && - req.goal_constraints[0].orientation_constraints.empty()) - { - ROS_ERROR_STREAM_NAMED("trajopt_planner", "orientation constraint is not defined"); - res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; - return false; - } - - ROS_INFO(" ======================================= Constraints from request goal_constraints"); - for (auto goal_cnt : req.goal_constraints) - { - JointPoseTermInfoPtr joint_pos_term = std::make_shared(); - // When using MotionPlanning Display in RViz, the created request has no name for the constraint - setJointPoseTermInfoParams(joint_pos_term, (goal_cnt.name != "") ? goal_cnt.name : "goal_tmp"); - - trajopt::DblVec joint_goal_constraints; - for (const moveit_msgs::JointConstraint& joint_goal_constraint : goal_cnt.joint_constraints) - { - joint_goal_constraints.push_back(joint_goal_constraint.position); - } - joint_pos_term->targets = joint_goal_constraints; - problem_info.cnt_infos.push_back(joint_pos_term); - } - - ROS_INFO(" ======================================= Constraints from request start_state"); - // add the start pos from request as a constraint - auto joint_start_pos = std::make_shared(); - - joint_start_pos->targets = start_joint_values; - setJointPoseTermInfoParams(joint_start_pos, "start_pos"); - problem_info.cnt_infos.push_back(joint_start_pos); - - ROS_INFO(" ======================================= Velocity Constraints, hard-coded"); - // TODO: should be defined by user, its parameters should be added to setup.yaml - auto joint_vel = std::make_shared(); - - joint_vel->coeffs = std::vector(dof, 5.0); - joint_vel->targets = std::vector(dof, 0.0); - joint_vel->first_step = 0; - joint_vel->last_step = problem_info.basic_info.n_steps - 1; - joint_vel->name = "joint_vel"; - joint_vel->term_type = trajopt_interface::TT_COST; - problem_info.cost_infos.push_back(joint_vel); - - ROS_INFO(" ======================================= Visibility Constraints"); - if (!req.goal_constraints[0].visibility_constraints.empty()) - { - // TODO: Add visibility constraint - } - - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.improve_ratio_threshold: " << params_.improve_ratio_threshold); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.min_trust_box_size: " << params_.min_trust_box_size); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.min_approx_improve: " << params_.min_approx_improve); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.params_.min_approx_improve_frac: " << params_.min_approx_improve_frac); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_iter: " << params_.max_iter); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_shrink_ratio: " << params_.trust_shrink_ratio); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_expand_ratio: " << params_.trust_expand_ratio); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.cnt_tolerance: " << params_.cnt_tolerance); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_merit_coeff_increases: " << params_.max_merit_coeff_increases); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.merit_coeff_increase_ratio: " << params_.merit_coeff_increase_ratio); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_time: " << params_.max_time); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.initial_merit_error_coeff: " << params_.initial_merit_error_coeff); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_box_size: " << params_.trust_box_size); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.n_steps: " << problem_info.basic_info.n_steps); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt_upper_lim: " << problem_info.basic_info.dt_upper_lim); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt_lower_lim: " << problem_info.basic_info.dt_lower_lim); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.start_fixed: " << problem_info.basic_info.start_fixed); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.use_time: " << problem_info.basic_info.use_time); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.convex_solver: " << problem_info.basic_info.convex_solver); - - std::string problem_info_type; - switch (problem_info.init_info.type) - { - case InitInfo::STATIONARY: - problem_info_type = "STATIONARY"; - break; - case InitInfo::JOINT_INTERPOLATED: - problem_info_type = "JOINT_INTERPOLATED"; - break; - case InitInfo::GIVEN_TRAJ: - problem_info_type = "GIVEN_TRAJ"; - break; - } - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.type: " << problem_info_type); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt: " << problem_info.init_info.dt); - - ROS_INFO(" ======================================= Construct problem"); - trajopt_problem_ = ConstructProblem(problem_info); - - ROS_INFO_STREAM_NAMED("num_cost", trajopt_problem_->getNumCosts()); - ROS_INFO_STREAM_NAMED("num_constraints", trajopt_problem_->getNumConstraints()); - - ROS_INFO(" ======================================= TrajOpt Optimization"); - sco::BasicTrustRegionSQP opt(trajopt_problem_); - - opt.setParameters(params_); - opt.initialize(trajopt::trajToDblVec(trajopt_problem_->GetInitTraj())); - - // Add all callbacks - for (const sco::Optimizer::Callback& callback : optimizer_callbacks_) - { - opt.addCallback(callback); - } - - // Optimize - ros::WallTime create_time = ros::WallTime::now(); - opt.optimize(); - - ROS_INFO(" ======================================= TrajOpt Solution"); - trajopt::TrajArray opt_solution = trajopt::getTraj(opt.x(), trajopt_problem_->GetVars()); - - // assume that the trajectory is now optimized, fill in the output structure: - ROS_INFO_STREAM_NAMED("num_rows", opt_solution.rows()); - ROS_INFO_STREAM_NAMED("num_cols", opt_solution.cols()); - - res.trajectory.resize(1); - res.trajectory[0].joint_trajectory.joint_names = group_joint_names; - res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header; - - // fill in the entire trajectory - res.trajectory[0].joint_trajectory.points.resize(opt_solution.rows()); - for (int i = 0; i < opt_solution.rows(); ++i) - { - res.trajectory[0].joint_trajectory.points[i].positions.resize(opt_solution.cols()); - for (size_t j = 0; j < opt_solution.cols(); ++j) - { - res.trajectory[0].joint_trajectory.points[i].positions[j] = opt_solution(i, j); - } - // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints. - res.trajectory[0].joint_trajectory.points[i].time_from_start = ros::Duration(0.0); - } - - res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - res.processing_time.push_back((ros::WallTime::now() - start_time).toSec()); - - ROS_INFO(" ======================================= check if final state is within goal tolerances"); - kinematic_constraints::JointConstraint joint_cnt(planning_scene->getRobotModel()); - moveit::core::RobotState last_state(*current_state); - last_state.setJointGroupPositions(req.group_name, res.trajectory[0].joint_trajectory.points.back().positions); - - for (int jn = 0; jn < res.trajectory[0].joint_trajectory.points.back().positions.size(); ++jn) - { - ROS_INFO_STREAM_NAMED("joint_value", res.trajectory[0].joint_trajectory.points.back().positions[jn] - << " " << req.goal_constraints.back().joint_constraints[jn].position); - } - - bool constraints_are_ok = true; - int joint_cnt_index = 0; - for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints.back().joint_constraints) - { - ROS_INFO("index %i: jc.configure(constraint)=> %d, jc.decide(last_state).satisfied=> %d, tolerance %f", - joint_cnt_index, joint_cnt.configure(constraint), joint_cnt.decide(last_state).satisfied, - constraint.tolerance_above); - constraints_are_ok = constraints_are_ok and joint_cnt.configure(constraint); - constraints_are_ok = constraints_are_ok and joint_cnt.decide(last_state).satisfied; - if (not constraints_are_ok) - { - ROS_ERROR_STREAM_NAMED("trajopt_planner", "Goal constraints are violated: " << constraint.joint_name); - res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED; - return false; - } - joint_cnt_index = joint_cnt_index + 1; - } - - ROS_INFO(" ==================================== Response"); - res.trajectory_start = req.start_state; - - ROS_INFO(" ==================================== Debug Response"); - ROS_INFO_STREAM_NAMED("group_name", res.group_name); - ROS_INFO_STREAM_NAMED("start_traj_name_size", res.trajectory_start.joint_state.name.size()); - ROS_INFO_STREAM_NAMED("start_traj_position_size", res.trajectory_start.joint_state.position.size()); - ROS_INFO_STREAM_NAMED("traj_name_size", res.trajectory[0].joint_trajectory.joint_names.size()); - ROS_INFO_STREAM_NAMED("traj_point_size", res.trajectory[0].joint_trajectory.points.size()); - return true; -} - -void TrajOptInterface::setDefaultTrajOPtParams() -{ - sco::BasicTrustRegionSQPParameters params; - params_ = params; -} - -void TrajOptInterface::setTrajOptParams(sco::BasicTrustRegionSQPParameters& params) -{ - nh_.param("trajopt_param/improve_ratio_threshold", params.improve_ratio_threshold, 0.25); - nh_.param("trajopt_param/min_trust_box_size", params.min_trust_box_size, 1e-4); - nh_.param("trajopt_param/min_approx_improve", params.min_approx_improve, 1e-4); - nh_.param("trajopt_param/min_approx_improve_frac", params.min_approx_improve_frac, -static_cast(INFINITY)); - nh_.param("trajopt_param/max_iter", params.max_iter, 100.0); - nh_.param("trajopt_param/trust_shrink_ratio", params.trust_shrink_ratio, 0.1); - - nh_.param("trajopt_param/trust_expand_ratio", params.trust_expand_ratio, 1.5); - nh_.param("trajopt_param/cnt_tolerance", params.cnt_tolerance, 1e-4); - nh_.param("trajopt_param/max_merit_coeff_increases", params.max_merit_coeff_increases, 5.0); - nh_.param("trajopt_param/merit_coeff_increase_ratio", params.merit_coeff_increase_ratio, 10.0); - nh_.param("trajopt_param/max_time", params.max_time, static_cast(INFINITY)); - nh_.param("trajopt_param/merit_error_coeff", params.initial_merit_error_coeff, 10.0); - nh_.param("trajopt_param/trust_box_size", params.trust_box_size, 1e-1); -} - -void TrajOptInterface::setProblemInfoParam(ProblemInfo& problem_info) -{ - nh_.param("problem_info/basic_info/n_steps", problem_info.basic_info.n_steps, 20); - nh_.param("problem_info/basic_info/dt_upper_lim", problem_info.basic_info.dt_upper_lim, 2.0); - nh_.param("problem_info/basic_info/dt_lower_lim", problem_info.basic_info.dt_lower_lim, 100.0); - nh_.param("problem_info/basic_info/start_fixed", problem_info.basic_info.start_fixed, true); - nh_.param("problem_info/basic_info/use_time", problem_info.basic_info.use_time, false); - int convex_solver_index; - nh_.param("problem_info/basic_info/convex_solver", convex_solver_index, 1); - switch (convex_solver_index) - { - case 1: - problem_info.basic_info.convex_solver = sco::ModelType::AUTO_SOLVER; - break; - case 2: - problem_info.basic_info.convex_solver = sco::ModelType::BPMPD; - break; - case 3: - problem_info.basic_info.convex_solver = sco::ModelType::OSQP; - break; - case 4: - problem_info.basic_info.convex_solver = sco::ModelType::QPOASES; - break; - case 5: - problem_info.basic_info.convex_solver = sco::ModelType::GUROBI; - break; - } - - nh_.param("problem_info/init_info/dt", problem_info.init_info.dt, 0.5); - int type_index; - nh_.param("problem_info/init_info/type", type_index, 1); - switch (type_index) - { - case 1: - problem_info.init_info.type = InitInfo::STATIONARY; - break; - case 2: - problem_info.init_info.type = InitInfo::JOINT_INTERPOLATED; - break; - case 3: - problem_info.init_info.type = InitInfo::GIVEN_TRAJ; - break; - } -} - -void TrajOptInterface::setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name) -{ - int term_type_index; - std::string term_type_address = "joint_pos_term_info/" + name + "/term_type"; - nh_.param(term_type_address, term_type_index, 1); - - switch (term_type_index) - { - case 1: - jp->term_type = TT_COST; - break; - case 2: - jp->term_type = TT_CNT; - break; - case 3: - jp->term_type = TT_USE_TIME; - break; - } - - nh_.getParam("joint_pos_term_info/" + name + "/first_timestep", jp->first_step); - nh_.getParam("joint_pos_term_info/" + name + "/last_timestep", jp->last_step); - nh_.getParam("joint_pos_term_info/" + name + "/name", jp->name); -} - -trajopt::DblVec TrajOptInterface::extractStartJointValues(const planning_interface::MotionPlanRequest& req, - const std::vector& group_joint_names) -{ - std::unordered_map all_joints; - trajopt::DblVec start_joint_vals; - - for (int joint_index = 0; joint_index < req.start_state.joint_state.position.size(); ++joint_index) - { - all_joints[req.start_state.joint_state.name[joint_index]] = req.start_state.joint_state.position[joint_index]; - } - - for (auto joint_name : group_joint_names) - { - ROS_INFO(" joint position from start state, name: %s, value: %f", joint_name.c_str(), all_joints[joint_name]); - start_joint_vals.push_back(all_joints[joint_name]); - } - - return start_joint_vals; -} - -void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res) -{ - // TODO: Create the actual implementation -} - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp b/moveit_planners/trajopt/src/trajopt_planner_manager.cpp deleted file mode 100644 index 055e093ebd..0000000000 --- a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, 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: Omid Heidari - Desc: TrajOpt planning plugin -*/ - -#include - -#include - -#include - -#include - -namespace trajopt_interface -{ -class TrajOptPlannerManager : public planning_interface::PlannerManager -{ -public: - TrajOptPlannerManager() : planning_interface::PlannerManager() - { - } - - bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) override - { - ROS_INFO(" ======================================= initialize gets called"); - - if (!ns.empty()) - nh_ = ros::NodeHandle(ns); - std::string trajopt_ns = ns.empty() ? "trajopt" : ns + "/trajopt"; - - for (const std::string& gpName : model->getJointModelGroupNames()) - { - ROS_INFO(" ======================================= group name: %s, robot model: %s", gpName.c_str(), - model->getName().c_str()); - planning_contexts_[gpName] = std::make_shared("trajopt_planning_context", gpName, model); - } - - return true; - } - - bool canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const override - { - return req.trajectory_constraints.constraints.empty(); - } - - std::string getDescription() const override - { - return "TrajOpt"; - } - - void getPlanningAlgorithms(std::vector& algs) const override - { - algs.clear(); - algs.push_back("trajopt"); - } - - planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - moveit_msgs::MoveItErrorCodes& error_code) const override - { - ROS_INFO(" ======================================= getPlanningContext() is called "); - error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - - if (req.group_name.empty()) - { - ROS_ERROR("No group specified to plan for"); - error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; - return planning_interface::PlanningContextPtr(); - } - - if (!planning_scene) - { - ROS_ERROR("No planning scene supplied as input"); - error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; - return planning_interface::PlanningContextPtr(); - } - - // create PlanningScene using hybrid collision detector - planning_scene::PlanningScenePtr ps = planning_scene->diff(); - - // set FCL for the collision - ps->allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); - - // retrieve and configure existing context - const TrajOptPlanningContextPtr& context = planning_contexts_.at(req.group_name); - - ROS_INFO(" ======================================= context is made "); - - context->setPlanningScene(ps); - context->setMotionPlanRequest(req); - - error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - - return context; - } - -private: - ros::NodeHandle nh_; - -protected: - std::map planning_contexts_; -}; - -} // namespace trajopt_interface - -// register the TrajOptPlannerManager class as a plugin -CLASS_LOADER_REGISTER_CLASS(trajopt_interface::TrajOptPlannerManager, planning_interface::PlannerManager); diff --git a/moveit_planners/trajopt/src/trajopt_planning_context.cpp b/moveit_planners/trajopt/src/trajopt_planning_context.cpp deleted file mode 100644 index 4a7405642e..0000000000 --- a/moveit_planners/trajopt/src/trajopt_planning_context.cpp +++ /dev/null @@ -1,75 +0,0 @@ -#include -#include -#include -#include - -#include - -#include - -#include -#include - -namespace trajopt_interface -{ -TrajOptPlanningContext::TrajOptPlanningContext(const std::string& context_name, const std::string& group_name, - const moveit::core::RobotModelConstPtr& model) - : planning_interface::PlanningContext(context_name, group_name), robot_model_(model) -{ - ROS_INFO(" ======================================= TrajOptPlanningContext is constructed"); - trajopt_interface_ = std::make_shared(); -} - -bool TrajOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) -{ - moveit_msgs::MotionPlanDetailedResponse res_msg; - bool trajopt_solved = trajopt_interface_->solve(planning_scene_, request_, res_msg); - - if (trajopt_solved) - { - res.trajectory.resize(1); - res.trajectory[0] = std::make_shared(robot_model_, getGroupName()); - - moveit::core::RobotState start_state(robot_model_); - moveit::core::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); - res.trajectory[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); - - res.description.push_back("plan"); - // TODO: Add the initial trajectory to res (MotionPlanDetailedResponse) - res.processing_time = res_msg.processing_time; - res.error_code = res_msg.error_code; - return true; - } - else - { - res.error_code = res_msg.error_code; - return false; - } -} - -bool TrajOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) -{ - planning_interface::MotionPlanDetailedResponse res_detailed; - bool planning_success = solve(res_detailed); - - res.error_code = res_detailed.error_code; - - if (planning_success) - { - res.trajectory = res_detailed.trajectory[0]; - res.planning_time = res_detailed.processing_time[0]; - } - - return planning_success; -} - -bool TrajOptPlanningContext::terminate() -{ - ROS_ERROR_STREAM_NAMED("trajopt_planning_context", "TrajOpt is not interruptible yet"); - return false; -} -void TrajOptPlanningContext::clear() -{ -} - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/test/trajectory_test.cpp b/moveit_planners/trajopt/test/trajectory_test.cpp deleted file mode 100644 index 53a44b052b..0000000000 --- a/moveit_planners/trajopt/test/trajectory_test.cpp +++ /dev/null @@ -1,212 +0,0 @@ -/********************************************************************* - * Software License Agreement - * - * Copyright (c) 2019, PickNik Inc. - * All rights reserved. - * - *********************************************************************/ - -/* Author: Omid Heidari - Desc: Test the trajectory planned by trajopt - */ - -// C++ -#include -#include -#include - -// ROS -#include - -// Testing -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -class TrajectoryTest : public ::testing::Test -{ -public: - TrajectoryTest() - { - } - -protected: - void SetUp() override - { - node_handle_ = ros::NodeHandle("~"); - robot_model_ = moveit::core::loadTestingRobotModel("panda"); - bool robot_model_ok_ = static_cast(robot_model_); - if (!robot_model_ok_) - ROS_ERROR_STREAM_NAMED("trajectory_test", "robot model is not loaded correctly"); - } - -protected: - moveit::core::RobotModelPtr robot_model_; - std::vector group_joint_names_; - const std::string PLANNING_GROUP = "panda_arm"; - const double GOAL_TOLERANCE = 0.1; - ros::NodeHandle node_handle_; -}; // class TrajectoryTest - -TEST_F(TrajectoryTest, concatVectorValidation) -{ - std::vector vec_a = { 1, 2, 3, 4, 5 }; - std::vector vec_b = { 6, 7, 8, 9, 10 }; - std::vector vec_c = trajopt_interface::concatVector(vec_a, vec_b); - EXPECT_EQ(vec_c.size(), vec_a.size() + vec_b.size()); - - // Check if the output of concatVector is correct. - // Loop over the output and the input vectors to see if they match - std::size_t length_ab = vec_a.size() + vec_b.size(); - for (std::size_t index = 0; index < length_ab; ++index) - { - if (index < vec_a.size()) - { - EXPECT_EQ(vec_c[index], vec_a[index]); - } - else - { - EXPECT_EQ(vec_c[index], vec_b[index - vec_a.size()]); - } - } -} - -TEST_F(TrajectoryTest, goalTolerance) -{ - const std::string NODE_NAME = "trajectory_test"; - - // Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group - auto current_state = std::make_shared(robot_model_); - current_state->setToDefaultValues(); - - const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLANNING_GROUP); - EXPECT_NE(joint_model_group, nullptr); - const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); - EXPECT_EQ(joint_names.size(), 7); - - auto planning_scene = std::make_shared(robot_model_); - - // Create response and request - planning_interface::MotionPlanRequest req; - planning_interface::MotionPlanResponse res; - - // Set start state - // ====================================================================================== - std::vector start_joint_values = { 0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 }; - auto start_state = std::make_shared(robot_model_); - start_state->setJointGroupPositions(joint_model_group, start_joint_values); - start_state->update(); - - req.start_state.joint_state.name = joint_names; - req.start_state.joint_state.position = start_joint_values; - req.goal_constraints.clear(); - req.group_name = PLANNING_GROUP; - - // Set the goal state and joints tolerance - // ======================================================================================== - auto goal_state = std::make_shared(robot_model_); - std::vector goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 }; - goal_state->setJointGroupPositions(joint_model_group, goal_joint_values); - goal_state->update(); - moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(*goal_state, joint_model_group); - req.goal_constraints.push_back(joint_goal); - req.goal_constraints[0].name = "goal_pos"; - // Set joint tolerance - std::vector goal_joint_constraint = req.goal_constraints[0].joint_constraints; - for (std::size_t x = 0; x < goal_joint_constraint.size(); ++x) - { - req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.001; - req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.001; - } - - // Load planner - // ====================================================================================== - // We will now construct a loader to load a planner, by name. - // Note that we are using the ROS pluginlib library here. - std::unique_ptr> planner_plugin_loader; - planning_interface::PlannerManagerPtr planner_instance; - - std::string planner_plugin_name = "trajopt_interface/TrajOptPlanner"; - node_handle_.setParam("planning_plugin", planner_plugin_name); - - // Make sure the planner plugin is loaded - EXPECT_TRUE(node_handle_.getParam("planning_plugin", planner_plugin_name)); - try - { - planner_plugin_loader = std::make_shared>( - "moveit_core", "planning_interface::PlannerManager"); - } - catch (pluginlib::PluginlibException& ex) - { - ROS_FATAL_STREAM_NAMED(NODE_NAME, "Exception while creating planning plugin loader " << ex.what()); - } - try - { - planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); - if (!planner_instance->initialize(robot_model_, node_handle_.getNamespace())) - ROS_FATAL_STREAM_NAMED(NODE_NAME, "Could not initialize planner instance"); - ROS_INFO_STREAM_NAMED(NODE_NAME, "Using planning interface '" << planner_instance->getDescription() << '\''); - } - catch (pluginlib::PluginlibException& ex) - { - const std::vector& classes = planner_plugin_loader->getDeclaredClasses(); - std::stringstream ss; - for (std::size_t i = 0; i < classes.size(); ++i) - ss << classes[i] << ' '; - ROS_ERROR_STREAM_NAMED(NODE_NAME, "Exception while loading planner '" << planner_plugin_name << "': " << ex.what() - << '\n' - << "Available plugins: " << ss.str()); - } - - // Create planning context - // ======================================================================================== - planning_interface::PlanningContextPtr context = - planner_instance->getPlanningContext(planning_scene, req, res.error_code); - - context->solve(res); - EXPECT_EQ(res.error_code.val, res.error_code.SUCCESS); - - moveit_msgs::MotionPlanResponse response; - res.getMessage(response); - - // Check the difference between the last step in the solution and the goal - // ======================================================================================== - std::vector joints_values_last_step = response.trajectory.joint_trajectory.points.back().positions; - - for (std::size_t joint_index = 0; joint_index < joints_values_last_step.size(); ++joint_index) - { - double goal_error = - abs(joints_values_last_step[joint_index] - req.goal_constraints[0].joint_constraints[joint_index].position); - std::cerr << "goal_error: " << goal_error << '\n'; - EXPECT_LT(goal_error, GOAL_TOLERANCE); - } -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "trajectory_test"); - - ros::AsyncSpinner spinner(1); - spinner.start(); - - int result = RUN_ALL_TESTS(); - ros::shutdown(); - - return result; -} diff --git a/moveit_planners/trajopt/test/trajectory_test.test b/moveit_planners/trajopt/test/trajectory_test.test deleted file mode 100644 index bec08fda3d..0000000000 --- a/moveit_planners/trajopt/test/trajectory_test.test +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/moveit_planners/trajopt/trajopt_interface_plugin_description.xml b/moveit_planners/trajopt/trajopt_interface_plugin_description.xml deleted file mode 100644 index 051864285a..0000000000 --- a/moveit_planners/trajopt/trajopt_interface_plugin_description.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The trajopt motion planner plugin which implements sequential convex optimization to solve motion planning problem. - - - diff --git a/moveit_setup_assistant/unported_templates/README.md b/moveit_setup_assistant/unported_templates/README.md index 8bb545d5d8..33dc6ec378 100644 --- a/moveit_setup_assistant/unported_templates/README.md +++ b/moveit_setup_assistant/unported_templates/README.md @@ -2,4 +2,3 @@ * `joystick_control.launch` - Control the Rviz Motion Planning Plugin with a joystick * `ompl-chomp_planning_pipeline.launch.xml` - * `run_benchmark_ompl.launch` - Launch file for benchmarking OMPL planners - * `run_benchmark_trajopt.launch` diff --git a/moveit_setup_assistant/unported_templates/run_benchmark_trajopt.launch b/moveit_setup_assistant/unported_templates/run_benchmark_trajopt.launch deleted file mode 100644 index 50631869af..0000000000 --- a/moveit_setup_assistant/unported_templates/run_benchmark_trajopt.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - From f3559d02ad7911faf8bc3d878b6dafae61e1c613 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 8 Dec 2023 11:58:24 -0700 Subject: [PATCH 08/17] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20actions/st?= =?UTF-8?q?ale=20from=208=20to=209=20(#2590)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bumps [actions/stale](https://github.com/actions/stale) from 8 to 9. - [Release notes](https://github.com/actions/stale/releases) - [Changelog](https://github.com/actions/stale/blob/main/CHANGELOG.md) - [Commits](https://github.com/actions/stale/compare/v8...v9) --- updated-dependencies: - dependency-name: actions/stale dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/stale.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/stale.yaml b/.github/workflows/stale.yaml index 9fde349d0e..9e3e867e9d 100644 --- a/.github/workflows/stale.yaml +++ b/.github/workflows/stale.yaml @@ -12,7 +12,7 @@ jobs: issues: write pull-requests: write steps: - - uses: actions/stale@v8 + - uses: actions/stale@v9 with: stale-issue-label: 'stale' stale-pr-label: 'stale' From 7e8431a855a11f90c245db9ac9ce36d907b81888 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Sat, 9 Dec 2023 12:56:36 -0700 Subject: [PATCH 09/17] Add missing header (#2592) --- .../include/moveit/planning_interface/planning_request_adapter.h | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h index 1a25e49f5c..f001f6792d 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request_adapter.h @@ -40,6 +40,7 @@ #include #include +#include #include #include From b2a3b16b2a3572bde9fa5378b246a5305da30a5d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 12 Dec 2023 06:59:28 -0600 Subject: [PATCH 10/17] Sync Ruckig with MoveIt1 (#2596) * Debug Ruckig tests (MoveIt1 3300) * Add a test, termination condition bugfix (MoveIt1 3348) * Mitigate Ruckig overshoot (MoveIt1 3376) * Small variable fixup --- .../src/ruckig_traj_smoothing.cpp | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index bfa88e450d..31a72d3265 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -50,7 +50,7 @@ namespace constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2 constexpr double DEFAULT_MAX_JERK = 1000; // rad/s^3 -constexpr double MAX_DURATION_EXTENSION_FACTOR = 10.0; +constexpr double MAX_DURATION_EXTENSION_FACTOR = 50.0; constexpr double DURATION_EXTENSION_FRACTION = 1.1; // If "mitigate_overshoot" is enabled, overshoot is checked with this timestep constexpr double OVERSHOOT_CHECK_PERIOD = 0.01; // sec @@ -251,7 +251,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, const size_t num_waypoints = trajectory.getWayPointCount(); const moveit::core::JointModelGroup* const group = trajectory.getGroup(); const size_t num_dof = group->getVariableCount(); - ruckig::OutputParameter ruckig_output{ num_dof }; + ruckig::Trajectory ruckig_output(num_dof); // This lib does not work properly when angles wrap, so we need to unwind the path first trajectory.unwind(); @@ -268,7 +268,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, double duration_extension_factor = 1; bool smoothing_complete = false; size_t waypoint_idx = 0; - while ((duration_extension_factor < MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete) + while ((duration_extension_factor <= MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete) { while (waypoint_idx < num_waypoints - 1) { @@ -278,15 +278,14 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, getNextRuckigInput(curr_waypoint, next_waypoint, group, ruckig_input); // Run Ruckig - ruckig::Trajectory ruckig_trajectory(num_dof); - ruckig_result = ruckig.calculate(ruckig_input, ruckig_trajectory); + ruckig_result = ruckig.calculate(ruckig_input, ruckig_output); // Step through the trajectory at the given OVERSHOOT_CHECK_PERIOD and check for overshoot. // We will extend the duration to mitigate it. bool overshoots = false; if (mitigate_overshoot) { - overshoots = checkOvershoot(ruckig_trajectory, num_dof, ruckig_input, overshoot_threshold); + overshoots = checkOvershoot(ruckig_output, num_dof, ruckig_input, overshoot_threshold); } // The difference between Result::Working and Result::Finished is that Finished can be reached in one @@ -297,7 +296,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, if (!overshoots && (waypoint_idx == num_waypoints - 2) && (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished)) { - trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_trajectory.get_duration()); + trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_output.get_duration()); smoothing_complete = true; break; } @@ -306,19 +305,27 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, if (overshoots || (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished)) { duration_extension_factor *= DURATION_EXTENSION_FRACTION; + // Reset the trajectory + trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */); const std::vector& move_group_idx = group->getVariableIndexList(); extendTrajectoryDuration(duration_extension_factor, waypoint_idx, num_dof, move_group_idx, original_trajectory, trajectory); initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input); - // Continue the loop from failed segment, but with increased duration extension factor + // Begin the for() loop again break; } ++waypoint_idx; } } + if (duration_extension_factor > MAX_DURATION_EXTENSION_FACTOR) + { + RCLCPP_ERROR_STREAM(getLogger(), + "Ruckig extended the trajectory duration to its maximum and still did not find a solution"); + } + if (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished) { RCLCPP_ERROR_STREAM(getLogger(), "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result); @@ -347,7 +354,6 @@ void RuckigSmoothing::extendTrajectoryDuration(const double duration_extension_f target_state->setVariableVelocity(move_group_idx.at(joint), (1 / duration_extension_factor) * target_state->getVariableVelocity(move_group_idx.at(joint))); - double prev_velocity = prev_state->getVariableVelocity(move_group_idx.at(joint)); double curr_velocity = target_state->getVariableVelocity(move_group_idx.at(joint)); target_state->setVariableAcceleration(move_group_idx.at(joint), (curr_velocity - prev_velocity) / timestep); From 754d9191ab791744fa2aafbf08eb04b0e4ada5a7 Mon Sep 17 00:00:00 2001 From: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Date: Tue, 12 Dec 2023 19:54:45 +0100 Subject: [PATCH 11/17] Replaced single value joint_limit_margin with list of joint_limit_margin (#2576) * Replaced joint_limit_margin with list of margins: joint_limit_margin. Enabling setting individual margins for each joint. * Dimension comment update * Adding a dimension check within the validateParams() function of servo.cpp to give a clear error message if the size of joint_limit_margis does not match the number of joints of the move_group * Formatting fix Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * Fix panda_simulated_config.yaml --------- Co-authored-by: AndyZe Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../config/panda_simulated_config.yaml | 2 +- .../moveit_servo/config/servo_parameters.yaml | 8 ++++---- .../moveit_servo/config/test_config_panda.yaml | 2 +- .../include/moveit_servo/utils/common.hpp | 4 ++-- moveit_ros/moveit_servo/src/servo.cpp | 14 +++++++++++++- moveit_ros/moveit_servo/src/utils/common.cpp | 6 +++--- 6 files changed, 24 insertions(+), 12 deletions(-) diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index 9ff8ea16bc..748544e4b6 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -41,7 +41,7 @@ move_group_name: panda_arm # Often 'manipulator' or 'arm' ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.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. +joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) ## Topic names diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml index 56696b1742..1bd2fcef94 100644 --- a/moveit_ros/moveit_servo/config/servo_parameters.yaml +++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml @@ -306,12 +306,12 @@ servo: description: "Halt all joints in cartesian mode, else halt only the joints at their limit" } - joint_limit_margin: { - type: double, - default_value: 0.1, + joint_limit_margins: { + type: double_array, + default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1], description: "Added as a buffer to joint limits [radians]. If moving quickly, make this larger.", validation: { - gt<>: 0.0 + lower_element_bounds<>: 0.0 } } diff --git a/moveit_ros/moveit_servo/config/test_config_panda.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml index 987af440fe..d0ad02adf5 100644 --- a/moveit_ros/moveit_servo/config/test_config_panda.yaml +++ b/moveit_ros/moveit_servo/config/test_config_panda.yaml @@ -42,7 +42,7 @@ move_group_name: panda_arm # Often 'manipulator' or 'arm' ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.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. +joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) ## Topic names diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp index c897fe7106..78904d3863 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp @@ -153,11 +153,11 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, * @param positions The joint positions. * @param velocities The current commanded velocities. * @param joint_bounds The allowable limits for the robot joints. - * @param margin Additional buffer on the actual joint limits. + * @param margins Additional buffer on the actual joint limits. * @return The joints that are violating the specified position limits. */ std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const moveit::core::JointBoundsVector& joint_bounds, double margin); + const moveit::core::JointBoundsVector& joint_bounds, const std::vector& margins); /** * \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped. diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 40229ebf28..6310b83d47 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -246,6 +246,18 @@ bool Servo::validateParams(const servo::Params& servo_params) const servo_params.active_subgroup.c_str(), servo_params.move_group_name.c_str()); params_valid = false; } + if (servo_params.joint_limit_margins.size() != + robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount()) + { + RCLCPP_ERROR(logger_, + "Parameter 'joint_limit_margins' must have the same number of elements as the number of joints in the " + "move_group. " + "Size of 'joint_limit_margins' is '%li', but number of joints in '%s' is '%i'. " + "Check the parameters YAML file used to launch this node.", + servo_params.joint_limit_margins.size(), servo_params.move_group_name.c_str(), + robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount()); + params_valid = false; + } return params_valid; } @@ -503,7 +515,7 @@ KinematicState Servo::getNextJointState(const ServoInput& command) // Check if any joints are going past joint position limits const std::vector joints_to_halt = - jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, servo_params_.joint_limit_margin); + jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, servo_params_.joint_limit_margins); // Apply halting if any joints need to be halted. if (!joints_to_halt.empty()) diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index ea630f4ab6..2a4a4817b0 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -341,7 +341,7 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, } std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, - const moveit::core::JointBoundsVector& joint_bounds, double margin) + const moveit::core::JointBoundsVector& joint_bounds, const std::vector& margins) { std::vector joint_idxs_to_halt; for (size_t i = 0; i < joint_bounds.size(); i++) @@ -349,8 +349,8 @@ std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::Vec const auto joint_bound = (joint_bounds[i])->front(); if (joint_bound.position_bounded_) { - const bool negative_bound = velocities[i] < 0 && positions[i] < (joint_bound.min_position_ + margin); - const bool positive_bound = velocities[i] > 0 && positions[i] > (joint_bound.max_position_ - margin); + const bool negative_bound = velocities[i] < 0 && positions[i] < (joint_bound.min_position_ + margins[i]); + const bool positive_bound = velocities[i] > 0 && positions[i] > (joint_bound.max_position_ - margins[i]); if (negative_bound || positive_bound) { joint_idxs_to_halt.push_back(i); From a7879726818667fd3e2c3e9036856aff0048103a Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 12 Dec 2023 20:36:31 +0100 Subject: [PATCH 12/17] Remove LMA kinematics plugin (#2595) --- .dockerignore | 2 - .github/CODEOWNERS.disabled | 1 - MIGRATION.md | 1 + moveit_kinematics/CMakeLists.txt | 5 - .../src/cached_ik_kinematics_plugin.cpp | 6 - .../lma_kinematics_plugin/CMakeLists.txt | 20 - .../lma_kinematics_plugin.h | 162 -------- .../src/lma_kinematics_parameters.yaml | 37 -- .../src/lma_kinematics_plugin.cpp | 364 ------------------ .../lma_kinematics_plugin_description.xml | 7 - moveit_kinematics/package.xml | 1 - moveit_kinematics/test/CMakeLists.txt | 7 - .../test/config/fanuc-lma-singular-test.yaml | 201 ---------- .../test/config/fanuc-lma-test.yaml | 137 ------- .../test/config/panda-lma-singular-test.yaml | 45 --- .../test/config/panda-lma-test.yaml | 40 -- .../test/launch/fanuc-lma-singular.test.py | 51 --- .../test/launch/fanuc-lma.test.py | 51 --- .../test/launch/panda-lma-singular.test.py | 51 --- .../test/launch/panda-lma.test.py | 51 --- 20 files changed, 1 insertion(+), 1239 deletions(-) delete mode 100644 moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt delete mode 100644 moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h delete mode 100644 moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_parameters.yaml delete mode 100644 moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp delete mode 100644 moveit_kinematics/lma_kinematics_plugin_description.xml delete mode 100644 moveit_kinematics/test/config/fanuc-lma-singular-test.yaml delete mode 100644 moveit_kinematics/test/config/fanuc-lma-test.yaml delete mode 100644 moveit_kinematics/test/config/panda-lma-singular-test.yaml delete mode 100644 moveit_kinematics/test/config/panda-lma-test.yaml delete mode 100644 moveit_kinematics/test/launch/fanuc-lma-singular.test.py delete mode 100644 moveit_kinematics/test/launch/fanuc-lma.test.py delete mode 100644 moveit_kinematics/test/launch/panda-lma-singular.test.py delete mode 100644 moveit_kinematics/test/launch/panda-lma.test.py diff --git a/.dockerignore b/.dockerignore index d5ac92b441..250f1b74de 100644 --- a/.dockerignore +++ b/.dockerignore @@ -17,11 +17,9 @@ !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 diff --git a/.github/CODEOWNERS.disabled b/.github/CODEOWNERS.disabled index 960b877e64..69edcdd359 100644 --- a/.github/CODEOWNERS.disabled +++ b/.github/CODEOWNERS.disabled @@ -91,7 +91,6 @@ /moveit_planners/ompl/ @BryceStevenWilley @mamoll /moveit_planners/chomp/chomp_interface/ @raghavendersahdev @knorth55 @bmagyar -/moveit_planners/chomp/chomp_optimizer_adapter/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/chomp/chomp_motion_planner/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/pilz_industrial_motion_planner @jschleicher @ct2034 /moveit_planners/pilz_industrial_motion_planner_testutils/ @jschleicher @ct2034 diff --git a/MIGRATION.md b/MIGRATION.md index 3d92437e69..8a851b09d2 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,6 +3,7 @@ API changes in MoveIt releases ## ROS Rolling +- [12/2023] LMA kinematics plugin is removed to remove the maintenance work because better alternatives exist like KDL or TracIK - [10/2023] The planning pipeline now has a vector of planner plugins rather than a single one. Please update the planner plugin parameter e.g. like this: ```diff - planning_plugin: ompl_interface/OMPLPlanner diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index 1d9ab713db..06615d6140 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -25,7 +25,6 @@ include(ConfigExtras.cmake) set(THIS_PACKAGE_INCLUDE_DIRS kdl_kinematics_plugin/include - lma_kinematics_plugin/include srv_kinematics_plugin/include cached_ik_kinematics_plugin/include ) @@ -36,8 +35,6 @@ set(THIS_PACKAGE_LIBRARIES moveit_cached_ik_kinematics_plugin kdl_kinematics_parameters moveit_kdl_kinematics_plugin - lma_kinematics_parameters - moveit_lma_kinematics_plugin srv_kinematics_parameters moveit_srv_kinematics_plugin ) @@ -58,7 +55,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ) pluginlib_export_plugin_description_file(moveit_core kdl_kinematics_plugin_description.xml) -pluginlib_export_plugin_description_file(moveit_core lma_kinematics_plugin_description.xml) pluginlib_export_plugin_description_file(moveit_core srv_kinematics_plugin_description.xml) pluginlib_export_plugin_description_file(moveit_core cached_ik_kinematics_plugin_description.xml) @@ -67,7 +63,6 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) add_subdirectory(cached_ik_kinematics_plugin) add_subdirectory(ikfast_kinematics_plugin) add_subdirectory(kdl_kinematics_plugin) -add_subdirectory(lma_kinematics_plugin) add_subdirectory(srv_kinematics_plugin) add_subdirectory(test) diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp b/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp index d565018280..e103aa2637 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp +++ b/moveit_kinematics/cached_ik_kinematics_plugin/src/cached_ik_kinematics_plugin.cpp @@ -36,8 +36,6 @@ #include #include -// compilation error: KDL and LMA kinematics plugins declare same types -//#include #include #include @@ -45,10 +43,6 @@ PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, kinematics::KinematicsBase); -// register CachedIKKinematicsPlugin as a KinematicsBase implementation -// PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, -// kinematics::KinematicsBase); - // register CachedIKKinematicsPlugin as a KinematicsBase implementation PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin, kinematics::KinematicsBase); diff --git a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt deleted file mode 100644 index 4e1a0c98ab..0000000000 --- a/moveit_kinematics/lma_kinematics_plugin/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -generate_parameter_library( - lma_kinematics_parameters # cmake target name for the parameter library - src/lma_kinematics_parameters.yaml # path to input yaml file -) - -add_library(moveit_lma_kinematics_plugin SHARED src/lma_kinematics_plugin.cpp) -set_target_properties(moveit_lma_kinematics_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - -ament_target_dependencies(moveit_lma_kinematics_plugin - rclcpp - moveit_core - moveit_msgs - tf2_kdl -) - -target_link_libraries(moveit_lma_kinematics_plugin - lma_kinematics_parameters -) - -install(DIRECTORY include/ DESTINATION include/moveit_kinematics) diff --git a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h b/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h deleted file mode 100644 index 7143013cef..0000000000 --- a/moveit_kinematics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h +++ /dev/null @@ -1,162 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, CRI group, NTU, Singapore - * 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 CRI group 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: Francisco Suarez-Ruiz */ - -#pragma once - -// ROS2 -#include -#include -#include - -// ROS msgs -#include -#include -#include -#include -#include - -// KDL -#include -#include -#include - -// MoveIt -#include -#include -#include - -namespace lma_kinematics_plugin -{ -/** - * @brief Implementation of kinematics using Levenberg-Marquardt (LMA) solver from KDL. - * This version supports any kinematic chain without mimic joints. - */ -class LMAKinematicsPlugin : public kinematics::KinematicsBase -{ -public: - /** - * @brief Default constructor - */ - LMAKinematicsPlugin(); - - bool - getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool searchPositionIK( - const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; - - bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, - std::vector& poses) const override; - - bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, - const std::string& group_name, const std::string& base_frame, - const std::vector& tip_frames, double search_discretization) override; - - /** - * @brief Return all the joint names in the order they are used internally - */ - const std::vector& getJointNames() const override; - - /** - * @brief Return all the link names in the order they are represented internally - */ - const std::vector& getLinkNames() const override; - -private: - bool timedOut(const rclcpp::Time& start_time, double duration) const; - - /** @brief Check whether the solution lies within the consistency limits of the seed state - * @param seed_state Seed state - * @param consistency_limits - * @param solution solution configuration - * @return true if check succeeds - */ - bool checkConsistency(const Eigen::VectorXd& seed_state, const std::vector& consistency_limits, - const Eigen::VectorXd& solution) const; - /** Check whether joint values satisfy joint limits */ - bool obeysLimits(const Eigen::VectorXd& values) const; - /** Harmonize revolute joint values into the range -2 Pi .. 2 Pi */ - void harmonize(Eigen::VectorXd& values) const; - - void getRandomConfiguration(Eigen::VectorXd& jnt_array) const; - - /** @brief Get a random configuration within consistency limits close to the seed state - * @param seed_state Seed state - * @param consistency_limits - * @param jnt_array Returned random configuration - */ - void getRandomConfiguration(const Eigen::VectorXd& seed_state, const std::vector& consistency_limits, - Eigen::VectorXd& jnt_array) const; - - bool initialized_; ///< Internal variable that indicates whether solver is configured and ready - - unsigned int dimension_; ///< Dimension of the group - moveit_msgs::msg::KinematicSolverInfo solver_info_; ///< Stores information for the inverse kinematics solver - - const moveit::core::JointModelGroup* joint_model_group_; - moveit::core::RobotStatePtr state_; - KDL::Chain kdl_chain_; - std::unique_ptr fk_solver_; - std::vector joints_; - std::vector joint_names_; - rclcpp::Node::SharedPtr node_; - - std::shared_ptr param_listener_; - lma_kinematics::Params params_; -}; -} // namespace lma_kinematics_plugin diff --git a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_parameters.yaml b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_parameters.yaml deleted file mode 100644 index 6d55ae2610..0000000000 --- a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_parameters.yaml +++ /dev/null @@ -1,37 +0,0 @@ -lma_kinematics: - - max_solver_iterations: { - type: int, - default_value: 500, - description: "Maximum solver iterations", - validation: { - gt_eq<>: [ 0.0 ] - } - } - - epsilon: { - type: double, - default_value: 0.00001, - description: "Epsilon. Default is 1e-5", - validation: { - gt<>: [ 0.0 ] - } - } - - orientation_vs_position: { - type: double, - default_value: 1.0, - description: "Weight of orientation error vs position error - * < 1.0: orientation has less importance than position - * > 1.0: orientation has more importance than position - * = 0.0: perform position-only IK", - validation: { - gt_eq<>: [ 0.0 ] - } - } - - position_only_ik: { - type: bool, - default_value: false, - description: "position_only_ik overrules orientation_vs_position. If true, sets orientation_vs_position weight to 0.0", - } diff --git a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp deleted file mode 100644 index 3fedba53d1..0000000000 --- a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp +++ /dev/null @@ -1,364 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, CRI group, NTU, Singapore - * 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 CRI group 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: Francisco Suarez-Ruiz */ - -#include -#include -#include - -#include -#include -#include -#include - -// register as a KinematicsBase implementation -#include -CLASS_LOADER_REGISTER_CLASS(lma_kinematics_plugin::LMAKinematicsPlugin, kinematics::KinematicsBase) - -namespace lma_kinematics_plugin -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_lma_kinematics_plugin.lma_kinematics_plugin"); - -LMAKinematicsPlugin::LMAKinematicsPlugin() : initialized_(false) -{ -} - -void LMAKinematicsPlugin::getRandomConfiguration(Eigen::VectorXd& jnt_array) const -{ - state_->setToRandomPositions(joint_model_group_); - state_->copyJointGroupPositions(joint_model_group_, &jnt_array[0]); -} - -void LMAKinematicsPlugin::getRandomConfiguration(const Eigen::VectorXd& seed_state, - const std::vector& consistency_limits, - Eigen::VectorXd& jnt_array) const -{ - joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), &jnt_array[0], - &seed_state[0], consistency_limits); -} - -bool LMAKinematicsPlugin::checkConsistency(const Eigen::VectorXd& seed_state, - const std::vector& consistency_limits, - const Eigen::VectorXd& solution) const -{ - for (std::size_t i = 0; i < dimension_; ++i) - { - if (fabs(seed_state(i) - solution(i)) > consistency_limits[i]) - return false; - } - return true; -} - -bool LMAKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, - const std::string& group_name, const std::string& base_frame, - const std::vector& tip_frames, double search_discretization) -{ - node_ = node; - - // Get Solver Parameters - std::string kinematics_param_prefix = "robot_description_kinematics." + group_name; - param_listener_ = std::make_shared(node, kinematics_param_prefix); - params_ = param_listener_->get_params(); - - storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); - joint_model_group_ = robot_model_->getJointModelGroup(group_name); - if (!joint_model_group_) - return false; - - if (!joint_model_group_->isChain()) - { - RCLCPP_ERROR(LOGGER, "Group '%s' is not a chain", group_name.c_str()); - return false; - } - if (!joint_model_group_->isSingleDOFJoints()) - { - RCLCPP_ERROR(LOGGER, "Group '%s' includes joints that have more than 1 DOF", group_name.c_str()); - return false; - } - - KDL::Tree kdl_tree; - - if (!kdl_parser::treeFromUrdfModel(*robot_model.getURDF(), kdl_tree)) - { - RCLCPP_ERROR(LOGGER, "Could not initialize tree object"); - return false; - } - if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_)) - { - RCLCPP_ERROR(LOGGER, "Could not initialize chain object"); - return false; - } - - for (const moveit::core::JointModel* jm : joint_model_group_->getJointModels()) - { - if (jm->getType() == moveit::core::JointModel::REVOLUTE || jm->getType() == moveit::core::JointModel::PRISMATIC) - { - joints_.push_back(jm); - joint_names_.push_back(jm->getName()); - } - } - dimension_ = joints_.size(); - - // Setup the joint state groups that we need - state_ = std::make_shared(robot_model_); - - fk_solver_ = std::make_unique(kdl_chain_); - - initialized_ = true; - RCLCPP_DEBUG(LOGGER, "LMA solver initialized"); - return true; -} - -bool LMAKinematicsPlugin::timedOut(const rclcpp::Time& start_time, double duration) const -{ - return ((node_->now() - start_time).seconds() >= duration); -} - -bool LMAKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, - const std::vector& ik_seed_state, std::vector& solution, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const -{ - std::vector consistency_limits; - - // limit search to a single attempt by setting a timeout of zero - return searchPositionIK(ik_pose, ik_seed_state, 0.0, consistency_limits, solution, IKCallbackFn(), error_code, - options); -} - -bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, - const std::vector& ik_seed_state, double timeout, - std::vector& solution, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const -{ - std::vector consistency_limits; - - return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, - options); -} - -bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, - const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const -{ - return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, - options); -} - -bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, - const std::vector& ik_seed_state, double timeout, - std::vector& solution, const IKCallbackFn& solution_callback, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const -{ - std::vector consistency_limits; - return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, - options); -} - -void LMAKinematicsPlugin::harmonize(Eigen::VectorXd& values) const -{ - size_t i = 0; - for (auto* jm : joints_) - jm->harmonizePosition(&values[i++]); -} - -bool LMAKinematicsPlugin::obeysLimits(const Eigen::VectorXd& values) const -{ - size_t i = 0; - for (const auto& jm : joints_) - { - if (!jm->satisfiesPositionBounds(&values[i++])) - return false; - } - return true; -} - -bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, - const std::vector& ik_seed_state, double timeout, - const std::vector& consistency_limits, std::vector& solution, - const IKCallbackFn& solution_callback, - moveit_msgs::msg::MoveItErrorCodes& error_code, - const kinematics::KinematicsQueryOptions& options) const -{ - rclcpp::Time start_time = node_->now(); - if (!initialized_) - { - RCLCPP_ERROR(LOGGER, "kinematics solver not initialized"); - error_code.val = error_code.NO_IK_SOLUTION; - return false; - } - - if (ik_seed_state.size() != dimension_) - { - RCLCPP_ERROR(LOGGER, "Seed state must have size %d instead of size %zu", dimension_, ik_seed_state.size()); - error_code.val = error_code.NO_IK_SOLUTION; - return false; - } - - if (!consistency_limits.empty() && consistency_limits.size() != dimension_) - { - RCLCPP_ERROR_STREAM(LOGGER, "Consistency limits be empty or must have size " << dimension_ << " instead of size " - << consistency_limits.size()); - error_code.val = error_code.NO_IK_SOLUTION; - return false; - } - - const auto orientation_vs_position_weight = params_.position_only_ik ? 0.0 : params_.orientation_vs_position; - if (orientation_vs_position_weight == 0.0) - RCLCPP_INFO(LOGGER, "Using position only ik"); - - Eigen::Matrix cartesian_weights; - cartesian_weights(0) = 1; - cartesian_weights(1) = 1; - cartesian_weights(2) = 1; - cartesian_weights(3) = orientation_vs_position_weight; - cartesian_weights(4) = orientation_vs_position_weight; - cartesian_weights(5) = orientation_vs_position_weight; - - KDL::JntArray jnt_seed_state(dimension_); - KDL::JntArray jnt_pos_in(dimension_); - KDL::JntArray jnt_pos_out(dimension_); - jnt_seed_state.data = Eigen::Map(ik_seed_state.data(), ik_seed_state.size()); - jnt_pos_in = jnt_seed_state; - - KDL::ChainIkSolverPos_LMA ik_solver_pos(kdl_chain_, cartesian_weights, params_.epsilon, params_.max_solver_iterations); - solution.resize(dimension_); - - KDL::Frame pose_desired; - tf2::fromMsg(ik_pose, pose_desired); - - RCLCPP_DEBUG_STREAM(LOGGER, "searchPositionIK2: Position request pose is " - << ik_pose.position.x << ' ' << ik_pose.position.y << ' ' << ik_pose.position.z << ' ' - << ik_pose.orientation.x << ' ' << ik_pose.orientation.y << ' ' - << ik_pose.orientation.z << ' ' << ik_pose.orientation.w); - unsigned int attempt = 0; - do - { - ++attempt; - if (attempt > 1) // randomly re-seed after first attempt - { - if (!consistency_limits.empty()) - { - getRandomConfiguration(jnt_seed_state.data, consistency_limits, jnt_pos_in.data); - } - else - { - getRandomConfiguration(jnt_pos_in.data); - } - RCLCPP_DEBUG_STREAM(LOGGER, "New random configuration (" << attempt << "): " << jnt_pos_in); - } - - int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out); - if (ik_valid == 0 || options.return_approximate_solution) // found acceptable solution - { - harmonize(jnt_pos_out.data); - if (!consistency_limits.empty() && !checkConsistency(jnt_seed_state.data, consistency_limits, jnt_pos_out.data)) - continue; - if (!obeysLimits(jnt_pos_out.data)) - continue; - - Eigen::Map(solution.data(), solution.size()) = jnt_pos_out.data; - if (solution_callback) - { - solution_callback(ik_pose, solution, error_code); - if (error_code.val != error_code.SUCCESS) - continue; - } - - // solution passed consistency check and solution callback - error_code.val = error_code.SUCCESS; - RCLCPP_DEBUG_STREAM(LOGGER, "Solved after " << (node_->now() - start_time).seconds() << " < " << timeout - << "s and " << attempt << " attempts"); - return true; - } - } while (!timedOut(start_time, timeout)); - - RCLCPP_DEBUG_STREAM(LOGGER, "IK timed out after " << (node_->now() - start_time).seconds() << " > " << timeout - << "s and " << attempt << " attempts"); - error_code.val = error_code.TIMED_OUT; - return false; -} - -bool LMAKinematicsPlugin::getPositionFK(const std::vector& link_names, - const std::vector& joint_angles, - std::vector& poses) const -{ - if (!initialized_) - { - RCLCPP_ERROR(LOGGER, "kinematics solver not initialized"); - return false; - } - poses.resize(link_names.size()); - if (joint_angles.size() != dimension_) - { - RCLCPP_ERROR(LOGGER, "Joint angles vector must have size: %d", dimension_); - return false; - } - - KDL::Frame p_out; - KDL::JntArray jnt_pos_in(dimension_); - jnt_pos_in.data = Eigen::Map(joint_angles.data(), joint_angles.size()); - - bool valid = true; - for (unsigned int i = 0; i < poses.size(); ++i) - { - if (fk_solver_->JntToCart(jnt_pos_in, p_out) >= 0) - { - poses[i] = tf2::toMsg(p_out); - } - else - { - RCLCPP_ERROR(LOGGER, "Could not compute FK for %s", link_names[i].c_str()); - valid = false; - } - } - return valid; -} - -const std::vector& LMAKinematicsPlugin::getJointNames() const -{ - return joint_names_; -} - -const std::vector& LMAKinematicsPlugin::getLinkNames() const -{ - return getTipFrames(); -} - -} // namespace lma_kinematics_plugin diff --git a/moveit_kinematics/lma_kinematics_plugin_description.xml b/moveit_kinematics/lma_kinematics_plugin_description.xml deleted file mode 100644 index 2cba3e5a93..0000000000 --- a/moveit_kinematics/lma_kinematics_plugin_description.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - A implementation of kinematics as a plugin based on KDL ChainIkSolverPos_LMA. - - - diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index fb7b102296..70a134833e 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -55,7 +55,6 @@ ament_cmake - diff --git a/moveit_kinematics/test/CMakeLists.txt b/moveit_kinematics/test/CMakeLists.txt index ef37d24647..2057c0e654 100644 --- a/moveit_kinematics/test/CMakeLists.txt +++ b/moveit_kinematics/test/CMakeLists.txt @@ -22,13 +22,6 @@ if(BUILD_TESTING) add_ros_test(launch/panda-kdl-singular.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") add_ros_test(launch/panda-kdl.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - # LMA testing - set(ARGS ARGS ik_plugin:=lma_kinematics_plugin/LMAKinematicsPlugin) - add_ros_test(launch/fanuc-lma-singular.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - add_ros_test(launch/fanuc-lma.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - add_ros_test(launch/panda-lma-singular.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - add_ros_test(launch/panda-lma.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - # Run ikfast tests only if the corresponding packages were built # TODO (vatanaksoytezer): Enable ikfast tests # find_package(fanuc_ikfast_plugin QUIET) diff --git a/moveit_kinematics/test/config/fanuc-lma-singular-test.yaml b/moveit_kinematics/test/config/fanuc-lma-singular-test.yaml deleted file mode 100644 index 7c465b077d..0000000000 --- a/moveit_kinematics/test/config/fanuc-lma-singular-test.yaml +++ /dev/null @@ -1,201 +0,0 @@ -tip_link: "tool0" -root_link: "base_link" -group: "manipulator" -ik_timeout: 0.2 -tolerance: 0.1 -joint_names: - - "joint_1" - - "joint_2" - - "joint_3" - - "joint_4" - - "joint_5" - - "joint_6" - -# LMA params -ik_plugin_name: "lma_kinematics_plugin/LMAKinematicsPlugin" -num_ik_cb_tests: 0 -num_ik_multiple_tests: 0 -num_nearest_ik_tests: 0 -publish_trajectory: False - -# Test inputs -num_fk_tests: 0 -num_ik_tests: 0 -seed: - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - -# Test poses -unit_test_poses: - pose_0: - joints: - - -0.24967849540594997 - - -0.0064208285266353273 - - -0.24251650108977454 - - -2.3129180342434519 - - -0.34257870605790114 - - -0.79882125927421033 - pose: - - 0.889898 - - -0.201372 - - 1.09478 - - 0.7073017393841021 - - -0.00017737593464318564 - - 0.7069117395278034 - - 0.00010256596220803817 - type: absolute - pose_1: - joints: - - 0.0 - - 0.17225521580111153 - - 0.18616019238929934 - - 0.0 - - -0.013904976588187847 - - 0.0 - pose: - - 0.1 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - type: relative - pose_10: - joints: - - 0.0 - - 0.006181632386654151 - - 0.021837005762580415 - - 0.0 - - -0.1156553733759268 - - 0.0 - pose: - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - -0.1 - - 0.0 - type: relative - pose_11: - pose: - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.1 - type: relative - pose_12: - pose: - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - -0.1 - type: relative - pose_2: - joints: - - 0.0 - - -0.1631486200930685 - - -0.15067469903041658 - - 0.0 - - -0.012473921062651848 - - 0.0 - pose: - - -0.1 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - type: relative - pose_3: - pose: - - 0.0 - - 0.1 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - type: relative - pose_4: - pose: - - 0.0 - - -0.1 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - type: relative - pose_5: - joints: - - 0.0 - - 0.0684508072897886 - - 0.2317804402761828 - - 0.0 - - -0.16332963298639425 - - 0.0 - pose: - - 0.0 - - 0.0 - - 0.1 - - 0.0 - - 0.0 - - 0.0 - type: relative - pose_6: - joints: - - 0.0 - - -0.03827095462397467 - - -0.19079719308481202 - - 0.0 - - 0.15252623846083735 - - 0.0 - pose: - - 0.0 - - 0.0 - - -0.1 - - 0.0 - - 0.0 - - 0.0 - type: relative - pose_7: - pose: - - 0.0 - - 0.0 - - 0.0 - - 0.1 - - 0.0 - - 0.0 - type: relative - pose_8: - pose: - - 0.0 - - 0.0 - - 0.0 - - -0.1 - - 0.0 - - 0.0 - type: relative - pose_9: - joints: - - 0 - - -0.004222773386364292 - - -0.019776669463347087 - - 0 - - 0.11555389607698333 - - 0 - pose: - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.1 - - 0.0 - type: relative - size: 13 diff --git a/moveit_kinematics/test/config/fanuc-lma-test.yaml b/moveit_kinematics/test/config/fanuc-lma-test.yaml deleted file mode 100644 index dcd57c0358..0000000000 --- a/moveit_kinematics/test/config/fanuc-lma-test.yaml +++ /dev/null @@ -1,137 +0,0 @@ -tip_link: "tool0" -root_link: "base_link" -group: "manipulator" -ik_timeout: 0.2 -tolerance: 0.1 -joint_names: - - "joint_1" - - "joint_2" - - "joint_3" - - "joint_4" - - "joint_5" - - "joint_6" - -# LMA params -ik_plugin_name: "lma_kinematics_plugin/LMAKinematicsPlugin" -num_ik_cb_tests: 0 -num_ik_multiple_tests: 0 -num_nearest_ik_tests: 0 -publish_trajectory: False - -# Test inputs -num_fk_tests: 100 -num_ik_tests: 100 -consistency_limits: -- 0.4 -- 0.4 -- 0.4 -- 0.4 -- 0.4 -- 0.4 -seed: -- 0.0 -- -0.32 -- -0.5 -- 0.0 -- -0.5 -- 0.0 - -# Test poses -unit_test_poses: - pose_0: - joints: - - 0.0 - - -0.152627 - - -0.367847 - - 0.0 - - -0.46478 - - 0.0 - pose: - - 0.1 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - type: relative - pose_1: - joints: - - 0.1582256 - - -0.3066389 - - -0.490349 - - 0.250946 - - -0.5159858 - - -0.319381 - pose: - - 0.0 - - 0.1 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - type: relative - pose_2: - joints: - - 0.0 - - -0.287588 - - -0.324304 - - 0.0 - - -0.643285 - - 0.0 - pose: - - 0.0 - - 0.0 - - 0.1 - - 0.0 - - 0.0 - - 0.0 - type: relative - pose_3: - joints: - - -0.0159181 - - -0.319276 - - -0.499953 - - -0.231014 - - -0.511806 - - 0.212341 - pose: - - 0.0 - - 0.0 - - 0.0 - - 0.1 - - 0.0 - - 0.0 - type: relative - pose_4: - joints: - - 0.0 - - -0.331586 - - -0.520375 - - 0.0 - - -0.391211 - - 0.0 - pose: - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.1 - - 0.0 - type: relative - pose_5: - joints: - - 0.0 - - -0.32 - - -0.5 - - 0.0 - - -0.5 - - -0.1 - pose: - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.1 - type: relative - size: 6 diff --git a/moveit_kinematics/test/config/panda-lma-singular-test.yaml b/moveit_kinematics/test/config/panda-lma-singular-test.yaml deleted file mode 100644 index 61423ebdef..0000000000 --- a/moveit_kinematics/test/config/panda-lma-singular-test.yaml +++ /dev/null @@ -1,45 +0,0 @@ -tip_link: "panda_link8" -root_link: "panda_link0" -group: "panda_arm" -ik_timeout: 0.2 -tolerance: 0.1 -joint_names: - - "panda_joint1" - - "panda_joint2" - - "panda_joint3" - - "panda_joint4" - - "panda_joint5" - - "panda_joint6" - - "panda_joint7" - -# LMA params -ik_plugin_name: "lma_kinematics_plugin/LMAKinematicsPlugin" -num_ik_cb_tests: 0 -num_ik_multiple_tests: 0 -num_nearest_ik_tests: 0 -publish_trajectory: False - -# Test inputs -num_fk_tests: 0 -num_ik_tests: 0 -seed: - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - -# Test poses -unit_tests_poses: - size: 1 - pose_0: - pose: - - 0.0 - - 0.0 - - 0.1 - - 0.0 - - 0.0 - - 0.0 - type: "relative" diff --git a/moveit_kinematics/test/config/panda-lma-test.yaml b/moveit_kinematics/test/config/panda-lma-test.yaml deleted file mode 100644 index 8dcf4ee800..0000000000 --- a/moveit_kinematics/test/config/panda-lma-test.yaml +++ /dev/null @@ -1,40 +0,0 @@ -tip_link: "panda_link8" -root_link: "panda_link0" -group: "panda_arm" -ik_timeout: 0.2 -tolerance: 0.1 -joint_names: - - "panda_joint1" - - "panda_joint2" - - "panda_joint3" - - "panda_joint4" - - "panda_joint5" - - "panda_joint6" - - "panda_joint7" - -# LMA params -ik_plugin_name: "lma_kinematics_plugin/LMAKinematicsPlugin" -num_ik_cb_tests: 0 -num_ik_multiple_tests: 0 -num_nearest_ik_tests: 0 -publish_trajectory: False - -# Test inputs -num_fk_tests: 100 -num_ik_tests: 100 -seed: - - -0.5 - - -0.5 - - 0.3 - - -2 - - 0.8 - - 1.8 - - 1.9 -consistency_limits: - - 0.4 - - 0.4 - - 0.4 - - 0.4 - - 0.4 - - 0.4 - - 0.4 diff --git a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py b/moveit_kinematics/test/launch/fanuc-lma-singular.test.py deleted file mode 100644 index 661ac1acce..0000000000 --- a/moveit_kinematics/test/launch/fanuc-lma-singular.test.py +++ /dev/null @@ -1,51 +0,0 @@ -import launch_testing -import pytest -import unittest -from launch import LaunchDescription -from launch_ros.actions import Node -from launch_testing.util import KeepAliveProc -from moveit_configs_utils import MoveItConfigsBuilder -from launch_param_builder import ParameterBuilder - - -@pytest.mark.rostest -def generate_test_description(): - moveit_configs = MoveItConfigsBuilder("moveit_resources_fanuc").to_dict() - test_param = ( - ParameterBuilder("moveit_kinematics") - .yaml("config/fanuc-lma-singular-test.yaml") - .to_dict() - ) - - fanuc_lma_singular = Node( - package="moveit_kinematics", - executable="test_kinematics_plugin", - name="fanuc_lma_singular", - parameters=[ - moveit_configs, - test_param, - ], - output="screen", - ) - - return ( - LaunchDescription( - [ - fanuc_lma_singular, - KeepAliveProc(), - launch_testing.actions.ReadyToTest(), - ] - ), - {"fanuc_lma_singular": fanuc_lma_singular}, - ) - - -class TestTerminatingProcessStops(unittest.TestCase): - def test_gtest_run_complete(self, proc_info, fanuc_lma_singular): - proc_info.assertWaitForShutdown(process=fanuc_lma_singular, timeout=4000.0) - - -@launch_testing.post_shutdown_test() -class TestOutcome(unittest.TestCase): - def test_exit_codes(self, proc_info): - launch_testing.asserts.assertExitCodes(proc_info) diff --git a/moveit_kinematics/test/launch/fanuc-lma.test.py b/moveit_kinematics/test/launch/fanuc-lma.test.py deleted file mode 100644 index ff95fdbe8f..0000000000 --- a/moveit_kinematics/test/launch/fanuc-lma.test.py +++ /dev/null @@ -1,51 +0,0 @@ -import launch_testing -import pytest -import unittest -from launch import LaunchDescription -from launch_ros.actions import Node -from launch_testing.util import KeepAliveProc -from moveit_configs_utils import MoveItConfigsBuilder -from launch_param_builder import ParameterBuilder - - -@pytest.mark.rostest -def generate_test_description(): - moveit_configs = MoveItConfigsBuilder("moveit_resources_fanuc").to_dict() - test_param = ( - ParameterBuilder("moveit_kinematics") - .yaml("config/fanuc-lma-test.yaml") - .to_dict() - ) - - fanuc_lma = Node( - package="moveit_kinematics", - executable="test_kinematics_plugin", - name="fanuc_lma", - parameters=[ - moveit_configs, - test_param, - ], - output="screen", - ) - - return ( - LaunchDescription( - [ - fanuc_lma, - KeepAliveProc(), - launch_testing.actions.ReadyToTest(), - ] - ), - {"fanuc_lma": fanuc_lma}, - ) - - -class TestTerminatingProcessStops(unittest.TestCase): - def test_gtest_run_complete(self, proc_info, fanuc_lma): - proc_info.assertWaitForShutdown(process=fanuc_lma, timeout=4000.0) - - -@launch_testing.post_shutdown_test() -class TestOutcome(unittest.TestCase): - def test_exit_codes(self, proc_info): - launch_testing.asserts.assertExitCodes(proc_info) diff --git a/moveit_kinematics/test/launch/panda-lma-singular.test.py b/moveit_kinematics/test/launch/panda-lma-singular.test.py deleted file mode 100644 index 1d69248260..0000000000 --- a/moveit_kinematics/test/launch/panda-lma-singular.test.py +++ /dev/null @@ -1,51 +0,0 @@ -import launch_testing -import pytest -import unittest -from launch import LaunchDescription -from launch_ros.actions import Node -from launch_testing.util import KeepAliveProc -from moveit_configs_utils import MoveItConfigsBuilder -from launch_param_builder import ParameterBuilder - - -@pytest.mark.rostest -def generate_test_description(): - moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() - test_param = ( - ParameterBuilder("moveit_kinematics") - .yaml("config/panda-lma-singular-test.yaml") - .to_dict() - ) - - panda_lma_singular = Node( - package="moveit_kinematics", - executable="test_kinematics_plugin", - name="panda_lma_singular", - parameters=[ - moveit_configs, - test_param, - ], - output="screen", - ) - - return ( - LaunchDescription( - [ - panda_lma_singular, - KeepAliveProc(), - launch_testing.actions.ReadyToTest(), - ] - ), - {"panda_lma_singular": panda_lma_singular}, - ) - - -class TestTerminatingProcessStops(unittest.TestCase): - def test_gtest_run_complete(self, proc_info, panda_lma_singular): - proc_info.assertWaitForShutdown(process=panda_lma_singular, timeout=4000.0) - - -@launch_testing.post_shutdown_test() -class TestOutcome(unittest.TestCase): - def test_exit_codes(self, proc_info): - launch_testing.asserts.assertExitCodes(proc_info) diff --git a/moveit_kinematics/test/launch/panda-lma.test.py b/moveit_kinematics/test/launch/panda-lma.test.py deleted file mode 100644 index ded012562d..0000000000 --- a/moveit_kinematics/test/launch/panda-lma.test.py +++ /dev/null @@ -1,51 +0,0 @@ -import launch_testing -import pytest -import unittest -from launch import LaunchDescription -from launch_ros.actions import Node -from launch_testing.util import KeepAliveProc -from moveit_configs_utils import MoveItConfigsBuilder -from launch_param_builder import ParameterBuilder - - -@pytest.mark.rostest -def generate_test_description(): - moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_dict() - test_param = ( - ParameterBuilder("moveit_kinematics") - .yaml("config/panda-lma-test.yaml") - .to_dict() - ) - - panda_lma = Node( - package="moveit_kinematics", - executable="test_kinematics_plugin", - name="panda_lma", - parameters=[ - moveit_configs, - test_param, - ], - output="screen", - ) - - return ( - LaunchDescription( - [ - panda_lma, - KeepAliveProc(), - launch_testing.actions.ReadyToTest(), - ] - ), - {"panda_lma": panda_lma}, - ) - - -class TestTerminatingProcessStops(unittest.TestCase): - def test_gtest_run_complete(self, proc_info, panda_lma): - proc_info.assertWaitForShutdown(process=panda_lma, timeout=4000.0) - - -@launch_testing.post_shutdown_test() -class TestOutcome(unittest.TestCase): - def test_exit_codes(self, proc_info): - launch_testing.asserts.assertExitCodes(proc_info) From 8d2eb900d222f6609ee2588f6f5ebb97986635bd Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Wed, 13 Dec 2023 09:55:16 -0700 Subject: [PATCH 13/17] Node logging for the rest of MoveIt (#2599) --- ...t_bullet_continuous_collision_checking.cpp | 8 +- .../test/test_fcl_env.cpp | 8 +- .../src/collision_distance_field_types.cpp | 28 +-- .../constraint_samplers/test/pr2_arm_ik.cpp | 21 +- .../test/pr2_arm_kinematics_plugin.cpp | 53 +++--- .../include/moveit/robot_model/robot_model.h | 4 +- .../utils/src/robot_model_test_utils.cpp | 36 ++-- .../cached_ik_kinematics_plugin.h | 6 +- .../src/ik_cache.cpp | 26 ++- .../ikfast61_moveit_plugin_template.cpp | 177 +++++++++-------- .../src/kdl_kinematics_plugin.cpp | 76 ++++---- .../src/srv_kinematics_plugin.cpp | 77 ++++---- moveit_kinematics/test/benchmark_ik.cpp | 11 +- .../test/test_kinematics_plugin.cpp | 25 ++- .../chomp_interface/src/chomp_interface.cpp | 11 +- .../chomp_interface/src/chomp_plugin.cpp | 13 +- .../src/chomp_optimizer.cpp | 179 +++--------------- .../src/chomp_planner.cpp | 51 ++--- .../src/detail/constrained_goal_sampler.cpp | 41 ++-- .../src/detail/constraints_library.cpp | 148 ++++++--------- .../src/detail/ompl_constraints.cpp | 23 ++- .../src/detail/state_validity_checker.cpp | 17 +- .../src/model_based_planning_context.cpp | 177 ++++++++--------- .../ompl_interface/src/ompl_interface.cpp | 27 ++- .../src/ompl_planner_manager.cpp | 16 +- .../model_based_state_space.cpp | 83 ++++---- .../work_space/pose_model_state_space.cpp | 67 ++++--- .../src/planning_context_manager.cpp | 53 +++--- .../test_constrained_planning_state_space.cpp | 13 +- ...est_constrained_state_validity_checker.cpp | 17 +- .../test/test_ompl_constraints.cpp | 18 +- .../test/test_planning_context_manager.cpp | 4 +- .../ompl_interface/test/test_state_space.cpp | 12 +- .../test/test_state_validity_checker.cpp | 14 +- .../test/test_threadsafe_state_storage.cpp | 2 - .../src/command_list_manager.cpp | 22 ++- .../src/joint_limits_aggregator.cpp | 20 +- .../src/joint_limits_container.cpp | 10 +- .../src/limits_container.cpp | 8 +- .../src/move_group_sequence_action.cpp | 42 ++-- .../src/move_group_sequence_service.cpp | 16 +- .../src/pilz_industrial_motion_planner.cpp | 35 ++-- .../src/planning_context_loader_circ.cpp | 11 +- .../src/planning_context_loader_lin.cpp | 11 +- .../src/planning_context_loader_ptp.cpp | 10 +- .../trajectory_blender_transition_window.cpp | 39 ++-- .../src/trajectory_functions.cpp | 120 ++++++------ .../src/trajectory_generator.cpp | 16 +- .../src/trajectory_generator_circ.cpp | 14 +- .../src/trajectory_generator_lin.cpp | 14 +- .../src/trajectory_generator_ptp.cpp | 10 +- .../stomp/src/stomp_moveit_planner_plugin.cpp | 13 +- .../src/stomp_moveit_planning_context.cpp | 18 +- .../src/controller_manager_plugin.cpp | 37 ++-- .../src/moveit_simple_controller_manager.cpp | 41 ++-- .../moveit_ros/moveit_cpp/moveit_cpp.cpp | 16 +- .../planning_scene_monitor.cpp | 1 - .../trajectory_execution_manager.cpp | 2 - .../src/replan_invalidated_trajectory.cpp | 5 - .../src/single_plan_execution.cpp | 5 - .../test/hybrid_planning_demo_node.cpp | 32 ++-- .../src/collision_plugin_loader.cpp | 2 - .../src/display_random_state.cpp | 2 - .../src/planning_pipeline.cpp | 12 +- .../test/test_execution_manager.cpp | 2 - .../src/motion_planning_param_widget.cpp | 1 - .../src/robot_state_visualization.cpp | 12 +- .../test/test_perception.cpp | 2 - .../src/collisions_updater.cpp | 11 +- .../test/test_controllers.cpp | 2 - .../moveit_setup_framework/src/templates.cpp | 15 +- .../src/compute_default_collisions.cpp | 59 ++---- .../test/test_srdf.cpp | 2 - 73 files changed, 1159 insertions(+), 1073 deletions(-) diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp index 2c446892d7..94853d4d57 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -47,13 +47,17 @@ #include #include +#include #include #include namespace cb = collision_detection_bullet; -static const rclcpp::Logger TEST_LOGGER = rclcpp::get_logger("collision_detection.bullet_test"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("collision_detection.bullet_test"); +} /** \brief Brings the panda robot in user defined home position */ inline void setToHome(moveit::core::RobotState& panda_state) @@ -254,7 +258,7 @@ TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf) ASSERT_FALSE(res.collision); res.clear(); - RCLCPP_INFO(TEST_LOGGER, "Continuous to continuous collisions are not supported yet, therefore fail here."); + RCLCPP_INFO(getLogger(), "Continuous to continuous collisions are not supported yet, therefore fail here."); ASSERT_TRUE(res.collision); res.clear(); } diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp index 338bbd0a31..ff6458b044 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -63,7 +64,10 @@ inline void setToHome(moveit::core::RobotState& panda_state) panda_state.update(); } -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection_fcl.test.test_fcl_env"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit_collision_detection_fcl.test.test_fcl_env"); +} class CollisionDetectionEnvTest : public testing::Test { @@ -262,7 +266,7 @@ TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionSelf) res.clear(); // c_env_->checkSelfCollision(req, res, state1, state2, *acm_); - RCLCPP_INFO(LOGGER, "Continuous to continuous collisions are not supported yet, therefore fail here."); + RCLCPP_INFO(getLogger(), "Continuous to continuous collisions are not supported yet, therefore fail here."); ASSERT_TRUE(res.collision); res.clear(); } diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp index 4cdc53fa7a..40bde7f13c 100644 --- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp +++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp @@ -42,13 +42,19 @@ #include #include #include +#include static const double EPSILON{ 0.0001 }; namespace collision_detection { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit_collision_distance_field.collision_distance_field_types"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("collision_distance_field_types"); +} +} // namespace std::vector determineCollisionSpheres(const bodies::Body* body, Eigen::Isometry3d& relative_transform) { @@ -158,7 +164,7 @@ bool getCollisionSphereGradients(const distance_field::DistanceField* distance_f double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds); if (!in_bounds && grad.norm() > EPSILON) { - RCLCPP_DEBUG(LOGGER, "Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z()); + RCLCPP_DEBUG(getLogger(), "Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z()); return true; } @@ -216,7 +222,7 @@ bool getCollisionSphereCollision(const distance_field::DistanceField* distance_f if (!in_bounds && grad.norm() > 0) { - RCLCPP_DEBUG(LOGGER, "Collision sphere point is out of bounds"); + RCLCPP_DEBUG(getLogger(), "Collision sphere point is out of bounds"); return true; } @@ -243,7 +249,7 @@ bool getCollisionSphereCollision(const distance_field::DistanceField* distance_f double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds); if (!in_bounds && (grad.norm() > 0)) { - RCLCPP_DEBUG(LOGGER, "Collision sphere point is out of bounds"); + RCLCPP_DEBUG(getLogger(), "Collision sphere point is out of bounds"); return true; } if (maximum_value > dist && (sphere_list[i].radius_ - dist > tolerance)) @@ -324,8 +330,8 @@ void BodyDecomposition::init(const std::vector& shapes, c } bodies::mergeBoundingSpheres(bounding_spheres, relative_bounding_sphere_); - RCLCPP_DEBUG(LOGGER, "BodyDecomposition generated %zu collision spheres out of %zu shapes", collision_spheres_.size(), - shapes.size()); + RCLCPP_DEBUG(getLogger(), "BodyDecomposition generated %zu collision spheres out of %zu shapes", + collision_spheres_.size(), shapes.size()); } BodyDecomposition::~BodyDecomposition() @@ -450,7 +456,7 @@ void getProximityGradientMarkers(const std::string& frame_id, const std::string& rclcpp::Clock ros_clock; if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size()) { - RCLCPP_WARN(LOGGER, "Size mismatch between gradients %u and decompositions %u", + RCLCPP_WARN(getLogger(), "Size mismatch between gradients %u and decompositions %u", static_cast(gradients.size()), static_cast((posed_decompositions.size() + posed_vector_decompositions.size()))); return; @@ -484,12 +490,12 @@ void getProximityGradientMarkers(const std::string& frame_id, const std::string& } else { - RCLCPP_DEBUG(LOGGER, "Negative length for %u %d %lf", i, arrow_mark.id, gradients[i].gradients[j].norm()); + RCLCPP_DEBUG(getLogger(), "Negative length for %u %d %lf", i, arrow_mark.id, gradients[i].gradients[j].norm()); } } else { - RCLCPP_DEBUG(LOGGER, "Negative dist %lf for %u %d", gradients[i].distances[j], i, arrow_mark.id); + RCLCPP_DEBUG(getLogger(), "Negative dist %lf for %u %d", gradients[i].distances[j], i, arrow_mark.id); } arrow_mark.points.resize(2); if (i < posed_decompositions.size()) @@ -551,7 +557,7 @@ void getCollisionMarkers(const std::string& frame_id, const std::string& ns, con rclcpp::Clock ros_clock; if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size()) { - RCLCPP_WARN(LOGGER, "Size mismatch between gradients %zu and decompositions %zu", gradients.size(), + RCLCPP_WARN(getLogger(), "Size mismatch between gradients %zu and decompositions %zu", gradients.size(), posed_decompositions.size() + posed_vector_decompositions.size()); return; } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index 1df9d8c7af..88a387ab70 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -35,6 +35,7 @@ /* Author: Sachin Chitta, E. Gil Jones */ #include +#include #include "pr2_arm_ik.h" /**** List of angles (for reference) ******* @@ -49,7 +50,13 @@ using namespace angles; namespace pr2_arm_kinematics { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constaint_samplers.test.pr2_arm_ik"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit_constaint_samplers.test.pr2_arm_ik"); +} +} // namespace PR2ArmIK::PR2ArmIK() { @@ -69,11 +76,11 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& { if (link->parent_joint) { - RCLCPP_ERROR(LOGGER, "Could not find joint: %s", link->parent_joint->name.c_str()); + RCLCPP_ERROR(getLogger(), "Could not find joint: %s", link->parent_joint->name.c_str()); } else { - RCLCPP_ERROR(LOGGER, "Link %s has no parent joint", link->name.c_str()); + RCLCPP_ERROR(getLogger(), "Link %s has no parent joint", link->name.c_str()); } return false; } @@ -82,7 +89,8 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& link_offset.push_back(link->parent_joint->parent_to_joint_origin_transform); angle_multipliers_.push_back(joint->axis.x * fabs(joint->axis.x) + joint->axis.y * fabs(joint->axis.y) + joint->axis.z * fabs(joint->axis.z)); - RCLCPP_DEBUG(LOGGER, "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, joint->axis.y, joint->axis.z); + RCLCPP_DEBUG(getLogger(), "Joint axis: %d, %f, %f, %f", 6 - num_joints, joint->axis.x, joint->axis.y, + joint->axis.z); if (joint->type != urdf::Joint::CONTINUOUS) { if (joint->safety) @@ -101,7 +109,7 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& { min_angles_.push_back(0.0); max_angles_.push_back(0.0); - RCLCPP_WARN(LOGGER, "No joint limits or joint '%s'", joint->name.c_str()); + RCLCPP_WARN(getLogger(), "No joint limits or joint '%s'", joint->name.c_str()); } } continuous_joint_.push_back(false); @@ -133,7 +141,8 @@ bool PR2ArmIK::init(const urdf::ModelInterface& robot_model, const std::string& if (num_joints != 7) { - RCLCPP_ERROR(LOGGER, "PR2ArmIK:: Chain from %s to %s does not have 7 joints", root_name.c_str(), tip_name.c_str()); + RCLCPP_ERROR(getLogger(), "PR2ArmIK:: Chain from %s to %s does not have 7 joints", root_name.c_str(), + tip_name.c_str()); return false; } diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 4552a8ad8e..a1e1978a35 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -41,6 +41,7 @@ #include #include +#include #include "pr2_arm_kinematics_plugin.h" using namespace KDL; @@ -48,7 +49,13 @@ using namespace std; namespace pr2_arm_kinematics { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constraint_samplers.test.pr2_arm_kinematics_plugin"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit_constraint_samplers.test.pr2_arm_kinematics_plugin"); +} +} // namespace bool PR2ArmIKSolver::getCount(int& count, int max_count, int min_count) { @@ -110,7 +117,7 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i if (free_angle_ == 0) { if (verbose) - RCLCPP_WARN(LOGGER, "Solving with %f", q_init(0)); + RCLCPP_WARN(getLogger(), "Solving with %f", q_init(0)); pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik); } else @@ -128,14 +135,12 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i { if (verbose) { - RCLCPP_WARN(LOGGER, "Solution : %d", static_cast(solution_ik.size())); + RCLCPP_WARN(getLogger(), "Solution : %d", static_cast(solution_ik.size())); for (int j = 0; j < static_cast(solution_ik[i].size()); ++j) { - RCLCPP_WARN(LOGGER, "%d: %f", j, solution_ik[i][j]); + RCLCPP_WARN(getLogger(), "%d: %f", j, solution_ik[i][j]); } - RCLCPP_WARN(LOGGER, " "); - RCLCPP_WARN(LOGGER, " "); } double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init); if (tmp_distance < min_distance) @@ -175,7 +180,8 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& (initial_guess - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position) / search_discretization_angle_); if (verbose) { - RCLCPP_WARN(LOGGER, "%f %f %f %d %d \n\n", initial_guess, pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, + RCLCPP_WARN(getLogger(), "%f %f %f %d %d \n\n", initial_guess, + pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments, num_negative_increments); } @@ -187,17 +193,17 @@ int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& return -1; q_init(free_angle_) = initial_guess + search_discretization_angle_ * count; if (verbose) - RCLCPP_WARN(LOGGER, "%d, %f", count, q_init(free_angle_)); + RCLCPP_WARN(getLogger(), "%d, %f", count, q_init(free_angle_)); loop_time = rclcpp::Clock(RCL_ROS_TIME).now().seconds() - start_time.seconds(); } if (loop_time >= timeout) { - RCLCPP_WARN(LOGGER, "IK Timed out in %f seconds", timeout); + RCLCPP_WARN(getLogger(), "IK Timed out in %f seconds", timeout); return TIMED_OUT; } else { - RCLCPP_WARN(LOGGER, "No IK solution was found"); + RCLCPP_WARN(getLogger(), "No IK solution was found"); return NO_IK_SOLUTION; } return NO_IK_SOLUTION; @@ -210,12 +216,13 @@ bool getKDLChain(const urdf::ModelInterface& model, const std::string& root_name KDL::Tree tree; if (!kdl_parser::treeFromUrdfModel(model, tree)) { - RCLCPP_ERROR(LOGGER, "Could not initialize tree object"); + RCLCPP_ERROR(getLogger(), "Could not initialize tree object"); return false; } if (!tree.getChain(root_name, tip_name, kdl_chain)) { - RCLCPP_ERROR(LOGGER, "Could not initialize chain object for base %s tip %s", root_name.c_str(), tip_name.c_str()); + RCLCPP_ERROR(getLogger(), "Could not initialize chain object for base %s tip %s", root_name.c_str(), + tip_name.c_str()); return false; } return true; @@ -275,11 +282,11 @@ bool PR2ArmKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, std::string xml_string; dimension_ = 7; - RCLCPP_WARN(LOGGER, "Loading KDL Tree"); + RCLCPP_WARN(getLogger(), "Loading KDL Tree"); if (!getKDLChain(*robot_model.getURDF(), base_frame_, tip_frames_[0], kdl_chain_)) { active_ = false; - RCLCPP_ERROR(LOGGER, "Could not load kdl tree"); + RCLCPP_ERROR(getLogger(), "Could not load kdl tree"); } jnt_to_pose_solver_ = std::make_shared(kdl_chain_); free_angle_ = 2; @@ -288,7 +295,7 @@ bool PR2ArmKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, *robot_model.getURDF(), base_frame_, tip_frames_[0], search_discretization, free_angle_); if (!pr2_arm_ik_solver_->active_) { - RCLCPP_ERROR(LOGGER, "Could not load ik"); + RCLCPP_ERROR(getLogger(), "Could not load ik"); active_ = false; } else @@ -301,17 +308,17 @@ bool PR2ArmKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, { for (const std::string& joint_name : ik_solver_info_.joint_names) { - RCLCPP_WARN(LOGGER, "PR2Kinematics:: joint name: %s", joint_name.c_str()); + RCLCPP_WARN(getLogger(), "PR2Kinematics:: joint name: %s", joint_name.c_str()); } for (const std::string& link_name : ik_solver_info_.link_names) { - RCLCPP_WARN(LOGGER, "PR2Kinematics can solve IK for %s", link_name.c_str()); + RCLCPP_WARN(getLogger(), "PR2Kinematics can solve IK for %s", link_name.c_str()); } for (const std::string& link_name : fk_solver_info_.link_names) { - RCLCPP_WARN(LOGGER, "PR2Kinematics can solve FK for %s", link_name.c_str()); + RCLCPP_WARN(getLogger(), "PR2Kinematics can solve FK for %s", link_name.c_str()); } - RCLCPP_WARN(LOGGER, "PR2KinematicsPlugin::active for %s", group_name.c_str()); + RCLCPP_WARN(getLogger(), "PR2KinematicsPlugin::active for %s", group_name.c_str()); } active_ = true; } @@ -335,7 +342,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik { if (!active_) { - RCLCPP_ERROR(LOGGER, "kinematics not active"); + RCLCPP_ERROR(getLogger(), "kinematics not active"); error_code.val = error_code.PLANNING_FAILED; return false; } @@ -375,7 +382,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik } else { - RCLCPP_WARN(LOGGER, "An IK solution could not be found"); + RCLCPP_WARN(getLogger(), "An IK solution could not be found"); error_code.val = error_code.NO_IK_SOLUTION; return false; } @@ -423,7 +430,7 @@ const std::vector& PR2ArmKinematicsPlugin::getJointNames() const { if (!active_) { - RCLCPP_ERROR(LOGGER, "kinematics not active"); + RCLCPP_ERROR(getLogger(), "kinematics not active"); } return ik_solver_info_.joint_names; } @@ -432,7 +439,7 @@ const std::vector& PR2ArmKinematicsPlugin::getLinkNames() const { if (!active_) { - RCLCPP_ERROR(LOGGER, "kinematics not active"); + RCLCPP_ERROR(getLogger(), "kinematics not active"); } return fk_solver_info_.link_names; } diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h index 7f5e387412..6c4d1aad1b 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h +++ b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h @@ -60,14 +60,14 @@ namespace core { MOVEIT_CLASS_FORWARD(RobotModel); // Defines RobotModelPtr, ConstPtr, WeakPtr... etc -static inline void checkInterpolationParamBounds(const rclcpp::Logger& LOGGER, double t) +static inline void checkInterpolationParamBounds(const rclcpp::Logger& logger, double t) { if (std::isnan(t) || std::isinf(t)) { throw Exception("Interpolation parameter is NaN or inf."); } - RCLCPP_WARN_STREAM_EXPRESSION(LOGGER, t < 0. || t > 1., "Interpolation parameter is not in the range [0, 1]: " << t); + RCLCPP_WARN_STREAM_EXPRESSION(logger, t < 0. || t > 1., "Interpolation parameter is not in the range [0, 1]: " << t); } /** \brief Definition of a kinematic model. This class is not thread diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index 505efe2b9b..f53ba4912a 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include @@ -51,7 +52,13 @@ namespace moveit { namespace core { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_utils.robot_model_test_utils"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("robot_model_test_utils"); +} +} // namespace moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name) { @@ -77,7 +84,8 @@ urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name) urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path); if (urdf_model == nullptr) { - RCLCPP_ERROR(LOGGER, "Cannot find URDF for %s. Make sure moveit_resources_ is installed", + RCLCPP_ERROR(getLogger(), + "Cannot find URDF for %s. Make sure moveit_resources_ is installed", robot_name.c_str()); } return urdf_model; @@ -150,22 +158,22 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& boost::split_regex(link_names, section, boost::regex("->")); if (link_names.empty()) { - RCLCPP_ERROR(LOGGER, "No links specified (empty section?)"); + RCLCPP_ERROR(getLogger(), "No links specified (empty section?)"); is_valid_ = false; return; } // First link should already be added. if (!urdf_model_->getLink(link_names[0])) { - RCLCPP_ERROR(LOGGER, "Link %s not present in builder yet!", link_names[0].c_str()); + RCLCPP_ERROR(getLogger(), "Link %s not present in builder yet!", link_names[0].c_str()); is_valid_ = false; return; } if (!joint_origins.empty() && link_names.size() - 1 != joint_origins.size()) { - RCLCPP_ERROR(LOGGER, "There should be one more link (%zu) than there are joint origins (%zu)", link_names.size(), - joint_origins.size()); + RCLCPP_ERROR(getLogger(), "There should be one more link (%zu) than there are joint origins (%zu)", + link_names.size(), joint_origins.size()); is_valid_ = false; return; } @@ -176,7 +184,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& // These links shouldn't be present already. if (urdf_model_->getLink(link_names[i])) { - RCLCPP_ERROR(LOGGER, "Link %s is already specified", link_names[i].c_str()); + RCLCPP_ERROR(getLogger(), "Link %s is already specified", link_names[i].c_str()); is_valid_ = false; return; } @@ -223,7 +231,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string& } else { - RCLCPP_ERROR(LOGGER, "No such joint type as %s", type.c_str()); + RCLCPP_ERROR(getLogger(), "No such joint type as %s", type.c_str()); is_valid_ = false; return; } @@ -246,7 +254,7 @@ void RobotModelBuilder::addInertial(const std::string& link_name, double mass, g { if (!urdf_model_->getLink(link_name)) { - RCLCPP_ERROR(LOGGER, "Link %s not present in builder yet!", link_name.c_str()); + RCLCPP_ERROR(getLogger(), "Link %s not present in builder yet!", link_name.c_str()); is_valid_ = false; return; } @@ -283,7 +291,7 @@ void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std: { if (dims.size() != 3) { - RCLCPP_ERROR(LOGGER, "There can only be 3 dimensions of a box (given %zu!)", dims.size()); + RCLCPP_ERROR(getLogger(), "There can only be 3 dimensions of a box (given %zu!)", dims.size()); is_valid_ = false; return; } @@ -309,7 +317,7 @@ void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urd { if (!urdf_model_->getLink(link_name)) { - RCLCPP_ERROR(LOGGER, "Link %s not present in builder yet!", link_name.c_str()); + RCLCPP_ERROR(getLogger(), "Link %s not present in builder yet!", link_name.c_str()); is_valid_ = false; return; } @@ -327,7 +335,7 @@ void RobotModelBuilder::addLinkVisual(const std::string& link_name, const urdf:: { if (!urdf_model_->getLink(link_name)) { - RCLCPP_ERROR(LOGGER, "Link %s not present in builder yet!", link_name.c_str()); + RCLCPP_ERROR(getLogger(), "Link %s not present in builder yet!", link_name.c_str()); is_valid_ = false; return; } @@ -430,7 +438,7 @@ moveit::core::RobotModelPtr RobotModelBuilder::build() } catch (urdf::ParseError& e) { - RCLCPP_ERROR(LOGGER, "Failed to build tree: %s", e.what()); + RCLCPP_ERROR(getLogger(), "Failed to build tree: %s", e.what()); return robot_model; } @@ -441,7 +449,7 @@ moveit::core::RobotModelPtr RobotModelBuilder::build() } catch (urdf::ParseError& e) { - RCLCPP_ERROR(LOGGER, "Failed to find root link: %s", e.what()); + RCLCPP_ERROR(getLogger(), "Failed to find root link: %s", e.what()); return robot_model; } srdf_writer_->updateSRDFModel(*urdf_model_); diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h index 5024be7829..b7a70b6086 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h @@ -47,11 +47,10 @@ #include #include #include +#include namespace cached_ik_kinematics_plugin { -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit_cached_ik_kinematics_plugin.cached_ik_kinematics_plugin"); /** \brief A cache of inverse kinematic solutions */ class IKCache @@ -281,7 +280,8 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin { if (tip_frames.size() != 1) { - RCLCPP_ERROR(LOGGER, "This solver does not support multiple tip frames"); + RCLCPP_ERROR(moveit::getLogger("cached_ik_kinematics_plugin"), + "This solver does not support multiple tip frames"); return false; } diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp index 78800b0019..91653cd348 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp +++ b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp @@ -39,9 +39,17 @@ #include #include +#include namespace cached_ik_kinematics_plugin { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("dynamics_solver"); +} +} // namespace IKCache::IKCache() { // set distance function for nearest-neighbor queries @@ -95,8 +103,8 @@ void IKCache::initializeCache(const std::string& robot_id, const std::string& gr unsigned int num_tips; cache_file.read(reinterpret_cast(&num_tips), sizeof(unsigned int)); - RCLCPP_INFO(LOGGER, "Found %d IK solutions for a %d-dof system with %d end effectors in %s", last_saved_cache_size_, - num_dofs, num_tips, cache_file_name_.string().c_str()); + RCLCPP_INFO(getLogger(), "Found %d IK solutions for a %d-dof system with %d end effectors in %s", + last_saved_cache_size_, num_dofs, num_tips, cache_file_name_.string().c_str()); unsigned int position_size = 3 * sizeof(tf2Scalar); unsigned int orientation_size = 4 * sizeof(tf2Scalar); @@ -123,9 +131,9 @@ void IKCache::initializeCache(const std::string& robot_id, const std::string& gr memcpy(&entry.second[0], buffer + offset_conf, config_size); ik_cache_.push_back(entry); } - RCLCPP_INFO(LOGGER, "freeing buffer"); + RCLCPP_INFO(getLogger(), "freeing buffer"); delete[] buffer; - RCLCPP_INFO(LOGGER, "freed buffer"); + RCLCPP_INFO(getLogger(), "freed buffer"); std::vector ik_entry_ptrs(last_saved_cache_size_); for (unsigned int i = 0; i < last_saved_cache_size_; ++i) ik_entry_ptrs[i] = &ik_cache_[i]; @@ -134,7 +142,7 @@ void IKCache::initializeCache(const std::string& robot_id, const std::string& gr num_joints_ = num_joints; - RCLCPP_INFO(LOGGER, "cache file %s initialized!", cache_file_name_.string().c_str()); + RCLCPP_INFO(getLogger(), "cache file %s initialized!", cache_file_name_.string().c_str()); } double IKCache::configDistance2(const std::vector& config1, const std::vector& config2) const @@ -216,9 +224,9 @@ void IKCache::updateCache(const IKEntry& nearest, const std::vector& poses void IKCache::saveCache() const { if (cache_file_name_.empty()) - RCLCPP_ERROR(LOGGER, "can't save cache before initialization"); + RCLCPP_ERROR(getLogger(), "can't save cache before initialization"); - RCLCPP_INFO(LOGGER, "writing %ld IK solutions to %s", ik_cache_.size(), cache_file_name_.string().c_str()); + RCLCPP_INFO(getLogger(), "writing %ld IK solutions to %s", ik_cache_.size(), cache_file_name_.string().c_str()); std::ofstream cache_file(cache_file_name_, std::ios_base::binary | std::ios_base::out); unsigned int position_size = 3 * sizeof(tf2Scalar); @@ -266,9 +274,9 @@ void IKCache::verifyCache(kdl_kinematics_plugin::KDLKinematicsPlugin& fk) const if (error > max_error) max_error = error; if (error > 1e-4) - RCLCPP_ERROR(LOGGER, "Cache entry is invalid, error = %g", error); + RCLCPP_ERROR(getLogger(), "Cache entry is invalid, error = %g", error); } - RCLCPP_INFO(LOGGER, "Max. error in cache entries is %g", max_error); + RCLCPP_INFO(getLogger(), "Max. error in cache entries is %g", max_error); } IKCache::Pose::Pose(const geometry_msgs::msg::Pose& pose) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index d0c71b113e..14c2d776dd 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -48,6 +48,7 @@ #include #include #include +#include using namespace moveit::core; @@ -62,6 +63,14 @@ enum SEARCH_MODE namespace _NAMESPACE_ { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("_ROBOT_NAME___GROUP_NAME__ikfast_solver"); +} +} // namespace + #define IKFAST_NO_MAIN // Don't include main() from IKFast /// \brief The types of inverse kinematics parameterizations supported. @@ -151,8 +160,6 @@ struct LimitObeyingSol // Code generated by IKFast56/61 #include "_ROBOT_NAME___GROUP_NAME__ikfast_solver.cpp" -static const rclcpp::Logger LOGGER = rclcpp::get_logger("_ROBOT_NAME___GROUP_NAME__ikfast_solver"); - class IKFastKinematicsPlugin : public kinematics::KinematicsBase { std::vector joint_names_; @@ -391,7 +398,7 @@ bool IKFastKinematicsPlugin::computeRelativeTransform(const std::string& from, c if (robot_model_->getRigidlyConnectedParentLinkModel(from_link) != robot_model_->getRigidlyConnectedParentLinkModel(to_link)) { - RCLCPP_ERROR_STREAM(LOGGER, "Link frames " << from << " and " << to << " are not rigidly connected."); + RCLCPP_ERROR_STREAM(getLogger(), "Link frames " << from << " and " << to << " are not rigidly connected."); return false; } @@ -407,7 +414,7 @@ bool IKFastKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, { if (tip_frames.size() != 1) { - RCLCPP_ERROR(LOGGER, "Expecting exactly one tip frame."); + RCLCPP_ERROR(getLogger(), "Expecting exactly one tip frame."); return false; } @@ -418,22 +425,22 @@ bool IKFastKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); - RCLCPP_INFO_STREAM(LOGGER, "Using link_prefix: '" << params_.link_prefix << '\''); + RCLCPP_INFO_STREAM(getLogger(), "Using link_prefix: '" << params_.link_prefix << '\''); // verbose error output. subsequent checks in computeRelativeTransform return false then if (!robot_model.hasLinkModel(tip_frames_[0])) - RCLCPP_ERROR_STREAM(LOGGER, "tip frame '" << tip_frames_[0] << "' does not exist."); + RCLCPP_ERROR_STREAM(getLogger(), "tip frame '" << tip_frames_[0] << "' does not exist."); if (!robot_model.hasLinkModel(base_frame_)) - RCLCPP_ERROR_STREAM(LOGGER, "base_frame '" << base_frame_ << "' does not exist."); + RCLCPP_ERROR_STREAM(getLogger(), "base_frame '" << base_frame_ << "' does not exist."); if (!robot_model.hasLinkModel(params_.link_prefix + IKFAST_TIP_FRAME_)) - RCLCPP_ERROR_STREAM(LOGGER, "prefixed tip frame '" << params_.link_prefix + IKFAST_TIP_FRAME_ - << "' does not exist. " - "Please check your link_prefix parameter."); + RCLCPP_ERROR_STREAM(getLogger(), "prefixed tip frame '" << params_.link_prefix + IKFAST_TIP_FRAME_ + << "' does not exist. " + "Please check your link_prefix parameter."); if (!robot_model.hasLinkModel(params_.link_prefix + IKFAST_BASE_FRAME_)) - RCLCPP_ERROR_STREAM(LOGGER, "prefixed base frame '" << params_.link_prefix + IKFAST_BASE_FRAME_ - << "' does not exist. " - "Please check your link_prefix parameter."); + RCLCPP_ERROR_STREAM(getLogger(), "prefixed base frame '" << params_.link_prefix + IKFAST_BASE_FRAME_ + << "' does not exist. " + "Please check your link_prefix parameter."); // This IKFast solution was generated with IKFAST_TIP_FRAME_ and IKFAST_BASE_FRAME_. // It is often the case that fixed joints are added to these links to model things like // a robot mounted on a table or a robot with an end effector attached to the last link. @@ -452,7 +459,7 @@ bool IKFastKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, if (free_params_.size() > 1) { - RCLCPP_ERROR(LOGGER, "Only one free joint parameter supported!"); + RCLCPP_ERROR(getLogger(), "Only one free joint parameter supported!"); return false; } else if (free_params_.size() == 1) @@ -465,21 +472,21 @@ bool IKFastKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group_name); if (!jmg) { - RCLCPP_ERROR_STREAM(LOGGER, "Unknown planning group: " << group_name); + RCLCPP_ERROR_STREAM(getLogger(), "Unknown planning group: " << group_name); return false; } - RCLCPP_DEBUG(LOGGER, "Registering joints and links"); + RCLCPP_DEBUG(getLogger(), "Registering joints and links"); const moveit::core::LinkModel* link = robot_model_->getLinkModel(tip_frames_[0]); const moveit::core::LinkModel* base_link = robot_model_->getLinkModel(base_frame_); while (link && link != base_link) { - RCLCPP_DEBUG_STREAM(LOGGER, "Link " << link->getName()); + RCLCPP_DEBUG_STREAM(getLogger(), "Link " << link->getName()); link_names_.push_back(link->getName()); const moveit::core::JointModel* joint = link->getParentJointModel(); if (joint->getType() != joint->UNKNOWN && joint->getType() != joint->FIXED && joint->getVariableCount() == 1) { - RCLCPP_DEBUG_STREAM(LOGGER, "Adding joint " << joint->getName()); + RCLCPP_DEBUG_STREAM(getLogger(), "Adding joint " << joint->getName()); joint_names_.push_back(joint->getName()); const moveit::core::VariableBounds& bounds = joint->getVariableBounds()[0]; @@ -492,8 +499,8 @@ bool IKFastKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, if (joint_names_.size() != num_joints_) { - RCLCPP_FATAL(LOGGER, "Joint numbers of RobotModel (%zd) and IKFast solver (%zd) do not match", joint_names_.size(), - num_joints_); + RCLCPP_FATAL(getLogger(), "Joint numbers of RobotModel (%zd) and IKFast solver (%zd) do not match", + joint_names_.size(), num_joints_); return false; } @@ -504,9 +511,9 @@ bool IKFastKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end()); for (size_t joint_id = 0; joint_id < num_joints_; ++joint_id) - RCLCPP_DEBUG_STREAM(LOGGER, joint_names_[joint_id] << ' ' << joint_min_vector_[joint_id] << ' ' - << joint_max_vector_[joint_id] << ' ' - << joint_has_limits_vector_[joint_id]); + RCLCPP_DEBUG_STREAM(getLogger(), joint_names_[joint_id] << ' ' << joint_min_vector_[joint_id] << ' ' + << joint_max_vector_[joint_id] << ' ' + << joint_has_limits_vector_[joint_id]); initialized_ = true; return true; @@ -516,28 +523,28 @@ void IKFastKinematicsPlugin::setSearchDiscretization(const std::mapfirst != redundant_joint_indices_[0]) { std::string redundant_joint = joint_names_[free_params_[0]]; - RCLCPP_ERROR_STREAM(LOGGER, "Attempted to discretize a non-redundant joint " - << discretization.begin()->first << ", only joint '" << redundant_joint - << "' with index " << redundant_joint_indices_[0] << " is redundant."); + RCLCPP_ERROR_STREAM(getLogger(), "Attempted to discretize a non-redundant joint " + << discretization.begin()->first << ", only joint '" << redundant_joint + << "' with index " << redundant_joint_indices_[0] << " is redundant."); return; } if (discretization.begin()->second <= 0.0) { - RCLCPP_ERROR_STREAM(LOGGER, "Discretization can not takes values that are <= 0"); + RCLCPP_ERROR_STREAM(getLogger(), "Discretization can not takes values that are <= 0"); return; } @@ -547,7 +554,7 @@ void IKFastKinematicsPlugin::setSearchDiscretization(const std::map& redundant_joint_indices) { - RCLCPP_ERROR(LOGGER, "Changing the redundant joints isn't permitted by this group's solver "); + RCLCPP_ERROR(getLogger(), "Changing the redundant joints isn't permitted by this group's solver "); return false; } @@ -601,7 +608,7 @@ size_t IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector& link_ // eerot differs depending on IK type. The Transform6D IK type is the only // one for which a 3x3 rotation matrix is returned, which means we can only // compute FK for that IK type. - RCLCPP_ERROR(LOGGER, "Can only compute FK for Transform6D IK type!"); + RCLCPP_ERROR(getLogger(), "Can only compute FK for Transform6D IK type!"); return false; } KDL::Frame p_out; if (link_names.size() == 0) { - RCLCPP_WARN_STREAM(LOGGER, "Link names with nothing"); + RCLCPP_WARN_STREAM(getLogger(), "Link names with nothing"); return false; } if (link_names.size() != 1 || link_names[0] != getTipFrame()) { - RCLCPP_ERROR(LOGGER, "Can compute FK for %s only", getTipFrame().c_str()); + RCLCPP_ERROR(getLogger(), "Can compute FK for %s only", getTipFrame().c_str()); return false; } @@ -797,7 +804,7 @@ bool IKFastKinematicsPlugin::getPositionFK(const std::vector& link_ if (joint_angles.size() != num_joints_) { - RCLCPP_ERROR(LOGGER, "Unexpected number of joint angles"); + RCLCPP_ERROR(getLogger(), "Unexpected number of joint angles"); return false; } @@ -866,7 +873,7 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik // Check if there are no redundant joints if (free_params_.size() == 0) { - RCLCPP_DEBUG_STREAM(LOGGER, "No need to search since no free params/redundant joints"); + RCLCPP_DEBUG_STREAM(getLogger(), "No need to search since no free params/redundant joints"); std::vector ik_poses(1, ik_pose); std::vector> solutions; @@ -874,7 +881,7 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik // Find all IK solutions within joint limits if (!getPositionIK(ik_poses, ik_seed_state, solutions, kinematic_result, options)) { - RCLCPP_DEBUG(LOGGER, "No solution whatsoever"); + RCLCPP_DEBUG(getLogger(), "No solution whatsoever"); error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; return false; } @@ -902,12 +909,12 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik if (error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { solution = solutions_obey_limits[i].value; - RCLCPP_DEBUG(LOGGER, "Solution passes callback"); + RCLCPP_DEBUG(getLogger(), "Solution passes callback"); return true; } } - RCLCPP_DEBUG_STREAM(LOGGER, "Solution has error code " << error_code.val); + RCLCPP_DEBUG_STREAM(getLogger(), "Solution has error code " << error_code.val); return false; } else @@ -922,14 +929,14 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik // Error Checking if (!initialized_) { - RCLCPP_ERROR(LOGGER, "Kinematics not active"); + RCLCPP_ERROR(getLogger(), "Kinematics not active"); error_code.val = error_code.NO_IK_SOLUTION; return false; } if (ik_seed_state.size() != num_joints_) { - RCLCPP_ERROR_STREAM(LOGGER, + RCLCPP_ERROR_STREAM(getLogger(), "Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; @@ -937,8 +944,8 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik if (!consistency_limits.empty() && consistency_limits.size() != num_joints_) { - RCLCPP_ERROR_STREAM(LOGGER, "Consistency limits be empty or must have size " << num_joints_ << " instead of size " - << consistency_limits.size()); + RCLCPP_ERROR_STREAM(getLogger(), "Consistency limits be empty or must have size " + << num_joints_ << " instead of size " << consistency_limits.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } @@ -981,11 +988,11 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik // ------------------------------------------------------------------------------------------------- // Begin searching - RCLCPP_DEBUG_STREAM(LOGGER, "Free param is " << free_params_[0] << " initial guess is " << initial_guess - << ", # positive increments: " << num_positive_increments - << ", # negative increments: " << num_negative_increments); + RCLCPP_DEBUG_STREAM(getLogger(), "Free param is " << free_params_[0] << " initial guess is " << initial_guess + << ", # positive increments: " << num_positive_increments + << ", # negative increments: " << num_negative_increments); if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000) - RCLCPP_WARN_STREAM_ONCE(LOGGER, "Large search space, consider increasing the search discretization"); + RCLCPP_WARN_STREAM_ONCE(getLogger(), "Large search space, consider increasing the search discretization"); double best_costs = -1.0; std::vector best_solution; @@ -996,7 +1003,7 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik IkSolutionList solutions; size_t numsol = solve(frame, vfree, solutions); - RCLCPP_DEBUG_STREAM(LOGGER, "Found " << numsol << " solutions from IKFast"); + RCLCPP_DEBUG_STREAM(getLogger(), "Found " << numsol << " solutions from IKFast"); if (numsol > 0) { @@ -1014,7 +1021,7 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik obeys_limits = false; break; } - // RCLCPP_INFO_STREAM(LOGGER,"Num " << i << " value " << sol[i] << " has limits " << + // RCLCPP_INFO_STREAM(getLogger(),"Num " << i << " value " << sol[i] << " has limits " << // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]); } if (obeys_limits) @@ -1066,10 +1073,10 @@ bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik } vfree[0] = initial_guess + search_discretization * counter; - // RCLCPP_DEBUG_STREAM(LOGGER,"Attempt " << counter << " with 0th free joint having value " << vfree[0]); + // RCLCPP_DEBUG_STREAM(getLogger(),"Attempt " << counter << " with 0th free joint having value " << vfree[0]); } - RCLCPP_DEBUG_STREAM(LOGGER, "Valid solutions: " << nvalid << '/' << nattempts); + RCLCPP_DEBUG_STREAM(getLogger(), "Valid solutions: " << nvalid << '/' << nattempts); if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0) { @@ -1089,18 +1096,19 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_po moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { - RCLCPP_DEBUG_STREAM(LOGGER, "getPositionIK"); + RCLCPP_DEBUG_STREAM(getLogger(), "getPositionIK"); if (!initialized_) { - RCLCPP_ERROR(LOGGER, "kinematics not active"); + RCLCPP_ERROR(getLogger(), "kinematics not active"); return false; } if (ik_seed_state.size() < num_joints_) { - RCLCPP_ERROR_STREAM(LOGGER, "ik_seed_state only has " << ik_seed_state.size() - << " entries, this ikfast solver requires " << num_joints_); + RCLCPP_ERROR_STREAM(getLogger(), "ik_seed_state only has " << ik_seed_state.size() + << " entries, this ikfast solver requires " + << num_joints_); return false; } @@ -1111,10 +1119,10 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_po if (joint_has_limits_vector_[i] && ((ik_seed_state[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) || (ik_seed_state[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE)))) { - RCLCPP_DEBUG_STREAM(LOGGER, "IK seed not in limits! " << static_cast(i) << " value " << ik_seed_state[i] - << " has limit: " << joint_has_limits_vector_[i] - << " being " << joint_min_vector_[i] << " to " - << joint_max_vector_[i]); + RCLCPP_DEBUG_STREAM(getLogger(), "IK seed not in limits! " << static_cast(i) << " value " << ik_seed_state[i] + << " has limit: " << joint_has_limits_vector_[i] + << " being " << joint_min_vector_[i] << " to " + << joint_max_vector_[i]); return false; } } @@ -1123,7 +1131,7 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_po for (std::size_t i = 0; i < free_params_.size(); ++i) { int p = free_params_[i]; - RCLCPP_ERROR(LOGGER, "%u is %f", p, ik_seed_state[p]); // DTC + RCLCPP_ERROR(getLogger(), "%u is %f", p, ik_seed_state[p]); // DTC vfree[i] = ik_seed_state[p]; } @@ -1132,7 +1140,7 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_po IkSolutionList solutions; size_t numsol = solve(frame, vfree, solutions); - RCLCPP_DEBUG_STREAM(LOGGER, "Found " << numsol << " solutions from IKFast"); + RCLCPP_DEBUG_STREAM(getLogger(), "Found " << numsol << " solutions from IKFast"); std::vector solutions_obey_limits; @@ -1143,8 +1151,8 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_po { std::vector sol; getSolution(solutions, ik_seed_state, s, sol); - RCLCPP_DEBUG(LOGGER, "Sol %d: %e %e %e %e %e %e", static_cast(s), sol[0], sol[1], sol[2], sol[3], - sol[4], sol[5]); + RCLCPP_DEBUG(getLogger(), "Sol %d: %e %e %e %e %e %e", static_cast(s), sol[0], sol[1], sol[2], + sol[3], sol[4], sol[5]); bool obeys_limits = true; for (std::size_t i = 0; i < sol.size(); ++i) @@ -1155,9 +1163,10 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_po { // One element of solution is not within limits obeys_limits = false; - RCLCPP_DEBUG_STREAM(LOGGER, "Not in limits! " << static_cast(i) << " value " << sol[i] - << " has limit: " << joint_has_limits_vector_[i] << " being " - << joint_min_vector_[i] << " to " << joint_max_vector_[i]); + RCLCPP_DEBUG_STREAM(getLogger(), "Not in limits! " << static_cast(i) << " value " << sol[i] + << " has limit: " << joint_has_limits_vector_[i] + << " being " << joint_min_vector_[i] << " to " + << joint_max_vector_[i]); break; } } @@ -1177,7 +1186,7 @@ bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_po } else { - RCLCPP_DEBUG(LOGGER, "No IK solution"); + RCLCPP_DEBUG(getLogger(), "No IK solution"); } // Sort the solutions under limits and find the one that is closest to ik_seed_state @@ -1199,33 +1208,34 @@ bool IKFastKinematicsPlugin::getPositionIK(const std::vector 1) { - RCLCPP_ERROR(LOGGER, "ik_poses contains multiple entries, only one is allowed"); + RCLCPP_ERROR(getLogger(), "ik_poses contains multiple entries, only one is allowed"); result.kinematic_error = kinematics::KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED; return false; } if (ik_seed_state.size() < num_joints_) { - RCLCPP_ERROR_STREAM(LOGGER, "ik_seed_state only has " << ik_seed_state.size() - << " entries, this ikfast solver requires " << num_joints_); + RCLCPP_ERROR_STREAM(getLogger(), "ik_seed_state only has " << ik_seed_state.size() + << " entries, this ikfast solver requires " + << num_joints_); return false; } @@ -1254,7 +1264,7 @@ bool IKFastKinematicsPlugin::getPositionIK(const std::vector (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE)))) { result.kinematic_error = kinematics::KinematicErrors::IK_SEED_OUTSIDE_LIMITS; - RCLCPP_ERROR_STREAM(LOGGER, "ik seed is out of bounds"); + RCLCPP_ERROR_STREAM(getLogger(), "ik seed is out of bounds"); return false; } } @@ -1281,7 +1291,7 @@ bool IKFastKinematicsPlugin::getPositionIK(const std::vector 0) { @@ -1306,9 +1316,10 @@ bool IKFastKinematicsPlugin::getPositionIK(const std::vector #include +#include #include #include @@ -47,7 +48,14 @@ namespace kdl_kinematics_plugin { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_kdl_kinematics_plugin.kdl_kinematics_plugin"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("kdl_kinematics_plugin"); +} +} // namespace + static rclcpp::Clock steady_clock = rclcpp::Clock(RCL_ROS_TIME); KDLKinematicsPlugin::KDLKinematicsPlugin() : initialized_(false) @@ -95,7 +103,7 @@ void KDLKinematicsPlugin::getJointWeights() auto it = std::find(joint_names.begin(), joint_names.end(), joint_name); if (it == joint_names.cend()) { - RCLCPP_WARN(LOGGER, "Joint '%s' is not an active joint in group '%s'", joint_name.c_str(), + RCLCPP_WARN(getLogger(), "Joint '%s' is not an active joint in group '%s'", joint_name.c_str(), joint_model_group_->getName().c_str()); continue; } @@ -105,9 +113,9 @@ void KDLKinematicsPlugin::getJointWeights() } RCLCPP_INFO_STREAM( - LOGGER, "Joint weights for group '" - << getGroupName() << "': " - << Eigen::Map(joint_weights_.data(), joint_weights_.size()).transpose()); + getLogger(), "Joint weights for group '" + << getGroupName() << "': " + << Eigen::Map(joint_weights_.data(), joint_weights_.size()).transpose()); } bool KDLKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, @@ -128,12 +136,12 @@ bool KDLKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const if (!joint_model_group_->isChain()) { - RCLCPP_ERROR(LOGGER, "Group '%s' is not a chain", group_name.c_str()); + RCLCPP_ERROR(getLogger(), "Group '%s' is not a chain", group_name.c_str()); return false; } if (!joint_model_group_->isSingleDOFJoints()) { - RCLCPP_ERROR(LOGGER, "Group '%s' includes joints that have more than 1 DOF", group_name.c_str()); + RCLCPP_ERROR(getLogger(), "Group '%s' includes joints that have more than 1 DOF", group_name.c_str()); return false; } @@ -141,12 +149,12 @@ bool KDLKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const if (!kdl_parser::treeFromUrdfModel(*robot_model.getURDF(), kdl_tree)) { - RCLCPP_ERROR(LOGGER, "Could not initialize tree object"); + RCLCPP_ERROR(getLogger(), "Could not initialize tree object"); return false; } if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_)) { - RCLCPP_ERROR(LOGGER, "Could not initialize chain object"); + RCLCPP_ERROR(getLogger(), "Could not initialize chain object"); return false; } @@ -165,7 +173,7 @@ bool KDLKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const if (!joint_model_group_->hasLinkModel(getTipFrame())) { - RCLCPP_ERROR(LOGGER, "Could not find tip name in joint group '%s'", group_name.c_str()); + RCLCPP_ERROR(getLogger(), "Could not find tip name in joint group '%s'", group_name.c_str()); return false; } solver_info_.link_names.push_back(getTipFrame()); @@ -233,7 +241,7 @@ bool KDLKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const fk_solver_ = std::make_unique(kdl_chain_); initialized_ = true; - RCLCPP_DEBUG(LOGGER, "KDL solver initialized"); + RCLCPP_DEBUG(getLogger(), "KDL solver initialized"); return true; } @@ -297,14 +305,14 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po const rclcpp::Time start_time = steady_clock.now(); if (!initialized_) { - RCLCPP_ERROR(LOGGER, "kinematics solver not initialized"); + RCLCPP_ERROR(getLogger(), "kinematics solver not initialized"); error_code.val = error_code.NO_IK_SOLUTION; return false; } if (ik_seed_state.size() != dimension_) { - RCLCPP_ERROR(LOGGER, "Seed state must have size %d instead of size %zu\n", dimension_, ik_seed_state.size()); + RCLCPP_ERROR(getLogger(), "Seed state must have size %d instead of size %zu\n", dimension_, ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } @@ -315,7 +323,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po { if (consistency_limits.size() != dimension_) { - RCLCPP_ERROR(LOGGER, "Consistency limits must be empty or have size %d instead of size %zu\n", dimension_, + RCLCPP_ERROR(getLogger(), "Consistency limits must be empty or have size %d instead of size %zu\n", dimension_, consistency_limits.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; @@ -330,7 +338,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po auto orientation_vs_position_weight = params_.position_only_ik ? 0.0 : params_.orientation_vs_position; if (orientation_vs_position_weight == 0.0) - RCLCPP_INFO(LOGGER, "Using position only ik"); + RCLCPP_INFO(getLogger(), "Using position only ik"); Eigen::Matrix cartesian_weights; cartesian_weights.topRows<3>().setConstant(1.0); @@ -348,10 +356,10 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po KDL::Frame pose_desired; tf2::fromMsg(ik_pose, pose_desired); - RCLCPP_DEBUG_STREAM(LOGGER, "searchPositionIK: Position request pose is " - << ik_pose.position.x << ' ' << ik_pose.position.y << ' ' << ik_pose.position.z << ' ' - << ik_pose.orientation.x << ' ' << ik_pose.orientation.y << ' ' - << ik_pose.orientation.z << ' ' << ik_pose.orientation.w); + RCLCPP_DEBUG_STREAM(getLogger(), "searchPositionIK: Position request pose is " + << ik_pose.position.x << ' ' << ik_pose.position.y << ' ' << ik_pose.position.z + << ' ' << ik_pose.orientation.x << ' ' << ik_pose.orientation.y << ' ' + << ik_pose.orientation.z << ' ' << ik_pose.orientation.w); unsigned int attempt = 0; do @@ -367,7 +375,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po { getRandomConfiguration(jnt_pos_in.data); } - RCLCPP_DEBUG_STREAM(LOGGER, "New random configuration (" << attempt << "): " << jnt_pos_in); + RCLCPP_DEBUG_STREAM(getLogger(), "New random configuration (" << attempt << "): " << jnt_pos_in); } int ik_valid = @@ -389,14 +397,14 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po // solution passed consistency check and solution callback error_code.val = error_code.SUCCESS; - RCLCPP_DEBUG_STREAM(LOGGER, "Solved after " << (steady_clock.now() - start_time).seconds() << " < " << timeout - << "s and " << attempt << " attempts"); + RCLCPP_DEBUG_STREAM(getLogger(), "Solved after " << (steady_clock.now() - start_time).seconds() << " < " + << timeout << "s and " << attempt << " attempts"); return true; } } while (!timedOut(start_time, timeout)); - RCLCPP_DEBUG_STREAM(LOGGER, "IK timed out after " << (steady_clock.now() - start_time).seconds() << " > " << timeout - << "s and " << attempt << " attempts"); + RCLCPP_DEBUG_STREAM(getLogger(), "IK timed out after " << (steady_clock.now() - start_time).seconds() << " > " + << timeout << "s and " << attempt << " attempts"); error_code.val = error_code.TIMED_OUT; return false; } @@ -415,7 +423,7 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, con extra_joint_weights.setOnes(); q_out = q_init; - RCLCPP_DEBUG_STREAM(LOGGER, "Input: " << q_init); + RCLCPP_DEBUG_STREAM(getLogger(), "Input: " << q_init); unsigned int i; bool success = false; @@ -423,7 +431,7 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, con { fk_solver_->JntToCart(q_out, f); delta_twist = diff(f, p_in); - RCLCPP_DEBUG_STREAM(LOGGER, "[" << std::setw(3) << i << "] delta_twist: " << delta_twist); + RCLCPP_DEBUG_STREAM(getLogger(), "[" << std::setw(3) << i << "] delta_twist: " << delta_twist); // check norms of position and orientation errors const double position_error = delta_twist.vel.Norm(); @@ -441,7 +449,7 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, con double old_step_size = step_size; step_size *= std::min(0.2, last_delta_twist_norm / delta_twist_norm); // reduce scale; KDL::Multiply(delta_q, step_size / old_step_size, delta_q); - RCLCPP_DEBUG(LOGGER, " error increased: %f -> %f, scale: %f", last_delta_twist_norm, delta_twist_norm, + RCLCPP_DEBUG(getLogger(), " error increased: %f -> %f, scale: %f", last_delta_twist_norm, delta_twist_norm, step_size); q_out = q_backup; // restore previous unclipped joint values } @@ -457,7 +465,7 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, con clipToJointLimits(q_out, delta_q, extra_joint_weights); const double delta_q_norm = delta_q.data.lpNorm<1>(); - RCLCPP_DEBUG(LOGGER, "[%3d] pos err: %f rot err: %f delta_q: %f", i, position_error, orientation_error, + RCLCPP_DEBUG(getLogger(), "[%3d] pos err: %f rot err: %f delta_q: %f", i, position_error, orientation_error, delta_q_norm); if (delta_q_norm < params_.epsilon) // stuck in singularity { @@ -473,12 +481,12 @@ int KDLKinematicsPlugin::CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, con KDL::Add(q_out, delta_q, q_out); - RCLCPP_DEBUG_STREAM(LOGGER, " delta_q: " << delta_q); - RCLCPP_DEBUG_STREAM(LOGGER, " q: " << q_out); + RCLCPP_DEBUG_STREAM(getLogger(), " delta_q: " << delta_q); + RCLCPP_DEBUG_STREAM(getLogger(), " q: " << q_out); } int result = (i == max_iter) ? -3 : (success ? 0 : -2); - RCLCPP_DEBUG_STREAM(LOGGER, "Result " << result << " after " << i << " iterations: " << q_out); + RCLCPP_DEBUG_STREAM(getLogger(), "Result " << result << " after " << i << " iterations: " << q_out); return result; } @@ -514,13 +522,13 @@ bool KDLKinematicsPlugin::getPositionFK(const std::vector& link_nam { if (!initialized_) { - RCLCPP_ERROR(LOGGER, "kinematics solver not initialized"); + RCLCPP_ERROR(getLogger(), "kinematics solver not initialized"); return false; } poses.resize(link_names.size()); if (joint_angles.size() != dimension_) { - RCLCPP_ERROR(LOGGER, "Joint angles vector must have size: %d", dimension_); + RCLCPP_ERROR(getLogger(), "Joint angles vector must have size: %d", dimension_); return false; } @@ -537,7 +545,7 @@ bool KDLKinematicsPlugin::getPositionFK(const std::vector& link_nam } else { - RCLCPP_ERROR(LOGGER, "Could not compute FK for %s", link_names[i].c_str()); + RCLCPP_ERROR(getLogger(), "Could not compute FK for %s", link_names[i].c_str()); valid = false; } } diff --git a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp index cbce29dcc1..e2707e64ce 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -39,6 +39,7 @@ #include #include #include +#include // Eigen #include @@ -49,7 +50,13 @@ CLASS_LOADER_REGISTER_CLASS(srv_kinematics_plugin::SrvKinematicsPlugin, kinemati namespace srv_kinematics_plugin { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_srv_kinematics_plugin.srv_kinematics_plugin"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("srv_kinematics_plugin"); +} +} // namespace SrvKinematicsPlugin::SrvKinematicsPlugin() : active_(false) { @@ -62,7 +69,7 @@ bool SrvKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const node_ = node; bool debug = false; - RCLCPP_INFO(LOGGER, "SrvKinematicsPlugin initializing"); + RCLCPP_INFO(getLogger(), "SrvKinematicsPlugin initializing"); // Get Solver Parameters std::string kinematics_param_prefix = "robot_description_kinematics." + group_name; @@ -84,10 +91,10 @@ bool SrvKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const // Get the dimension of the planning group dimension_ = joint_model_group_->getVariableCount(); - RCLCPP_INFO_STREAM(LOGGER, "Dimension planning group '" - << group_name << "': " << dimension_ - << ". Active Joints Models: " << joint_model_group_->getActiveJointModels().size() - << ". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size()); + RCLCPP_INFO_STREAM(getLogger(), "Dimension planning group '" + << group_name << "': " << dimension_ + << ". Active Joints Models: " << joint_model_group_->getActiveJointModels().size() + << ". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size()); // Copy joint names for (std::size_t i = 0; i < joint_model_group_->getJointModels().size(); ++i) @@ -97,7 +104,7 @@ bool SrvKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const if (debug) { - RCLCPP_ERROR(LOGGER, "tip links available:"); + RCLCPP_ERROR(getLogger(), "tip links available:"); std::copy(tip_frames_.begin(), tip_frames_.end(), std::ostream_iterator(std::cout, "\n")); } @@ -106,7 +113,8 @@ bool SrvKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const { if (!joint_model_group_->hasLinkModel(tip_frame)) { - RCLCPP_ERROR(LOGGER, "Could not find tip name '%s' in joint group '%s'", tip_frame.c_str(), group_name.c_str()); + RCLCPP_ERROR(getLogger(), "Could not find tip name '%s' in joint group '%s'", tip_frame.c_str(), + group_name.c_str()); return false; } ik_group_info_.link_names.push_back(tip_frame); @@ -117,22 +125,22 @@ bool SrvKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const robot_state_->setToDefaultValues(); // Create the ROS2 service client - RCLCPP_DEBUG(LOGGER, "IK Service client topic : %s", params_.kinematics_solver_service_name.c_str()); + RCLCPP_DEBUG(getLogger(), "IK Service client topic : %s", params_.kinematics_solver_service_name.c_str()); ik_service_client_ = node_->create_client(params_.kinematics_solver_service_name); if (!ik_service_client_->wait_for_service(std::chrono::seconds(1))) { // wait 0.1 seconds, blocking - RCLCPP_WARN_STREAM(LOGGER, + RCLCPP_WARN_STREAM(getLogger(), "Unable to connect to ROS service client with name: " << ik_service_client_->get_service_name()); } else { - RCLCPP_INFO_STREAM(LOGGER, + RCLCPP_INFO_STREAM(getLogger(), "Service client started with ROS service name: " << ik_service_client_->get_service_name()); } active_ = true; - RCLCPP_DEBUG(LOGGER, "ROS service-based kinematics solver initialized"); + RCLCPP_DEBUG(getLogger(), "ROS service-based kinematics solver initialized"); return true; } @@ -140,12 +148,12 @@ bool SrvKinematicsPlugin::setRedundantJoints(const std::vector& re { if (num_possible_redundant_joints_ < 0) { - RCLCPP_ERROR(LOGGER, "This group cannot have redundant joints"); + RCLCPP_ERROR(getLogger(), "This group cannot have redundant joints"); return false; } if (int(redundant_joints.size()) > num_possible_redundant_joints_) { - RCLCPP_ERROR(LOGGER, "This group can only have %d redundant joints", num_possible_redundant_joints_); + RCLCPP_ERROR(getLogger(), "This group can only have %d redundant joints", num_possible_redundant_joints_); return false; } @@ -247,7 +255,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vectorik_request.ik_link_name = getTipFrames()[0]; } - RCLCPP_DEBUG(LOGGER, "Calling service: %s", ik_service_client_->get_service_name()); + RCLCPP_DEBUG(getLogger(), "Calling service: %s", ik_service_client_->get_service_name()); auto result_future = ik_service_client_->async_send_request(ik_srv); const auto& response = result_future.get(); if (rclcpp::spin_until_future_complete(node_, result_future) == rclcpp::FutureReturnCode::SUCCESS) @@ -319,27 +327,28 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vectorget_service_name()); + RCLCPP_DEBUG_STREAM(getLogger(), + "Service call failed to connect to service: " << ik_service_client_->get_service_name()); error_code.val = error_code.FAILURE; return false; } // Convert the robot state message to our robot_state representation if (!moveit::core::robotStateMsgToRobotState(response->solution, *robot_state_)) { - RCLCPP_ERROR(LOGGER, "An error occurred converting received robot state message into internal robot state."); + RCLCPP_ERROR(getLogger(), "An error occurred converting received robot state message into internal robot state."); error_code.val = error_code.FAILURE; return false; } @@ -350,7 +359,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vector& link_nam { if (!active_) { - RCLCPP_ERROR(LOGGER, "kinematics not active"); + RCLCPP_ERROR(getLogger(), "kinematics not active"); return false; } poses.resize(link_names.size()); if (joint_angles.size() != dimension_) { - RCLCPP_ERROR(LOGGER, "Joint angles vector must have size: %d", dimension_); + RCLCPP_ERROR(getLogger(), "Joint angles vector must have size: %d", dimension_); return false; } - RCLCPP_ERROR(LOGGER, "Forward kinematics not implemented"); + RCLCPP_ERROR(getLogger(), "Forward kinematics not implemented"); return false; } diff --git a/moveit_kinematics/test/benchmark_ik.cpp b/moveit_kinematics/test/benchmark_ik.cpp index 324d89aebf..da3a0ca323 100644 --- a/moveit_kinematics/test/benchmark_ik.cpp +++ b/moveit_kinematics/test/benchmark_ik.cpp @@ -40,11 +40,10 @@ #include #include #include +#include namespace po = boost::program_options; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("cached_ik.measure_ik_call_cost"); - // Benchmark program measuring time to solve inverse kinematics of robot described in robot_description int main(int argc, char* argv[]) { @@ -78,6 +77,7 @@ int main(int argc, char* argv[]) rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("benchmark_ik"); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); + moveit::setNodeLoggerName(node->get_name()); // TODO(henningkayser): Load robot model from robot_description, fix kinematic param config // robot_model_loader::RobotModelLoader robot_model_loader(node); @@ -106,7 +106,8 @@ int main(int argc, char* argv[]) // skip group if there's no IK solver if (group->getSolverInstance() == nullptr) { - RCLCPP_WARN_STREAM(LOGGER, "No kinematic solver configured for group '" << group->getName() << "' - skipping"); + RCLCPP_WARN_STREAM(node->get_logger(), + "No kinematic solver configured for group '" << group->getName() << "' - skipping"); continue; } @@ -167,14 +168,14 @@ int main(int argc, char* argv[]) ++i; if (i % 100 == 0) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(node->get_logger(), "Avg. time per IK solver call is %g after %d calls. %g%% of calls failed to return a solution. " "%g%% of random joint configurations were ignored due to self-collisions.", ik_time.count() / static_cast(i), i, 100. * num_failed_calls / i, 100. * num_self_collisions / (num_self_collisions + i)); } } - RCLCPP_INFO(LOGGER, "Summary for group %s: %g %g %g", group->getName().c_str(), + RCLCPP_INFO(node->get_logger(), "Summary for group %s: %g %g %g", group->getName().c_str(), ik_time.count() / static_cast(i), 100. * num_failed_calls / i, 100. * num_self_collisions / (num_self_collisions + i)); } diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 677a2adaa7..cd1d1785bf 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -54,8 +54,12 @@ #include #include +#include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_kinematics_plugin"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("test_kinematics_plugin"); +} const std::string ROBOT_DESCRIPTION_PARAM = "robot_description"; const double DEFAULT_SEARCH_DISCRETIZATION = 0.01f; const double EXPECTED_SUCCESS_RATE = 0.8; @@ -97,7 +101,7 @@ class SharedData node_options.automatically_declare_parameters_from_overrides(true); node_ = rclcpp::Node::make_shared("moveit_kinematics_test", node_options); - RCLCPP_INFO_STREAM(LOGGER, "Loading robot model from " << node_->get_name() << '.' << ROBOT_DESCRIPTION_PARAM); + RCLCPP_INFO_STREAM(getLogger(), "Loading robot model from " << node_->get_name() << '.' << ROBOT_DESCRIPTION_PARAM); // load robot model rdf_loader::RDFLoader rdf_loader(node_, ROBOT_DESCRIPTION_PARAM); robot_model_ = std::make_shared(rdf_loader.getURDF(), rdf_loader.getSRDF()); @@ -183,7 +187,7 @@ class KinematicsTest : public ::testing::Test { *this = SharedData::instance(); - RCLCPP_INFO_STREAM(LOGGER, "Loading " << ik_plugin_name_); + RCLCPP_INFO_STREAM(getLogger(), "Loading " << ik_plugin_name_); kinematics_solver_ = SharedData::instance().createUniqueInstance(ik_plugin_name_); ASSERT_TRUE(bool(kinematics_solver_)) << "Failed to load plugin: " << ik_plugin_name_; @@ -386,7 +390,7 @@ TEST_F(KinematicsTest, randomWalkIK) if (!diff.isZero(1.05 * NEAR_JOINT)) { ++failures; - RCLCPP_WARN_STREAM(LOGGER, "jump in [" << i << "]: " << diff.transpose()); + RCLCPP_WARN_STREAM(getLogger(), "jump in [" << i << "]: " << diff.transpose()); } // update robot state to found pose @@ -446,14 +450,15 @@ TEST_F(KinematicsTest, unitIK) Eigen::Isometry3d initial, goal; tf2::fromMsg(poses[0], initial); - RCLCPP_DEBUG(LOGGER, "Initial: %f %f %f %f %f %f %f\n", poses[0].position.x, poses[0].position.y, poses[0].position.z, - poses[0].orientation.x, poses[0].orientation.y, poses[0].orientation.z, poses[0].orientation.w); + RCLCPP_DEBUG(getLogger(), "Initial: %f %f %f %f %f %f %f\n", poses[0].position.x, poses[0].position.y, + poses[0].position.z, poses[0].orientation.x, poses[0].orientation.y, poses[0].orientation.z, + poses[0].orientation.w); auto validate_ik = [&](const geometry_msgs::msg::Pose& goal, std::vector& truth) { // compute IK moveit_msgs::msg::MoveItErrorCodes error_code; - RCLCPP_DEBUG(LOGGER, "Goal %f %f %f %f %f %f %f\n", goal.position.x, goal.position.y, goal.position.z, + RCLCPP_DEBUG(getLogger(), "Goal %f %f %f %f %f %f %f\n", goal.position.x, goal.position.y, goal.position.z, goal.orientation.x, goal.orientation.y, goal.orientation.z, goal.orientation.w); kinematics_solver_->searchPositionIK(goal, seed_, timeout_, @@ -571,7 +576,7 @@ TEST_F(KinematicsTest, searchIK) if (num_ik_cb_tests_ > 0) { - RCLCPP_INFO_STREAM(LOGGER, "Success Rate: " << static_cast(success) / num_ik_tests_); + RCLCPP_INFO_STREAM(getLogger(), "Success Rate: " << static_cast(success) / num_ik_tests_); } EXPECT_GE(success, EXPECTED_SUCCESS_RATE * num_ik_tests_); } @@ -621,7 +626,7 @@ TEST_F(KinematicsTest, searchIKWithCallback) if (num_ik_cb_tests_ > 0) { - RCLCPP_INFO_STREAM(LOGGER, "Success Rate: " << static_cast(success) / num_ik_cb_tests_); + RCLCPP_INFO_STREAM(getLogger(), "Success Rate: " << static_cast(success) / num_ik_cb_tests_); } EXPECT_GE(success, EXPECTED_SUCCESS_RATE * num_ik_cb_tests_); } @@ -696,7 +701,7 @@ TEST_F(KinematicsTest, getIKMultipleSolutions) if (num_ik_cb_tests_ > 0) { - RCLCPP_INFO_STREAM(LOGGER, "Success Rate: " << static_cast(success) / num_ik_multiple_tests_); + RCLCPP_INFO_STREAM(getLogger(), "Success Rate: " << static_cast(success) / num_ik_multiple_tests_); } EXPECT_GE(success, EXPECTED_SUCCESS_RATE * num_ik_multiple_tests_); } diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp index 4ae65b8259..4592138113 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp @@ -35,10 +35,17 @@ /* Author: E. Gil Jones */ #include +#include namespace chomp_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_optimizer"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("chomp_optimizer"); +} +} // namespace CHOMPInterface::CHOMPInterface(const rclcpp::Node::SharedPtr& node) : ChompPlanner(), node_(node) { @@ -71,7 +78,7 @@ void CHOMPInterface::loadParams() if (node_->get_parameter("chomp.trajectory_initialization_method", method) && !params_.setTrajectoryInitializationMethod(method)) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Attempted to set trajectory_initialization_method to invalid value '%s'. Using default '%s' " "instead.", method.c_str(), params_.trajectory_initialization_method_.c_str()); diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp index 9ba86bca42..0bc97f92b2 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp @@ -37,13 +37,20 @@ #include #include #include +#include #include #include namespace chomp_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_optimizer"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("chomp_optimizer"); +} +} // namespace class CHOMPPlannerManager : public planning_interface::PlannerManager { @@ -71,14 +78,14 @@ class CHOMPPlannerManager : public planning_interface::PlannerManager if (req.group_name.empty()) { - RCLCPP_ERROR(LOGGER, "No group specified to plan for"); + RCLCPP_ERROR(getLogger(), "No group specified to plan for"); error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; return planning_interface::PlanningContextPtr(); } if (!planning_scene) { - RCLCPP_ERROR(LOGGER, "No planning scene supplied as input"); + RCLCPP_ERROR(getLogger(), "No planning scene supplied as input"); error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return planning_interface::PlanningContextPtr(); } diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index ce04f3f2ec..4e8a4d9e5a 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -49,7 +50,13 @@ namespace chomp { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_optimizer"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("chomp_optimizer"); +} +} // namespace ChompOptimizer::ChompOptimizer(ChompTrajectory* trajectory, const planning_scene::PlanningSceneConstPtr& planning_scene, const std::string& planning_group, const ChompParameters* parameters, @@ -64,13 +71,13 @@ ChompOptimizer::ChompOptimizer(ChompTrajectory* trajectory, const planning_scene , start_state_(start_state) , initialized_(false) { - RCLCPP_INFO(LOGGER, "Active collision detector is: %s", planning_scene->getCollisionDetectorName().c_str()); + RCLCPP_INFO(getLogger(), "Active collision detector is: %s", planning_scene->getCollisionDetectorName().c_str()); hy_env_ = dynamic_cast( planning_scene->getCollisionEnv(planning_scene->getCollisionDetectorName()).get()); if (!hy_env_) { - RCLCPP_WARN(LOGGER, "Could not initialize hybrid collision world from planning scene"); + RCLCPP_WARN(getLogger(), "Could not initialize hybrid collision world from planning scene"); return; } @@ -93,7 +100,7 @@ void ChompOptimizer::initialize() const auto wt = std::chrono::system_clock::now(); hy_env_->getCollisionGradients(req, res, state_, &planning_scene_->getAllowedCollisionMatrix(), gsr_); - RCLCPP_INFO(LOGGER, "First coll check took %f sec", + RCLCPP_INFO(getLogger(), "First coll check took %f sec", std::chrono::duration(std::chrono::system_clock::now() - wt).count()); num_collision_points_ = 0; for (const collision_detection::GradientInfo& gradient : gsr_->gradients_) @@ -180,7 +187,7 @@ void ChompOptimizer::initialize() for (int i = 0; i < num_joints_; ++i) { joint_names_.push_back(joint_model_group_->getActiveJointModels()[i]->getName()); - // RCLCPP_INFO(LOGGER,"Got joint %s", joint_names_[i].c_str()); + // RCLCPP_INFO(getLogger(),"Got joint %s", joint_names_[i].c_str()); registerParents(joint_model_group_->getActiveJointModels()[i]); fixed_link_resolution_map[joint_names_[i]] = joint_names_[i]; } @@ -238,7 +245,7 @@ void ChompOptimizer::initialize() } else { - RCLCPP_ERROR(LOGGER, "Couldn't find joint %s!", info.joint_name.c_str()); + RCLCPP_ERROR(getLogger(), "Couldn't find joint %s!", info.joint_name.c_str()); } j++; } @@ -266,12 +273,12 @@ void ChompOptimizer::registerParents(const moveit::core::JointModel* model) { if (model->getParentLinkModel() == nullptr) { - RCLCPP_ERROR(LOGGER, "Model %s not root but has nullptr link model parent", model->getName().c_str()); + RCLCPP_ERROR(getLogger(), "Model %s not root but has nullptr link model parent", model->getName().c_str()); return; } else if (model->getParentLinkModel()->getParentJointModel() == nullptr) { - RCLCPP_ERROR(LOGGER, "Model %s not root but has nullptr joint model parent", model->getName().c_str()); + RCLCPP_ERROR(getLogger(), "Model %s not root but has nullptr joint model parent", model->getName().c_str()); return; } parent_model = model->getParentLinkModel()->getParentJointModel(); @@ -311,7 +318,7 @@ bool ChompOptimizer::optimize() double s_cost = getSmoothnessCost(); double cost = c_cost + s_cost; - RCLCPP_DEBUG(LOGGER, "Collision cost %f, smoothness cost: %f", c_cost, s_cost); + RCLCPP_DEBUG(getLogger(), "Collision cost %f, smoothness cost: %f", c_cost, s_cost); /// TODO: HMC BASED COMMENTED CODE BELOW, Need to uncomment and perform extensive testing by varying the HMC /// parameters values in the chomp_planning.yaml file so that CHOMP can find optimal paths @@ -371,11 +378,11 @@ bool ChompOptimizer::optimize() if (iteration_ % 10 == 0) { - RCLCPP_DEBUG(LOGGER, "iteration: %d", iteration_); + RCLCPP_DEBUG(getLogger(), "iteration: %d", iteration_); if (isCurrentTrajectoryMeshToMeshCollisionFree()) { num_collision_free_iterations_ = 0; - RCLCPP_INFO(LOGGER, "Chomp Got mesh to mesh safety at iter %d. Breaking out early.", iteration_); + RCLCPP_INFO(getLogger(), "Chomp Got mesh to mesh safety at iter %d. Breaking out early.", iteration_); is_collision_free_ = true; iteration_++; should_break_out = true; @@ -390,13 +397,13 @@ bool ChompOptimizer::optimize() // if(safety == CollisionProximitySpace::MeshToMeshSafe) // { // num_collision_free_iterations_ = 0; - // RCLCPP_INFO(LOGGER,"Chomp Got mesh to mesh safety at iter %d. Breaking out early.", iteration_); + // RCLCPP_INFO(getLogger(),"Chomp Got mesh to mesh safety at iter %d. Breaking out early.", iteration_); // is_collision_free_ = true; // iteration_++; // should_break_out = true; // } else if(safety == CollisionProximitySpace::InCollisionSafe) { // num_collision_free_iterations_ = parameters_->getMaxIterationsAfterCollisionFree(); - // RCLCPP_INFO(LOGGER,"Chomp Got in collision safety at iter %d. Breaking out soon.", iteration_); + // RCLCPP_INFO(getLogger(),"Chomp Got in collision safety at iter %d. Breaking out soon.", iteration_); // is_collision_free_ = true; // iteration_++; // should_break_out = true; @@ -418,14 +425,14 @@ bool ChompOptimizer::optimize() } else { - RCLCPP_DEBUG(LOGGER, "cCost %f over threshold %f", c_cost, parameters_->collision_threshold_); + RCLCPP_DEBUG(getLogger(), "cCost %f over threshold %f", c_cost, parameters_->collision_threshold_); } } if (std::chrono::duration(std::chrono::system_clock::now() - start_time).count() > parameters_->planning_time_limit_) { - RCLCPP_WARN(LOGGER, "Breaking out early due to time limit constraints."); + RCLCPP_WARN(getLogger(), "Breaking out early due to time limit constraints."); break; } @@ -435,7 +442,7 @@ bool ChompOptimizer::optimize() // if(fabs(averageCostVelocity) < minimaThreshold && currentCostIter == -1 && !is_collision_free_ && // parameters_->getAddRandomness()) // { - // RCLCPP_INFO(LOGGER,"Detected local minima. Attempting to break out!"); + // RCLCPP_INFO(getLogger(),"Detected local minima. Attempting to break out!"); // int iter = 0; // bool success = false; // while(iter < 20 && !success) @@ -451,7 +458,7 @@ bool ChompOptimizer::optimize() // iter ++; // if(new_cost < original_cost) // { - // RCLCPP_INFO(LOGGER,"Got out of minimum in %d iters!", iter); + // RCLCPP_INFO(getLogger(),"Got out of minimum in %d iters!", iter); // averageCostVelocity = 0.0; // currentCostIter = 0; // success = true; @@ -469,7 +476,7 @@ bool ChompOptimizer::optimize() // if(!success) // { - // RCLCPP_INFO(LOGGER,"Failed to exit minimum!"); + // RCLCPP_INFO(getLogger(),"Failed to exit minimum!"); // } //} // else if (currentCostIter == -1) @@ -500,22 +507,22 @@ bool ChompOptimizer::optimize() if (is_collision_free_) { optimization_result = true; - RCLCPP_INFO(LOGGER, "Chomp path is collision free"); + RCLCPP_INFO(getLogger(), "Chomp path is collision free"); } else { optimization_result = false; - RCLCPP_ERROR(LOGGER, "Chomp path is not collision free!"); + RCLCPP_ERROR(getLogger(), "Chomp path is not collision free!"); } group_trajectory_.getTrajectory() = best_group_trajectory_; updateFullTrajectory(); - RCLCPP_INFO(LOGGER, "Terminated after %d iterations, using path from iteration %d", iteration_, + RCLCPP_INFO(getLogger(), "Terminated after %d iterations, using path from iteration %d", iteration_, last_improvement_iteration_); - RCLCPP_INFO(LOGGER, "Optimization core finished in %f sec", + RCLCPP_INFO(getLogger(), "Optimization core finished in %f sec", std::chrono::duration(std::chrono::system_clock::now() - start_time).count()); - RCLCPP_INFO(LOGGER, "Time per iteration %f sec", + RCLCPP_INFO(getLogger(), "Time per iteration %f sec", std::chrono::duration(std::chrono::system_clock::now() - start_time).count() / (iteration_ * 1.0)); return optimization_result; @@ -540,40 +547,6 @@ bool ChompOptimizer::isCurrentTrajectoryMeshToMeshCollisionFree() const return planning_scene_->isPathValid(start_state_msg, traj, planning_group_); } -/// TODO: HMC BASED COMMENTED CODE BELOW, Need to uncomment and perform extensive testing by varying the HMC parameters -/// values in the chomp_planning.yaml file so that CHOMP can find optimal paths - -// CollisionProximitySpace::TrajectorySafety ChompOptimizer::checkCurrentIterValidity() -// { -// JointTrajectory jointTrajectory; -// jointTrajectory.joint_names = joint_names_; -// jointTrajectory.header.frame_id = collision_space_->getCollisionModelsInterface()->getRobotFrameId(); -// jointTrajectory.header.stamp = ros::Time::now(); -// Constraints goalConstraints; -// Constraints pathConstraints; -// ArmNavigationErrorCodes errorCode; -// vector trajectoryErrorCodes; -// for(int i = 0; i < group_trajectory_.getNumPoints(); ++i) -// { -// JointTrajectoryPoint point; -// for(int j = 0; j < group_trajectory_.getNumJoints(); ++j) -// { -// point.positions.push_back(best_group_trajectory_(i, j)); -// } -// jointTrajectory.points.push_back(point); -// } - -// return collision_space_->isTrajectorySafe(jointTrajectory, goalConstraints, pathConstraints, planning_group_); -// /* -// bool valid = collision_space_->getCollisionModelsInterface()->isJointTrajectoryValid(*state_, -// jointTrajectory, -// goalConstraints, -// pathConstraints, errorCode, -// trajectoryErrorCodes, false); -// */ - -// } - void ChompOptimizer::calculateSmoothnessIncrements() { for (int i = 0; i < num_joints_; ++i) @@ -952,16 +925,6 @@ void ChompOptimizer::performForwardKinematics() if (point_is_in_collision_[i][j]) { state_is_in_collision_[i] = true; - // if(is_collision_free_ == true) { - // RCLCPP_INFO(LOGGER,"We know it's not collision free " << g); - // RCLCPP_INFO(LOGGER,"Sphere location " << info.sphere_locations[k].x() << " " << - // info.sphere_locations[k].y() << " " << info.sphere_locations[k].z()); - // RCLCPP_INFO(LOGGER,"Gradient " << info.gradients[k].x() << " " << info.gradients[k].y() << " " << - // info.gradients[k].z() << " distance " << info.distances[k] << " radii " << info.sphere_radii[k]); - // RCLCPP_INFO(LOGGER,"Radius " << info.sphere_radii[k] << " potential " << - // collision_point_potential_[i][j]); - // } - is_collision_free_ = false; } j++; @@ -970,8 +933,6 @@ void ChompOptimizer::performForwardKinematics() } } - // RCLCPP_INFO(LOGGER,"Total dur " << total_dur << " total checks " << end-start+1); - // now, get the vel and acc for each collision point (using finite differencing) for (int i = free_vars_start_; i <= free_vars_end_; ++i) { @@ -1039,84 +1000,4 @@ void ChompOptimizer::perturbTrajectory() } } -/// TODO: HMC BASED COMMENTED CODE BELOW, Need to uncomment and perform extensive testing by varying the HMC parameters -/// values in the chomp_planning.yaml file so that CHOMP can find optimal paths -// void ChompOptimizer::getRandomState(const RobotState currentState, const string& groupName, Eigen::VectorXd& -// state_vec) -// { -// const vector& jointStates = -// currentState->getJointStateGroup(groupName)->getJointStateVector(); -// for(size_t i = 0; i < jointStates.size(); ++i) -// { - -// bool continuous = false; - -// RobotState *::JointState* jointState = jointStates[i]; -// const RevoluteJointModel* revolute_joint -// = dynamic_cast(jointState->getJointModel()); -// if(revolute_joint && revolute_joint->continuous_) { -// continuous = true; -// } - -// map > bounds = jointState->getJointModel()->getAllVariableBounds(); -// int j = 0; -// for(map >::iterator it = bounds.begin(); it != bounds.end(); ++it) -// { -// double randVal = jointState->getJointStateValues()[j] + (rsl::uniform_real(0., 1.) -// * (parameters_->getRandomJumpAmount()) - -// rsl::uniform_real(0., 1.) * -// (parameters_->getRandomJumpAmount())); - -// if(!continuous) -// { -// if(randVal > it->second.second) -// { -// randVal = it->second.second; -// } -// else if(randVal < it->second.first) -// { -// randVal = it->second.first; -// } -// } - -// ROS_DEBUG("Joint " << it->first << " old value " << jointState->getJointStateValues()[j] << " new value -// " << randVal); -// state_vec(i) = randVal; - -// j++; -// } -// } -// } - -/* -void ChompOptimizer::getRandomMomentum() -{ - if (is_collision_free_) - random_momentum_.setZero(num_vars_free_, num_joints_); - else - for (int i = 0; i < num_joints_; ++i) - { - multivariate_gaussian_[i].sample(random_joint_momentum_); - random_momentum_.col(i) = stochasticity_factor_ * random_joint_momentum_; - } -} - -void ChompOptimizer::updateMomentum() -{ - // double alpha = 1.0 - parameters_->getHmcStochasticity(); - double eps = parameters_->getHmcDiscretization(); - if (iteration_ > 0) - momentum_ = (momentum_ + eps * final_increments_); - else - momentum_ = random_momentum_; - // momentum_ = alpha * (momentum_ + eps*final_increments_) + sqrt(1.0-alpha*alpha)*random_momentum_; -} - -void ChompOptimizer::updatePositionFromMomentum() -{ - double eps = parameters_->getHmcDiscretization(); - group_trajectory_.getFreeTrajectoryBlock() += eps * momentum_; -} -*/ - } // namespace chomp diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp index 75e60ba0e6..34c603054e 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp @@ -41,12 +41,19 @@ #include #include #include +#include #include namespace chomp { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("chomp_planner"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("chomp_planner"); +} +} // namespace void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const planning_interface::MotionPlanRequest& req, const ChompParameters& params, @@ -56,7 +63,7 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s res.planner_id = std::string("chomp"); if (!planning_scene) { - RCLCPP_ERROR(LOGGER, "No planning scene initialized."); + RCLCPP_ERROR(getLogger(), "No planning scene initialized."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return; } @@ -67,7 +74,7 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s if (!start_state.satisfiesBounds()) { - RCLCPP_ERROR(LOGGER, "Start state violates joint limits"); + RCLCPP_ERROR(getLogger(), "Start state violates joint limits"); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE; return; } @@ -77,7 +84,7 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s if (req.goal_constraints.size() != 1) { - RCLCPP_ERROR(LOGGER, "Expecting exactly one goal constraint, got: %zd", req.goal_constraints.size()); + RCLCPP_ERROR(getLogger(), "Expecting exactly one goal constraint, got: %zd", req.goal_constraints.size()); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; return; } @@ -85,7 +92,7 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s if (req.goal_constraints[0].joint_constraints.empty() || !req.goal_constraints[0].position_constraints.empty() || !req.goal_constraints[0].orientation_constraints.empty()) { - RCLCPP_ERROR(LOGGER, "Only joint-space goals are supported"); + RCLCPP_ERROR(getLogger(), "Only joint-space goals are supported"); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; return; } @@ -96,7 +103,7 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s goal_state.setVariablePosition(joint_constraint.joint_name, joint_constraint.position); if (!goal_state.satisfiesBounds()) { - RCLCPP_ERROR(LOGGER, "Goal state violates joint limits"); + RCLCPP_ERROR(getLogger(), "Goal state violates joint limits"); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE; return; } @@ -117,7 +124,7 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s { double start = (trajectory)(0, i); double end = (trajectory)(goal_index, i); - RCLCPP_INFO(LOGGER, "Start is %f end %f short %f", start, end, shortestAngularDistance(start, end)); + RCLCPP_INFO(getLogger(), "Start is %f end %f short %f", start, end, shortestAngularDistance(start, end)); (trajectory)(goal_index, i) = start + shortestAngularDistance(start, end); } } @@ -140,23 +147,23 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s { if (res.trajectory.empty()) { - RCLCPP_ERROR(LOGGER, "No input trajectory specified"); + RCLCPP_ERROR(getLogger(), "No input trajectory specified"); return; } else if (!(trajectory.fillInFromTrajectory(*res.trajectory[0]))) { - RCLCPP_ERROR(LOGGER, "Input trajectory has less than 2 points, " - "trajectory must contain at least start and goal state"); + RCLCPP_ERROR(getLogger(), "Input trajectory has less than 2 points, " + "trajectory must contain at least start and goal state"); return; } } else { - RCLCPP_ERROR(LOGGER, "invalid interpolation method specified in the chomp_planner file"); + RCLCPP_ERROR(getLogger(), "invalid interpolation method specified in the chomp_planner file"); return; } - RCLCPP_INFO(LOGGER, "CHOMP trajectory initialized using method: %s ", + RCLCPP_INFO(getLogger(), "CHOMP trajectory initialized using method: %s ", (params.trajectory_initialization_method_).c_str()); // optimize! @@ -195,12 +202,12 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s std::make_unique(&trajectory, planning_scene, req.group_name, ¶ms_nonconst, start_state); if (!optimizer->isInitialized()) { - RCLCPP_ERROR(LOGGER, "Could not initialize optimizer"); + RCLCPP_ERROR(getLogger(), "Could not initialize optimizer"); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return; } - RCLCPP_DEBUG(LOGGER, "Optimization took %ld sec to create", + RCLCPP_DEBUG(getLogger(), "Optimization took %ld sec to create", (std::chrono::system_clock::now() - create_time).count()); bool optimization_result = optimizer->optimize(); @@ -208,11 +215,11 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s // replan with updated parameters if no solution is found if (params_nonconst.enable_failure_recovery_) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Planned with Chomp Parameters (learning_rate, ridge_factor, " "planning_time_limit, max_iterations), attempt: # %d ", (replan_count + 1)); - RCLCPP_INFO(LOGGER, "Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ", + RCLCPP_INFO(getLogger(), "Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ", params_nonconst.learning_rate_, params_nonconst.ridge_factor_, params_nonconst.planning_time_limit_, params_nonconst.max_iterations_); @@ -233,12 +240,12 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s // resetting the CHOMP Parameters to the original values after a successful plan params_nonconst.setRecoveryParams(org_learning_rate, org_ridge_factor, org_planning_time_limit, org_max_iterations); - RCLCPP_DEBUG(LOGGER, "Optimization actually took %ld sec to run", + RCLCPP_DEBUG(getLogger(), "Optimization actually took %ld sec to run", (std::chrono::system_clock::now() - create_time).count()); create_time = std::chrono::system_clock::now(); // assume that the trajectory is now optimized, fill in the output structure: - RCLCPP_DEBUG(LOGGER, "Output trajectory has %zd joints", trajectory.getNumJoints()); + RCLCPP_DEBUG(getLogger(), "Output trajectory has %zd joints", trajectory.getNumJoints()); auto result = std::make_shared(planning_scene->getRobotModel(), req.group_name); // fill in the entire trajectory @@ -260,8 +267,8 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s res.description.resize(1); res.description[0] = "plan"; - RCLCPP_DEBUG(LOGGER, "Bottom took %ld sec to create", (std::chrono::system_clock::now() - create_time).count()); - RCLCPP_DEBUG(LOGGER, "Serviced planning request in %ld wall-seconds", + RCLCPP_DEBUG(getLogger(), "Bottom took %ld sec to create", (std::chrono::system_clock::now() - create_time).count()); + RCLCPP_DEBUG(getLogger(), "Serviced planning request in %ld wall-seconds", (std::chrono::system_clock::now() - start_time).count()); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; @@ -271,7 +278,7 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s // report planning failure if path has collisions if (not optimizer->isCollisionFree()) { - RCLCPP_ERROR(LOGGER, "Motion plan is invalid."); + RCLCPP_ERROR(getLogger(), "Motion plan is invalid."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return; } @@ -283,7 +290,7 @@ void ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s { if (!jc.configure(constraint) || !jc.decide(last_state).satisfied) { - RCLCPP_ERROR(LOGGER, "Goal constraints are violated: %s", constraint.joint_name.c_str()); + RCLCPP_ERROR(getLogger(), "Goal constraints are violated: %s", constraint.joint_name.c_str()); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED; return; } diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp index bf4376da49..4d226366dd 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp @@ -37,17 +37,23 @@ #include #include #include +#include #include namespace ompl_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.constrained_goal_sampler"); -} // namespace ompl_interface +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ompl_constrained_goal_sampler"); +} +} // namespace -ompl_interface::ConstrainedGoalSampler::ConstrainedGoalSampler(const ModelBasedPlanningContext* pc, - kinematic_constraints::KinematicConstraintSetPtr ks, - constraint_samplers::ConstraintSamplerPtr cs) +ConstrainedGoalSampler::ConstrainedGoalSampler(const ModelBasedPlanningContext* pc, + kinematic_constraints::KinematicConstraintSetPtr ks, + constraint_samplers::ConstraintSamplerPtr cs) : ob::GoalLazySamples( pc->getOMPLSimpleSetup()->getSpaceInformation(), [this](const GoalLazySamples* gls, ompl::base::State* state) { @@ -64,22 +70,20 @@ ompl_interface::ConstrainedGoalSampler::ConstrainedGoalSampler(const ModelBasedP { if (!constraint_sampler_) default_sampler_ = si_->allocStateSampler(); - RCLCPP_DEBUG(LOGGER, "Constructed a ConstrainedGoalSampler instance at address %p", this); + RCLCPP_DEBUG(getLogger(), "Constructed a ConstrainedGoalSampler instance at address %p", this); startSampling(); } -bool ompl_interface::ConstrainedGoalSampler::checkStateValidity(ob::State* new_goal, - const moveit::core::RobotState& state, - bool verbose) const +bool ConstrainedGoalSampler::checkStateValidity(ob::State* new_goal, const moveit::core::RobotState& state, + bool verbose) const { planning_context_->getOMPLStateSpace()->copyToOMPLState(new_goal, state); return static_cast(si_->getStateValidityChecker().get())->isValid(new_goal, verbose); } -bool ompl_interface::ConstrainedGoalSampler::stateValidityCallback(ob::State* new_goal, - const moveit::core::RobotState* state, - const moveit::core::JointModelGroup* jmg, - const double* jpos, bool verbose) const +bool ConstrainedGoalSampler::stateValidityCallback(ob::State* new_goal, const moveit::core::RobotState* state, + const moveit::core::JointModelGroup* jmg, const double* jpos, + bool verbose) const { // we copy the state to not change the seed state moveit::core::RobotState solution_state(*state); @@ -88,8 +92,7 @@ bool ompl_interface::ConstrainedGoalSampler::stateValidityCallback(ob::State* ne return checkStateValidity(new_goal, solution_state, verbose); } -bool ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(const ob::GoalLazySamples* gls, - ob::State* new_goal) +bool ConstrainedGoalSampler::sampleUsingConstraintSampler(const ob::GoalLazySamples* gls, ob::State* new_goal) { unsigned int max_attempts = planning_context_->getMaximumGoalSamplingAttempts(); unsigned int attempts_so_far = gls->samplingAttemptsCount(); @@ -144,9 +147,9 @@ bool ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(const if (!warned_invalid_samples_ && invalid_sampled_constraints_ >= (attempts_so_far * 8) / 10) { warned_invalid_samples_ = true; - RCLCPP_WARN(LOGGER, "More than 80%% of the sampled goal states " - "fail to satisfy the constraints imposed on the goal sampler. " - "Is the constrained sampler working correctly?"); + RCLCPP_WARN(getLogger(), "More than 80%% of the sampled goal states " + "fail to satisfy the constraints imposed on the goal sampler. " + "Is the constrained sampler working correctly?"); } } } @@ -164,3 +167,5 @@ bool ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(const } return false; } + +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 712d918f4d..20c1d97d7a 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -39,16 +39,20 @@ #include #include #include +#include #include #include namespace ompl_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.constraints_library"); - namespace { +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ompl_constraints_library"); +} + template void msgToHex(const T& msg, std::string& hex) { @@ -59,7 +63,7 @@ void msgToHex(const T& msg, std::string& hex) if (result != RMW_RET_OK) { // TODO(henningkayser): handle error - RCLCPP_ERROR(LOGGER, "Failed to serialize message!"); + RCLCPP_ERROR(getLogger(), "Failed to serialize message!"); return; } const size_t serial_size_arg = serialized_msg.buffer_length; @@ -81,7 +85,7 @@ void hexToMsg(const std::string& hex, T& msg) if (rcutils_result != RCUTILS_RET_OK) { // TODO(henningkayser): handle error - RCLCPP_ERROR(LOGGER, "Failed to allocate message buffer!"); + RCLCPP_ERROR(getLogger(), "Failed to allocate message buffer!"); return; } @@ -95,7 +99,7 @@ void hexToMsg(const std::string& hex, T& msg) if (result != RMW_RET_OK) { // TODO(henningkayser): handle error - RCLCPP_ERROR(LOGGER, "Failed to deserialize message!"); + RCLCPP_ERROR(getLogger(), "Failed to deserialize message!"); return; } } @@ -213,7 +217,7 @@ bool interpolateUsingStoredStates(const ConstraintApproximationStateStorage* sta return true; } -ompl_interface::InterpolationFunction ompl_interface::ConstraintApproximation::getInterpolationFunction() const +InterpolationFunction ConstraintApproximation::getInterpolationFunction() const { if (explicit_motions_ && milestones_ > 0 && milestones_ < state_storage_->size()) { @@ -241,12 +245,11 @@ allocConstraintApproximationStateSampler(const ob::StateSpace* space, const std: return std::make_shared(space, state_storage, milestones); } } -} // namespace ompl_interface -ompl_interface::ConstraintApproximation::ConstraintApproximation( - std::string group, std::string state_space_parameterization, bool explicit_motions, - moveit_msgs::msg::Constraints msg, std::string filename, ompl::base::StateStoragePtr storage, - std::size_t milestones) +ConstraintApproximation::ConstraintApproximation(std::string group, std::string state_space_parameterization, + bool explicit_motions, moveit_msgs::msg::Constraints msg, + std::string filename, ompl::base::StateStoragePtr storage, + std::size_t milestones) : group_(std::move(group)) , state_space_parameterization_(std::move(state_space_parameterization)) , explicit_motions_(explicit_motions) @@ -262,7 +265,7 @@ ompl_interface::ConstraintApproximation::ConstraintApproximation( } ompl::base::StateSamplerAllocator -ompl_interface::ConstraintApproximation::getStateSamplerAllocator(const moveit_msgs::msg::Constraints& /*unused*/) const +ConstraintApproximation::getStateSamplerAllocator(const moveit_msgs::msg::Constraints& /*unused*/) const { if (state_storage_->size() == 0) return ompl::base::StateSamplerAllocator(); @@ -270,63 +273,21 @@ ompl_interface::ConstraintApproximation::getStateSamplerAllocator(const moveit_m return allocConstraintApproximationStateSampler(ss, space_signature_, state_storage_, milestones_); }; } -/* -void ompl_interface::ConstraintApproximation::visualizeDistribution(const -std::string &link_name, unsigned int count, -visualization_msgs::MarkerArray &arr) const -{ - moveit::core::RobotState robot_state(robot_model_); - robot_state.setToDefaultValues(); - - ompl::RNG rng; - std_msgs::ColorRGBA color; - color.r = 0.0f; - color.g = 1.0f; - color.b = 1.0f; - color.a = 1.0f; - if (state_storage_->size() < count) - count = state_storage_->size(); - - for (std::size_t i = 0 ; i < count ; ++i) - { - state_storage_->getStateSpace()->as()->copyToRobotState(robot_state, -state_storage_->getState(rng.uniformInt(0, state_storage_->size() - 1))); - const Eigen::Vector3d &pos = -robot_state.getLinkState(link_name)->getGlobalLinkTransform().translation(); - - visualization_msgs::Marker mk; - mk.header.stamp = ros::Time::now(); - mk.header.frame_id = robot_model_->getModelFrame(); - mk.ns = "stored_constraint_data"; - mk.id = i; - mk.type = visualization_msgs::Marker::SPHERE; - mk.action = visualization_msgs::Marker::ADD; - mk.pose.position.x = pos.x(); - mk.pose.position.y = pos.y(); - mk.pose.position.z = pos.z(); - mk.pose.orientation.w = 1.0; - mk.scale.x = mk.scale.y = mk.scale.z = 0.035; - mk.color = color; - mk.lifetime = ros::Duration(30.0); - arr.markers.push_back(mk); - } - } -*/ -void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std::string& path) +void ConstraintsLibrary::loadConstraintApproximations(const std::string& path) { constraint_approximations_.clear(); std::ifstream fin((path + "/manifest").c_str()); if (!fin.good()) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Manifest not found in folder '%s'. Not loading " "constraint approximations.", path.c_str()); return; } - RCLCPP_INFO(LOGGER, "Loading constrained space approximations from '%s'...", path.c_str()); + RCLCPP_INFO(getLogger(), "Loading constrained space approximations from '%s'...", path.c_str()); while (fin.good() && !fin.eof()) { @@ -353,12 +314,12 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: if (context_->getGroupName() != group && context_->getOMPLStateSpace()->getParameterizationType() != state_space_parameterization) { - RCLCPP_INFO(LOGGER, "Ignoring constraint approximation of type '%s' for group '%s' from '%s'...", + RCLCPP_INFO(getLogger(), "Ignoring constraint approximation of type '%s' for group '%s' from '%s'...", state_space_parameterization.c_str(), group.c_str(), filename.c_str()); continue; } - RCLCPP_INFO(LOGGER, "Loading constraint approximation of type '%s' for group '%s' from '%s'...", + RCLCPP_INFO(getLogger(), "Loading constraint approximation of type '%s' for group '%s' from '%s'...", state_space_parameterization.c_str(), group.c_str(), filename.c_str()); moveit_msgs::msg::Constraints msg; hexToMsg(serialization, msg); @@ -367,24 +328,24 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std: auto cap = std::make_shared(group, state_space_parameterization, explicit_motions, msg, filename, ompl::base::StateStoragePtr(cass), milestones); if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end()) - RCLCPP_WARN(LOGGER, "Overwriting constraint approximation named '%s'", cap->getName().c_str()); + RCLCPP_WARN(getLogger(), "Overwriting constraint approximation named '%s'", cap->getName().c_str()); constraint_approximations_[cap->getName()] = cap; std::size_t sum = 0; for (std::size_t i = 0; i < cass->size(); ++i) sum += cass->getMetadata(i).first.size(); - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Loaded %lu states (%lu milestones) and %lu connections (%0.1lf per state) " "for constraint named '%s'%s", cass->size(), cap->getMilestoneCount(), sum, static_cast(sum) / static_cast(cap->getMilestoneCount()), msg.name.c_str(), explicit_motions ? ". Explicit motions included." : ""); } - RCLCPP_INFO(LOGGER, "Done loading constrained space approximations."); + RCLCPP_INFO(getLogger(), "Done loading constrained space approximations."); } -void ompl_interface::ConstraintsLibrary::saveConstraintApproximations(const std::string& path) +void ConstraintsLibrary::saveConstraintApproximations(const std::string& path) { - RCLCPP_INFO(LOGGER, "Saving %u constrained space approximations to '%s'", + RCLCPP_INFO(getLogger(), "Saving %u constrained space approximations to '%s'", static_cast(constraint_approximations_.size()), path.c_str()); try { @@ -414,17 +375,17 @@ void ompl_interface::ConstraintsLibrary::saveConstraintApproximations(const std: } else { - RCLCPP_ERROR(LOGGER, "Unable to save constraint approximation to '%s'", path.c_str()); + RCLCPP_ERROR(getLogger(), "Unable to save constraint approximation to '%s'", path.c_str()); } fout.close(); } -void ompl_interface::ConstraintsLibrary::clearConstraintApproximations() +void ConstraintsLibrary::clearConstraintApproximations() { constraint_approximations_.clear(); } -void ompl_interface::ConstraintsLibrary::printConstraintApproximations(std::ostream& out) const +void ConstraintsLibrary::printConstraintApproximations(std::ostream& out) const { for (const std::pair& constraint_approximation : constraint_approximations_) @@ -439,8 +400,8 @@ void ompl_interface::ConstraintsLibrary::printConstraintApproximations(std::ostr } } -const ompl_interface::ConstraintApproximationPtr& -ompl_interface::ConstraintsLibrary::getConstraintApproximation(const moveit_msgs::msg::Constraints& msg) const +const ConstraintApproximationPtr& +ConstraintsLibrary::getConstraintApproximation(const moveit_msgs::msg::Constraints& msg) const { auto it = constraint_approximations_.find(msg.name); if (it != constraint_approximations_.end()) @@ -450,27 +411,24 @@ ompl_interface::ConstraintsLibrary::getConstraintApproximation(const moveit_msgs return empty; } -ompl_interface::ConstraintApproximationConstructionResults -ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::msg::Constraints& constr, - const std::string& group, - const planning_scene::PlanningSceneConstPtr& scene, - const ConstraintApproximationConstructionOptions& options) +ConstraintApproximationConstructionResults +ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::msg::Constraints& constr, const std::string& group, + const planning_scene::PlanningSceneConstPtr& scene, + const ConstraintApproximationConstructionOptions& options) { return addConstraintApproximation(constr, constr, group, scene, options); } -ompl_interface::ConstraintApproximationConstructionResults -ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::msg::Constraints& constr_sampling, - const moveit_msgs::msg::Constraints& constr_hard, - const std::string& group, - const planning_scene::PlanningSceneConstPtr& scene, - const ConstraintApproximationConstructionOptions& options) +ConstraintApproximationConstructionResults ConstraintsLibrary::addConstraintApproximation( + const moveit_msgs::msg::Constraints& constr_sampling, const moveit_msgs::msg::Constraints& constr_hard, + const std::string& group, const planning_scene::PlanningSceneConstPtr& scene, + const ConstraintApproximationConstructionOptions& options) { ConstraintApproximationConstructionResults res; if (context_->getGroupName() != group && context_->getOMPLStateSpace()->getParameterizationType() != options.state_space_parameterization) { - RCLCPP_INFO(LOGGER, "Ignoring constraint approximation of type '%s' for group '%s'...", + RCLCPP_INFO(getLogger(), "Ignoring constraint approximation of type '%s' for group '%s'...", options.state_space_parameterization.c_str(), group.c_str()); return res; } @@ -483,7 +441,7 @@ ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs auto start = clock.now(); ompl::base::StateStoragePtr state_storage = constructConstraintApproximation(context_, constr_sampling, constr_hard, options, res); - RCLCPP_INFO(LOGGER, "Spent %lf seconds constructing the database", (clock.now() - start).seconds()); + RCLCPP_INFO(getLogger(), "Spent %lf seconds constructing the database", (clock.now() - start).seconds()); if (state_storage) { auto constraint_approx = std::make_shared( @@ -492,16 +450,16 @@ ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs ".ompldb", state_storage, res.milestones); if (constraint_approximations_.find(constraint_approx->getName()) != constraint_approximations_.end()) - RCLCPP_WARN(LOGGER, "Overwriting constraint approximation named '%s'", constraint_approx->getName().c_str()); + RCLCPP_WARN(getLogger(), "Overwriting constraint approximation named '%s'", constraint_approx->getName().c_str()); constraint_approximations_[constraint_approx->getName()] = constraint_approx; res.approx = constraint_approx; } else - RCLCPP_ERROR(LOGGER, "Unable to construct constraint approximation for group '%s'", group.c_str()); + RCLCPP_ERROR(getLogger(), "Unable to construct constraint approximation for group '%s'", group.c_str()); return res; } -ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstraintApproximation( +ompl::base::StateStoragePtr ConstraintsLibrary::constructConstraintApproximation( ModelBasedPlanningContext* pcontext, const moveit_msgs::msg::Constraints& constr_sampling, const moveit_msgs::msg::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options, ConstraintApproximationConstructionResults& result) @@ -551,19 +509,19 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra if (done != done_now) { done = done_now; - RCLCPP_INFO(LOGGER, "%d%% complete (kept %0.1lf%% sampled states)", done, + RCLCPP_INFO(getLogger(), "%d%% complete (kept %0.1lf%% sampled states)", done, 100.0 * static_cast(state_storage->size()) / static_cast(attempts)); } if (!slow_warn && attempts > 10 && attempts > state_storage->size() * 100) { slow_warn = true; - RCLCPP_WARN(LOGGER, "Computation of valid state database is very slow..."); + RCLCPP_WARN(getLogger(), "Computation of valid state database is very slow..."); } if (attempts > options.samples && state_storage->size() == 0) { - RCLCPP_ERROR(LOGGER, "Unable to generate any samples"); + RCLCPP_ERROR(getLogger(), "Unable to generate any samples"); break; } @@ -580,18 +538,18 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra } result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start); - RCLCPP_INFO(LOGGER, "Generated %u states in %lf seconds", static_cast(state_storage->size()), + RCLCPP_INFO(getLogger(), "Generated %u states in %lf seconds", static_cast(state_storage->size()), result.state_sampling_time); if (constrained_sampler) { result.sampling_success_rate = constrained_sampler->getConstrainedSamplingRate(); - RCLCPP_INFO(LOGGER, "Constrained sampling rate: %lf", result.sampling_success_rate); + RCLCPP_INFO(getLogger(), "Constrained sampling rate: %lf", result.sampling_success_rate); } result.milestones = state_storage->size(); if (options.edges_per_sample > 0) { - RCLCPP_INFO(LOGGER, "Computing graph connections (max %u edges per sample) ...", options.edges_per_sample); + RCLCPP_INFO(getLogger(), "Computing graph connections (max %u edges per sample) ...", options.edges_per_sample); // construct connections const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace(); @@ -609,7 +567,7 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra if (done != done_now) { done = done_now; - RCLCPP_INFO(LOGGER, "%d%% complete", done); + RCLCPP_INFO(getLogger(), "%d%% complete", done); } if (cass->getMetadata(j).first.size() >= options.edges_per_sample) continue; @@ -665,7 +623,7 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra } result.state_connection_time = ompl::time::seconds(ompl::time::now() - start); - RCLCPP_INFO(LOGGER, "Computed possible connexions in %lf seconds. Added %d connexions", + RCLCPP_INFO(getLogger(), "Computed possible connexions in %lf seconds. Added %d connexions", result.state_connection_time, good); pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states); @@ -675,6 +633,8 @@ ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstra // TODO(davetcoleman): this function did not originally return a value, // causing compiler warnings in ROS Melodic // Update with more intelligent logic as needed - RCLCPP_ERROR(LOGGER, "No StateStoragePtr found - implement better solution here."); + RCLCPP_ERROR(getLogger(), "No StateStoragePtr found - implement better solution here."); return state_storage; } + +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp index d7cce130b4..2b411448ea 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -38,13 +38,20 @@ #include #include +#include #include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_planners_ompl.ompl_constraints"); - namespace ompl_interface { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ompl_constraints"); +} +} // namespace + Bounds::Bounds() : size_(0) { } @@ -173,7 +180,7 @@ Eigen::MatrixXd BaseConstraint::robotGeometricJacobian(const Eigen::Ref& /*x*/) const { - RCLCPP_WARN_STREAM(LOGGER, + RCLCPP_WARN_STREAM(getLogger(), "BaseConstraint: Constraint method calcError was not overridden, so it should not be used."); return Eigen::VectorXd::Zero(getCoDimension()); } @@ -181,7 +188,7 @@ Eigen::VectorXd BaseConstraint::calcError(const Eigen::Ref& /*x*/) const { RCLCPP_WARN_STREAM( - LOGGER, "BaseConstraint: Constraint method calcErrorJacobian was not overridden, so it should not be used."); + getLogger(), "BaseConstraint: Constraint method calcErrorJacobian was not overridden, so it should not be used."); return Eigen::MatrixXd::Zero(getCoDimension(), n_); } @@ -242,7 +249,7 @@ void EqualityPositionConstraint::parseConstraintMsg(const moveit_msgs::msg::Cons if (dims.at(i) < getTolerance()) { RCLCPP_ERROR_STREAM( - LOGGER, + getLogger(), "Dimension: " << i << " of position constraint is smaller than the tolerance used to evaluate the constraints. " "This will make all states invalid and planning will fail. Please use a value between: " @@ -375,11 +382,11 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo std::vector ompl_constraints; if (num_pos_con > 1) { - RCLCPP_WARN(LOGGER, "Only a single position constraint is supported. Using the first one."); + RCLCPP_WARN(getLogger(), "Only a single position constraint is supported. Using the first one."); } if (num_ori_con > 1) { - RCLCPP_WARN(LOGGER, "Only a single orientation constraint is supported. Using the first one."); + RCLCPP_WARN(getLogger(), "Only a single orientation constraint is supported. Using the first one."); } if (num_pos_con > 0) { @@ -403,7 +410,7 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo } if (num_pos_con < 1 && num_ori_con < 1) { - RCLCPP_ERROR(LOGGER, "No path constraints found in planning request."); + RCLCPP_ERROR(getLogger(), "No path constraints found in planning request."); return nullptr; } return std::make_shared(num_dofs, ompl_constraints); diff --git a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp index b17858e837..b828fac874 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp @@ -39,10 +39,17 @@ #include #include #include +#include namespace ompl_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.state_validity_checker"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ompl_state_validity_checker"); +} +} // namespace ompl_interface::StateValidityChecker::StateValidityChecker(const ModelBasedPlanningContext* pc) : ompl::base::StateValidityChecker(pc->getOMPLSimpleSetup()->getSpaceInformation()) @@ -86,7 +93,7 @@ bool StateValidityChecker::isValid(const ompl::base::State* state, bool verbose) { if (verbose) { - RCLCPP_INFO(LOGGER, "State outside bounds"); + RCLCPP_INFO(getLogger(), "State outside bounds"); } const_cast(state)->as()->markInvalid(); return false; @@ -140,7 +147,7 @@ bool StateValidityChecker::isValid(const ompl::base::State* state, double& dist, { if (verbose) { - RCLCPP_INFO(LOGGER, "State outside bounds"); + RCLCPP_INFO(getLogger(), "State outside bounds"); } const_cast(state)->as()->markInvalid(0.0); return false; @@ -226,7 +233,7 @@ bool ConstrainedPlanningStateValidityChecker::isValid(const ompl::base::State* w // do not use the unwrapped state here, as satisfiesBounds expects a state of type ConstrainedStateSpace::StateType if (!si_->satisfiesBounds(wrapped_state)) // si_ = ompl::base::SpaceInformation { - RCLCPP_DEBUG(LOGGER, "State outside bounds"); + RCLCPP_DEBUG(getLogger(), "State outside bounds"); const_cast(state)->as()->markInvalid(); return false; } @@ -283,7 +290,7 @@ bool ConstrainedPlanningStateValidityChecker::isValid(const ompl::base::State* w // do not use the unwrapped state here, as satisfiesBounds expects a state of type ConstrainedStateSpace::StateType if (!si_->satisfiesBounds(wrapped_state)) // si_ = ompl::base::SpaceInformation { - RCLCPP_DEBUG(LOGGER, "State outside bounds"); + RCLCPP_DEBUG(getLogger(), "State outside bounds"); const_cast(state)->as()->markInvalid(0.0); return false; } diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 84c920e024..9815b956a7 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -50,6 +50,7 @@ #include #include +#include #include #include @@ -69,11 +70,16 @@ namespace ompl_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.model_based_planning_context"); -} // namespace ompl_interface +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ompl_model_based_planning_context"); +} +} // namespace -ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std::string& name, - const ModelBasedPlanningContextSpecification& spec) +ModelBasedPlanningContext::ModelBasedPlanningContext(const std::string& name, + const ModelBasedPlanningContextSpecification& spec) : planning_interface::PlanningContext(name, spec.state_space_->getJointModelGroup()->getName()) , spec_(spec) , complete_initial_robot_state_(spec.state_space_->getRobotModel()) @@ -100,8 +106,7 @@ ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext(const std:: constraints_library_ = std::make_shared(this); } -void ompl_interface::ModelBasedPlanningContext::configure(const rclcpp::Node::SharedPtr& node, - bool use_constraints_approximations) +void ModelBasedPlanningContext::configure(const rclcpp::Node::SharedPtr& node, bool use_constraints_approximations) { loadConstraintApproximations(node); if (!use_constraints_approximations) @@ -137,7 +142,7 @@ void ompl_interface::ModelBasedPlanningContext::configure(const rclcpp::Node::Sh if (constraint_approx) { getOMPLStateSpace()->setInterpolationFunction(constraint_approx->getInterpolationFunction()); - RCLCPP_INFO(LOGGER, "Using precomputed interpolation states"); + RCLCPP_INFO(getLogger(), "Using precomputed interpolation states"); } } @@ -146,11 +151,11 @@ void ompl_interface::ModelBasedPlanningContext::configure(const rclcpp::Node::Sh ompl_simple_setup_->setup(); } -void ompl_interface::ModelBasedPlanningContext::setProjectionEvaluator(const std::string& peval) +void ModelBasedPlanningContext::setProjectionEvaluator(const std::string& peval) { if (!spec_.state_space_) { - RCLCPP_ERROR(LOGGER, "No state space is configured yet"); + RCLCPP_ERROR(getLogger(), "No state space is configured yet"); return; } ob::ProjectionEvaluatorPtr projection_eval = getProjectionEvaluator(peval); @@ -158,8 +163,7 @@ void ompl_interface::ModelBasedPlanningContext::setProjectionEvaluator(const std spec_.state_space_->registerDefaultProjection(projection_eval); } -ompl::base::ProjectionEvaluatorPtr -ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::string& peval) const +ompl::base::ProjectionEvaluatorPtr ModelBasedPlanningContext::getProjectionEvaluator(const std::string& peval) const { if (peval.find_first_of("link(") == 0 && peval[peval.length() - 1] == ')') { @@ -170,7 +174,7 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str } else { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Attempted to set projection evaluator with respect to position of link '%s', " "but that link is not known to the kinematic model.", link_name.c_str()); @@ -199,12 +203,13 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str } else { - RCLCPP_WARN(LOGGER, "%s: Ignoring joint '%s' in projection since it has 0 DOF", name_.c_str(), joint.c_str()); + RCLCPP_WARN(getLogger(), "%s: Ignoring joint '%s' in projection since it has 0 DOF", name_.c_str(), + joint.c_str()); } } else { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "%s: Attempted to set projection evaluator with respect to value of joint " "'%s', but that joint is not known to the group '%s'.", name_.c_str(), joint.c_str(), getGroupName().c_str()); @@ -212,7 +217,7 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str } if (j.empty()) { - RCLCPP_ERROR(LOGGER, "%s: No valid joints specified for joint projection", name_.c_str()); + RCLCPP_ERROR(getLogger(), "%s: No valid joints specified for joint projection", name_.c_str()); } else { @@ -221,21 +226,21 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str } else { - RCLCPP_ERROR(LOGGER, "Unable to allocate projection evaluator based on description: '%s'", peval.c_str()); + RCLCPP_ERROR(getLogger(), "Unable to allocate projection evaluator based on description: '%s'", peval.c_str()); } return ob::ProjectionEvaluatorPtr(); } ompl::base::StateSamplerPtr -ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const ompl::base::StateSpace* state_space) const +ModelBasedPlanningContext::allocPathConstrainedSampler(const ompl::base::StateSpace* state_space) const { if (spec_.state_space_.get() != state_space) { - RCLCPP_ERROR(LOGGER, "%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str()); + RCLCPP_ERROR(getLogger(), "%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str()); return ompl::base::StateSamplerPtr(); } - RCLCPP_DEBUG(LOGGER, "%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str()); + RCLCPP_DEBUG(getLogger(), "%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str()); if (path_constraints_) { @@ -252,7 +257,7 @@ ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const omp ompl::base::StateSamplerPtr state_sampler = state_sampler_allocator(state_space); if (state_sampler) { - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "%s: Using precomputed state sampler (approximated constraint space) for constraint '%s'", name_.c_str(), path_constraints_msg_.name.c_str()); return state_sampler; @@ -270,15 +275,15 @@ ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const omp if (constraint_sampler) { - RCLCPP_INFO(LOGGER, "%s: Allocating specialized state sampler for state space", name_.c_str()); + RCLCPP_INFO(getLogger(), "%s: Allocating specialized state sampler for state space", name_.c_str()); return std::make_shared(this, constraint_sampler); } } - RCLCPP_DEBUG(LOGGER, "%s: Allocating default state sampler for state space", name_.c_str()); + RCLCPP_DEBUG(getLogger(), "%s: Allocating default state sampler for state space", name_.c_str()); return state_space->allocDefaultStateSampler(); } -void ompl_interface::ModelBasedPlanningContext::useConfig() +void ModelBasedPlanningContext::useConfig() { const std::map& config = spec_.config_; if (config.empty()) @@ -356,7 +361,7 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() } else { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Optimization objective %s is invalid or not defined, using PathLengthOptimizationObjective instead", optimizer.c_str()); objective = @@ -402,7 +407,7 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() if (it == cfg.end()) { if (name_ != getGroupName()) - RCLCPP_WARN(LOGGER, "%s: Attribute 'type' not specified in planner configuration", name_.c_str()); + RCLCPP_WARN(getLogger(), "%s: Attribute 'type' not specified in planner configuration", name_.c_str()); } else { @@ -412,7 +417,7 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() ompl_simple_setup_->setPlannerAllocator( [planner_name, &spec = spec_, allocator = spec_.planner_selector_(type)]( const ompl::base::SpaceInformationPtr& si) { return allocator(si, planner_name, spec); }); - RCLCPP_INFO(LOGGER, + RCLCPP_INFO(getLogger(), "Planner configuration '%s' will use planner '%s'. " "Additional configuration parameters will be set when the planner is constructed.", name_.c_str(), type.c_str()); @@ -425,16 +430,16 @@ void ompl_interface::ModelBasedPlanningContext::useConfig() ompl_simple_setup_->getSpaceInformation()->setup(); } -void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_msgs::msg::WorkspaceParameters& wparams) +void ModelBasedPlanningContext::setPlanningVolume(const moveit_msgs::msg::WorkspaceParameters& wparams) { if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 && wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 && wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0) { - RCLCPP_WARN(LOGGER, "It looks like the planning volume was not specified."); + RCLCPP_WARN(getLogger(), "It looks like the planning volume was not specified."); } - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = " "[%f, %f], z = [%f, %f]", name_.c_str(), wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y, wparams.max_corner.y, @@ -444,7 +449,7 @@ void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_m wparams.max_corner.y, wparams.min_corner.z, wparams.max_corner.z); } -void ompl_interface::ModelBasedPlanningContext::simplifySolution(double timeout) +void ModelBasedPlanningContext::simplifySolution(double timeout) { ompl::time::point start = ompl::time::now(); ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); @@ -454,7 +459,7 @@ void ompl_interface::ModelBasedPlanningContext::simplifySolution(double timeout) unregisterTerminationCondition(); } -void ompl_interface::ModelBasedPlanningContext::interpolateSolution() +void ModelBasedPlanningContext::interpolateSolution() { if (ompl_simple_setup_->haveSolutionPath()) { @@ -482,8 +487,8 @@ void ompl_interface::ModelBasedPlanningContext::interpolateSolution() } } -void ompl_interface::ModelBasedPlanningContext::convertPath(const ompl::geometric::PathGeometric& pg, - robot_trajectory::RobotTrajectory& traj) const +void ModelBasedPlanningContext::convertPath(const ompl::geometric::PathGeometric& pg, + robot_trajectory::RobotTrajectory& traj) const { moveit::core::RobotState ks = complete_initial_robot_state_; for (std::size_t i = 0; i < pg.getStateCount(); ++i) @@ -493,7 +498,7 @@ void ompl_interface::ModelBasedPlanningContext::convertPath(const ompl::geometri } } -bool ompl_interface::ModelBasedPlanningContext::getSolutionPath(robot_trajectory::RobotTrajectory& traj) const +bool ModelBasedPlanningContext::getSolutionPath(robot_trajectory::RobotTrajectory& traj) const { traj.clear(); if (ompl_simple_setup_->haveSolutionPath()) @@ -503,7 +508,7 @@ bool ompl_interface::ModelBasedPlanningContext::getSolutionPath(robot_trajectory return ompl_simple_setup_->haveSolutionPath(); } -void ompl_interface::ModelBasedPlanningContext::setVerboseStateValidityChecks(bool flag) +void ModelBasedPlanningContext::setVerboseStateValidityChecks(bool flag) { if (ompl_simple_setup_->getStateValidityChecker()) { @@ -511,7 +516,7 @@ void ompl_interface::ModelBasedPlanningContext::setVerboseStateValidityChecks(bo } } -ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal() +ompl::base::GoalPtr ModelBasedPlanningContext::constructGoal() { // ******************* set up the goal representation, based on goal constraints @@ -538,15 +543,14 @@ ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal() } else { - RCLCPP_ERROR(LOGGER, "Unable to construct goal representation"); + RCLCPP_ERROR(getLogger(), "Unable to construct goal representation"); } return ob::GoalPtr(); } ompl::base::PlannerTerminationCondition -ompl_interface::ModelBasedPlanningContext::constructPlannerTerminationCondition(double timeout, - const ompl::time::point& start) +ModelBasedPlanningContext::constructPlannerTerminationCondition(double timeout, const ompl::time::point& start) { auto it = spec_.config_.find("termination_condition"); if (it == spec_.config_.end()) @@ -560,7 +564,7 @@ ompl_interface::ModelBasedPlanningContext::constructPlannerTerminationCondition( if (termination_and_params.empty()) { - RCLCPP_ERROR(LOGGER, "Termination condition not specified"); + RCLCPP_ERROR(getLogger(), "Termination condition not specified"); } // Terminate if a maximum number of iterations is exceeded or a timeout occurs. @@ -576,7 +580,7 @@ ompl_interface::ModelBasedPlanningContext::constructPlannerTerminationCondition( } else { - RCLCPP_ERROR(LOGGER, "Missing argument to Iteration termination condition"); + RCLCPP_ERROR(getLogger(), "Missing argument to Iteration termination condition"); } } // Terminate if the cost has converged or a timeout occurs. @@ -608,20 +612,19 @@ ompl_interface::ModelBasedPlanningContext::constructPlannerTerminationCondition( } else { - RCLCPP_ERROR(LOGGER, "Unknown planner termination condition"); + RCLCPP_ERROR(getLogger(), "Unknown planner termination condition"); } // return a planner termination condition to suppress compiler warning return ob::plannerAlwaysTerminatingCondition(); } -void ompl_interface::ModelBasedPlanningContext::setCompleteInitialState( - const moveit::core::RobotState& complete_initial_robot_state) +void ModelBasedPlanningContext::setCompleteInitialState(const moveit::core::RobotState& complete_initial_robot_state) { complete_initial_robot_state_ = complete_initial_robot_state; complete_initial_robot_state_.update(); } -void ompl_interface::ModelBasedPlanningContext::clear() +void ModelBasedPlanningContext::clear() { if (!multi_query_planning_enabled_) { @@ -647,8 +650,8 @@ void ompl_interface::ModelBasedPlanningContext::clear() getOMPLStateSpace()->setInterpolationFunction(InterpolationFunction()); } -bool ompl_interface::ModelBasedPlanningContext::setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints, - moveit_msgs::msg::MoveItErrorCodes* /*error*/) +bool ModelBasedPlanningContext::setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints, + moveit_msgs::msg::MoveItErrorCodes* /*error*/) { // ******************* set the path constraints to use path_constraints_ = std::make_shared(getRobotModel()); @@ -658,9 +661,9 @@ bool ompl_interface::ModelBasedPlanningContext::setPathConstraints(const moveit_ return true; } -bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints( - const std::vector& goal_constraints, - const moveit_msgs::msg::Constraints& path_constraints, moveit_msgs::msg::MoveItErrorCodes* error) +bool ModelBasedPlanningContext::setGoalConstraints(const std::vector& goal_constraints, + const moveit_msgs::msg::Constraints& path_constraints, + moveit_msgs::msg::MoveItErrorCodes* error) { // ******************* check if the input is correct goal_constraints_.clear(); @@ -678,7 +681,7 @@ bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints( if (goal_constraints_.empty()) { - RCLCPP_WARN(LOGGER, "%s: No goal constraints specified. There is no problem to solve.", name_.c_str()); + RCLCPP_WARN(getLogger(), "%s: No goal constraints specified. There is no problem to solve.", name_.c_str()); if (error) { error->val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; @@ -691,8 +694,7 @@ bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints( return static_cast(goal); } -bool ompl_interface::ModelBasedPlanningContext::benchmark(double timeout, unsigned int count, - const std::string& filename) +bool ModelBasedPlanningContext::benchmark(double timeout, unsigned int count, const std::string& filename) { ompl_benchmark_.clearPlanners(); ompl_simple_setup_->setup(); @@ -709,7 +711,7 @@ bool ompl_interface::ModelBasedPlanningContext::benchmark(double timeout, unsign return filename.empty() ? ompl_benchmark_.saveResultsToFile() : ompl_benchmark_.saveResultsToFile(filename.c_str()); } -void ompl_interface::ModelBasedPlanningContext::startSampling() +void ModelBasedPlanningContext::startSampling() { bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES); if (gls) @@ -723,7 +725,7 @@ void ompl_interface::ModelBasedPlanningContext::startSampling() } } -void ompl_interface::ModelBasedPlanningContext::stopSampling() +void ModelBasedPlanningContext::stopSampling() { bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES); if (gls) @@ -737,7 +739,7 @@ void ompl_interface::ModelBasedPlanningContext::stopSampling() } } -void ompl_interface::ModelBasedPlanningContext::preSolve() +void ModelBasedPlanningContext::preSolve() { // clear previously computed solutions ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths(); @@ -750,25 +752,25 @@ void ompl_interface::ModelBasedPlanningContext::preSolve() ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter(); } -void ompl_interface::ModelBasedPlanningContext::postSolve() +void ModelBasedPlanningContext::postSolve() { stopSampling(); int v = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getValidMotionCount(); int iv = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getInvalidMotionCount(); - RCLCPP_DEBUG(LOGGER, "There were %d valid motions and %d invalid motions.", v, iv); + RCLCPP_DEBUG(getLogger(), "There were %d valid motions and %d invalid motions.", v, iv); // Debug OMPL setup and solution std::stringstream debug_out; ompl_simple_setup_->print(debug_out); - RCLCPP_DEBUG(LOGGER, "%s", rclcpp::get_c_string(debug_out.str())); + RCLCPP_DEBUG(getLogger(), "%s", rclcpp::get_c_string(debug_out.str())); } -void ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res) +void ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res) { res.error_code = solve(request_.allowed_planning_time, request_.num_planning_attempts); if (res.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { - RCLCPP_ERROR(LOGGER, "Unable to solve the planning problem"); + RCLCPP_ERROR(getLogger(), "Unable to solve the planning problem"); return; } double ptime = getLastPlanTime(); @@ -784,7 +786,7 @@ void ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion } // fill the response - RCLCPP_DEBUG(LOGGER, "%s: Returning successful solution with %lu states", getName().c_str(), + RCLCPP_DEBUG(getLogger(), "%s: Returning successful solution with %lu states", getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount()); res.trajectory = std::make_shared(getRobotModel(), getGroupName()); @@ -792,13 +794,13 @@ void ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion res.planning_time = ptime; } -void ompl_interface::ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) +void ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) { moveit_msgs::msg::MoveItErrorCodes moveit_result = solve(request_.allowed_planning_time, request_.num_planning_attempts); if (moveit_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { - RCLCPP_INFO(LOGGER, "Unable to solve the planning problem"); + RCLCPP_INFO(getLogger(), "Unable to solve the planning problem"); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return; } @@ -835,13 +837,12 @@ void ompl_interface::ModelBasedPlanningContext::solve(planning_interface::Motion getSolutionPath(*res.trajectory.back()); } - RCLCPP_DEBUG(LOGGER, "%s: Returning successful solution with %lu states", getName().c_str(), + RCLCPP_DEBUG(getLogger(), "%s: Returning successful solution with %lu states", getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount()); res.error_code.val = moveit_result.val; } -const moveit_msgs::msg::MoveItErrorCodes ompl_interface::ModelBasedPlanningContext::solve(double timeout, - unsigned int count) +const moveit_msgs::msg::MoveItErrorCodes ModelBasedPlanningContext::solve(double timeout, unsigned int count) { ompl::time::point start = ompl::time::now(); preSolve(); @@ -850,7 +851,7 @@ const moveit_msgs::msg::MoveItErrorCodes ompl_interface::ModelBasedPlanningConte result.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; if (count <= 1 || multi_query_planning_enabled_) // multi-query planners should always run in single instances { - RCLCPP_DEBUG(LOGGER, "%s: Solving the planning problem once...", name_.c_str()); + RCLCPP_DEBUG(getLogger(), "%s: Solving the planning problem once...", name_.c_str()); ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start); registerTerminationCondition(ptc); std::ignore = ompl_simple_setup_->solve(ptc); @@ -861,7 +862,7 @@ const moveit_msgs::msg::MoveItErrorCodes ompl_interface::ModelBasedPlanningConte } else { - RCLCPP_DEBUG(LOGGER, "%s: Solving the planning problem %u times...", name_.c_str(), count); + RCLCPP_DEBUG(getLogger(), "%s: Solving the planning problem %u times...", name_.c_str(), count); ompl_parallel_plan_.clearHybridizationPaths(); if (count <= max_planning_threads_) { @@ -954,42 +955,42 @@ const moveit_msgs::msg::MoveItErrorCodes ompl_interface::ModelBasedPlanningConte return result; } -void ompl_interface::ModelBasedPlanningContext::registerTerminationCondition(const ob::PlannerTerminationCondition& ptc) +void ModelBasedPlanningContext::registerTerminationCondition(const ob::PlannerTerminationCondition& ptc) { std::unique_lock slock(ptc_lock_); ptc_ = &ptc; } -void ompl_interface::ModelBasedPlanningContext::unregisterTerminationCondition() +void ModelBasedPlanningContext::unregisterTerminationCondition() { std::unique_lock slock(ptc_lock_); ptc_ = nullptr; } -int32_t ompl_interface::ModelBasedPlanningContext::logPlannerStatus(const og::SimpleSetupPtr& ompl_simple_setup) +int32_t ModelBasedPlanningContext::logPlannerStatus(const og::SimpleSetupPtr& ompl_simple_setup) { auto result = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; const ompl::base::PlannerStatus ompl_status = ompl_simple_setup->getLastPlannerStatus(); switch (ompl::base::PlannerStatus::StatusType(ompl_status)) { case ompl::base::PlannerStatus::UNKNOWN: - RCLCPP_WARN(LOGGER, "Motion planning failed for an unknown reason"); + RCLCPP_WARN(getLogger(), "Motion planning failed for an unknown reason"); result = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; break; case ompl::base::PlannerStatus::INVALID_START: - RCLCPP_WARN(LOGGER, "Invalid start state"); + RCLCPP_WARN(getLogger(), "Invalid start state"); result = moveit_msgs::msg::MoveItErrorCodes::START_STATE_INVALID; break; case ompl::base::PlannerStatus::INVALID_GOAL: - RCLCPP_WARN(LOGGER, "Invalid goal state"); + RCLCPP_WARN(getLogger(), "Invalid goal state"); result = moveit_msgs::msg::MoveItErrorCodes::GOAL_STATE_INVALID; break; case ompl::base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE: - RCLCPP_WARN(LOGGER, "Unrecognized goal type"); + RCLCPP_WARN(getLogger(), "Unrecognized goal type"); result = moveit_msgs::msg::MoveItErrorCodes::UNRECOGNIZED_GOAL_TYPE; break; case ompl::base::PlannerStatus::TIMEOUT: - RCLCPP_WARN(LOGGER, "Timed out: %.1fs ≥ %.1fs", ompl_simple_setup->getLastPlanComputationTime(), + RCLCPP_WARN(getLogger(), "Timed out: %.1fs ≥ %.1fs", ompl_simple_setup->getLastPlanComputationTime(), request_.allowed_planning_time); result = moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT; break; @@ -997,13 +998,13 @@ int32_t ompl_interface::ModelBasedPlanningContext::logPlannerStatus(const og::Si // timeout is a common reason for APPROXIMATE_SOLUTION if (ompl_simple_setup->getLastPlanComputationTime() > request_.allowed_planning_time) { - RCLCPP_WARN(LOGGER, "Planning timed out: %.1fs ≥ %.1fs", ompl_simple_setup->getLastPlanComputationTime(), + RCLCPP_WARN(getLogger(), "Planning timed out: %.1fs ≥ %.1fs", ompl_simple_setup->getLastPlanComputationTime(), request_.allowed_planning_time); result = moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT; } else { - RCLCPP_WARN(LOGGER, "Solution is approximate"); + RCLCPP_WARN(getLogger(), "Solution is approximate"); result = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; } break; @@ -1011,22 +1012,22 @@ int32_t ompl_interface::ModelBasedPlanningContext::logPlannerStatus(const og::Si result = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; break; case ompl::base::PlannerStatus::CRASH: - RCLCPP_WARN(LOGGER, "OMPL crashed!"); + RCLCPP_WARN(getLogger(), "OMPL crashed!"); result = moveit_msgs::msg::MoveItErrorCodes::CRASH; break; case ompl::base::PlannerStatus::ABORT: - RCLCPP_WARN(LOGGER, "OMPL was aborted"); + RCLCPP_WARN(getLogger(), "OMPL was aborted"); result = moveit_msgs::msg::MoveItErrorCodes::ABORT; break; default: // This should never happen - RCLCPP_WARN(LOGGER, "Unexpected PlannerStatus code from OMPL."); + RCLCPP_WARN(getLogger(), "Unexpected PlannerStatus code from OMPL."); result = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; } return result; } -bool ompl_interface::ModelBasedPlanningContext::terminate() +bool ModelBasedPlanningContext::terminate() { std::unique_lock slock(ptc_lock_); if (ptc_) @@ -1036,7 +1037,7 @@ bool ompl_interface::ModelBasedPlanningContext::terminate() return true; } -bool ompl_interface::ModelBasedPlanningContext::saveConstraintApproximations(const rclcpp::Node::SharedPtr& node) +bool ModelBasedPlanningContext::saveConstraintApproximations(const rclcpp::Node::SharedPtr& node) { std::string constraint_path; if (node->get_parameter("constraint_approximations_path", constraint_path)) @@ -1044,11 +1045,11 @@ bool ompl_interface::ModelBasedPlanningContext::saveConstraintApproximations(con constraints_library_->saveConstraintApproximations(constraint_path); return true; } - RCLCPP_WARN(LOGGER, "ROS param 'constraint_approximations' not found. Unable to save constraint approximations"); + RCLCPP_WARN(getLogger(), "ROS param 'constraint_approximations' not found. Unable to save constraint approximations"); return false; } -bool ompl_interface::ModelBasedPlanningContext::loadConstraintApproximations(const rclcpp::Node::SharedPtr& node) +bool ModelBasedPlanningContext::loadConstraintApproximations(const rclcpp::Node::SharedPtr& node) { std::string constraint_path; if (node->get_parameter("constraint_approximations_path", constraint_path)) @@ -1056,8 +1057,10 @@ bool ompl_interface::ModelBasedPlanningContext::loadConstraintApproximations(con constraints_library_->loadConstraintApproximations(constraint_path); std::stringstream ss; constraints_library_->printConstraintApproximations(ss); - RCLCPP_INFO_STREAM(LOGGER, ss.str()); + RCLCPP_INFO_STREAM(getLogger(), ss.str()); return true; } return false; } + +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index 61afcdbf4d..1291b791f0 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -39,10 +39,17 @@ #include #include #include +#include namespace ompl_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.ompl_interface"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ompl_interface"); +} +} // namespace OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model, const rclcpp::Node::SharedPtr& node, const std::string& parameter_namespace) @@ -53,7 +60,7 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model , context_manager_(robot_model, constraint_sampler_manager_) , use_constraints_approximations_(true) { - RCLCPP_DEBUG(LOGGER, "Initializing OMPL interface using ROS parameters"); + RCLCPP_DEBUG(getLogger(), "Initializing OMPL interface using ROS parameters"); loadPlannerConfigurations(); loadConstraintSamplers(); } @@ -68,7 +75,7 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model , context_manager_(robot_model, constraint_sampler_manager_) , use_constraints_approximations_(true) { - RCLCPP_DEBUG(LOGGER, "Initializing OMPL interface using specified configuration"); + RCLCPP_DEBUG(getLogger(), "Initializing OMPL interface using specified configuration"); setPlannerConfigurations(pconfig); loadConstraintSamplers(); } @@ -127,7 +134,7 @@ bool OMPLInterface::loadPlannerConfiguration(const std::string& group_name, cons if (planner_params_result.names.empty()) { - RCLCPP_ERROR(LOGGER, "Could not find the planner configuration '%s' on the param server", planner_id.c_str()); + RCLCPP_ERROR(getLogger(), "Could not find the planner configuration '%s' on the param server", planner_id.c_str()); return false; } @@ -179,9 +186,9 @@ void OMPLInterface::loadPlannerConfigurations() const rclcpp::Parameter parameter = node_->get_parameter(param_name); if (parameter.get_type() != type) { - RCLCPP_ERROR_STREAM(LOGGER, "Invalid type for parameter '" << name << "' expected [" - << rclcpp::to_string(type) << "] got [" - << rclcpp::to_string(parameter.get_type()) << ']'); + RCLCPP_ERROR_STREAM(getLogger(), "Invalid type for parameter '" + << name << "' expected [" << rclcpp::to_string(type) << "] got [" + << rclcpp::to_string(parameter.get_type()) << ']'); continue; } if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING) @@ -241,11 +248,11 @@ void OMPLInterface::loadPlannerConfigurations() for (const auto& [name, config_settings] : pconfig) { - RCLCPP_DEBUG(LOGGER, "Parameters for configuration '%s'", name.c_str()); + RCLCPP_DEBUG(getLogger(), "Parameters for configuration '%s'", name.c_str()); for (const auto& [param_name, param_value] : config_settings.config) { - RCLCPP_DEBUG_STREAM(LOGGER, " - " << param_name << " = " << param_value); + RCLCPP_DEBUG_STREAM(getLogger(), " - " << param_name << " = " << param_value); } } @@ -254,6 +261,6 @@ void OMPLInterface::loadPlannerConfigurations() void OMPLInterface::printStatus() { - RCLCPP_INFO(LOGGER, "OMPL ROS interface is running."); + RCLCPP_INFO(getLogger(), "OMPL ROS interface is running."); } } // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index ba06cf3d43..9e69e99d72 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -37,13 +37,19 @@ #include #include #include +#include #include namespace ompl_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.ompl_planner_manager"); -static const rclcpp::Logger OMPL_LOGGER = rclcpp::get_logger("ompl"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ompl_planner_manager"); +} +} // namespace class OMPLPlannerManager : public planning_interface::PlannerManager { @@ -62,13 +68,13 @@ class OMPLPlannerManager : public planning_interface::PlannerManager case ompl::msg::LOG_DEBUG: case ompl::msg::LOG_INFO: // LOG_INFO too verbose for MoveIt usage, so we reduce the logger level to DEBUG - RCLCPP_DEBUG(OMPL_LOGGER, "%s:%i - %s", filename, line, text.c_str()); + RCLCPP_DEBUG(getLogger(), "%s:%i - %s", filename, line, text.c_str()); break; case ompl::msg::LOG_WARN: - RCLCPP_WARN(OMPL_LOGGER, "%s:%i - %s", filename, line, text.c_str()); + RCLCPP_WARN(getLogger(), "%s:%i - %s", filename, line, text.c_str()); break; case ompl::msg::LOG_ERROR: - RCLCPP_ERROR(OMPL_LOGGER, "%s:%i - %s", filename, line, text.c_str()); + RCLCPP_ERROR(getLogger(), "%s:%i - %s", filename, line, text.c_str()); break; case ompl::msg::LOG_NONE: default: diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp index a8f29ea03e..361c580239 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp @@ -36,13 +36,19 @@ #include #include +#include namespace ompl_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.model_based_state_space"); -} // namespace ompl_interface +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ompl_model_based_state_space"); +} +} // namespace -ompl_interface::ModelBasedStateSpace::ModelBasedStateSpace(ModelBasedStateSpaceSpecification spec) +ModelBasedStateSpace::ModelBasedStateSpace(ModelBasedStateSpaceSpecification spec) : ompl::base::StateSpace(), spec_(std::move(spec)) { // set the state space name @@ -54,7 +60,7 @@ ompl_interface::ModelBasedStateSpace::ModelBasedStateSpace(ModelBasedStateSpaceS // make sure we have bounds for every joint stored within the spec (use default bounds if not specified) if (!spec_.joint_bounds_.empty() && spec_.joint_bounds_.size() != joint_model_vector_.size()) { - RCLCPP_ERROR(LOGGER, "Joint group '%s' has incorrect bounds specified. Using the default bounds instead.", + RCLCPP_ERROR(getLogger(), "Joint group '%s' has incorrect bounds specified. Using the default bounds instead.", spec_.joint_model_group_->getName().c_str()); spec_.joint_bounds_.clear(); } @@ -80,18 +86,18 @@ ompl_interface::ModelBasedStateSpace::ModelBasedStateSpace(ModelBasedStateSpaceS [this] { return getTagSnapToSegment(); }); } -ompl_interface::ModelBasedStateSpace::~ModelBasedStateSpace() = default; +ModelBasedStateSpace::~ModelBasedStateSpace() = default; -double ompl_interface::ModelBasedStateSpace::getTagSnapToSegment() const +double ModelBasedStateSpace::getTagSnapToSegment() const { return tag_snap_to_segment_; } -void ompl_interface::ModelBasedStateSpace::setTagSnapToSegment(double snap) +void ModelBasedStateSpace::setTagSnapToSegment(double snap) { if (snap < 0.0 || snap > 1.0) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. " "Value remains as previously set (%lf)", tag_snap_to_segment_); @@ -103,21 +109,20 @@ void ompl_interface::ModelBasedStateSpace::setTagSnapToSegment(double snap) } } -ompl::base::State* ompl_interface::ModelBasedStateSpace::allocState() const +ompl::base::State* ModelBasedStateSpace::allocState() const { auto* state = new StateType(); state->values = new double[variable_count_]; return state; } -void ompl_interface::ModelBasedStateSpace::freeState(ompl::base::State* state) const +void ModelBasedStateSpace::freeState(ompl::base::State* state) const { delete[] state->as()->values; delete state->as(); } -void ompl_interface::ModelBasedStateSpace::copyState(ompl::base::State* destination, - const ompl::base::State* source) const +void ModelBasedStateSpace::copyState(ompl::base::State* destination, const ompl::base::State* source) const { memcpy(destination->as()->values, source->as()->values, state_values_size_); destination->as()->tag = source->as()->tag; @@ -125,24 +130,24 @@ void ompl_interface::ModelBasedStateSpace::copyState(ompl::base::State* destinat destination->as()->distance = source->as()->distance; } -unsigned int ompl_interface::ModelBasedStateSpace::getSerializationLength() const +unsigned int ModelBasedStateSpace::getSerializationLength() const { return state_values_size_ + sizeof(int); } -void ompl_interface::ModelBasedStateSpace::serialize(void* serialization, const ompl::base::State* state) const +void ModelBasedStateSpace::serialize(void* serialization, const ompl::base::State* state) const { *reinterpret_cast(serialization) = state->as()->tag; memcpy(reinterpret_cast(serialization) + sizeof(int), state->as()->values, state_values_size_); } -void ompl_interface::ModelBasedStateSpace::deserialize(ompl::base::State* state, const void* serialization) const +void ModelBasedStateSpace::deserialize(ompl::base::State* state, const void* serialization) const { state->as()->tag = *reinterpret_cast(serialization); memcpy(state->as()->values, reinterpret_cast(serialization) + sizeof(int), state_values_size_); } -unsigned int ompl_interface::ModelBasedStateSpace::getDimension() const +unsigned int ModelBasedStateSpace::getDimension() const { unsigned int d = 0; for (const moveit::core::JointModel* i : joint_model_vector_) @@ -150,12 +155,12 @@ unsigned int ompl_interface::ModelBasedStateSpace::getDimension() const return d; } -double ompl_interface::ModelBasedStateSpace::getMaximumExtent() const +double ModelBasedStateSpace::getMaximumExtent() const { return spec_.joint_model_group_->getMaximumExtent(spec_.joint_bounds_); } -double ompl_interface::ModelBasedStateSpace::getMeasure() const +double ModelBasedStateSpace::getMeasure() const { double m = 1.0; for (const moveit::core::JointModel::Bounds* bounds : spec_.joint_bounds_) @@ -168,8 +173,7 @@ double ompl_interface::ModelBasedStateSpace::getMeasure() const return m; } -double ompl_interface::ModelBasedStateSpace::distance(const ompl::base::State* state1, - const ompl::base::State* state2) const +double ModelBasedStateSpace::distance(const ompl::base::State* state1, const ompl::base::State* state2) const { if (distance_function_) { @@ -181,8 +185,7 @@ double ompl_interface::ModelBasedStateSpace::distance(const ompl::base::State* s } } -bool ompl_interface::ModelBasedStateSpace::equalStates(const ompl::base::State* state1, - const ompl::base::State* state2) const +bool ModelBasedStateSpace::equalStates(const ompl::base::State* state1, const ompl::base::State* state2) const { for (unsigned int i = 0; i < variable_count_; ++i) { @@ -193,19 +196,19 @@ bool ompl_interface::ModelBasedStateSpace::equalStates(const ompl::base::State* return true; } -void ompl_interface::ModelBasedStateSpace::enforceBounds(ompl::base::State* state) const +void ModelBasedStateSpace::enforceBounds(ompl::base::State* state) const { spec_.joint_model_group_->enforcePositionBounds(state->as()->values, spec_.joint_bounds_); } -bool ompl_interface::ModelBasedStateSpace::satisfiesBounds(const ompl::base::State* state) const +bool ModelBasedStateSpace::satisfiesBounds(const ompl::base::State* state) const { return spec_.joint_model_group_->satisfiesPositionBounds(state->as()->values, spec_.joint_bounds_, std::numeric_limits::epsilon()); } -void ompl_interface::ModelBasedStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to, - const double t, ompl::base::State* state) const +void ModelBasedStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t, + ompl::base::State* state) const { // clear any cached info (such as validity known or not) state->as()->clearKnownInformation(); @@ -232,16 +235,15 @@ void ompl_interface::ModelBasedStateSpace::interpolate(const ompl::base::State* } } -double* ompl_interface::ModelBasedStateSpace::getValueAddressAtIndex(ompl::base::State* state, - const unsigned int index) const +double* ModelBasedStateSpace::getValueAddressAtIndex(ompl::base::State* state, const unsigned int index) const { if (index >= variable_count_) return nullptr; return state->as()->values + index; } -void ompl_interface::ModelBasedStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, - double minZ, double maxZ) +void ModelBasedStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, + double maxZ) { for (std::size_t i = 0; i < joint_model_vector_.size(); ++i) { @@ -264,7 +266,7 @@ void ompl_interface::ModelBasedStateSpace::setPlanningVolume(double minX, double } } -ompl::base::StateSamplerPtr ompl_interface::ModelBasedStateSpace::allocDefaultStateSampler() const +ompl::base::StateSamplerPtr ModelBasedStateSpace::allocDefaultStateSampler() const { class DefaultStateSampler : public ompl::base::StateSampler { @@ -303,12 +305,12 @@ ompl::base::StateSamplerPtr ompl_interface::ModelBasedStateSpace::allocDefaultSt new DefaultStateSampler(this, spec_.joint_model_group_, &spec_.joint_bounds_))); } -void ompl_interface::ModelBasedStateSpace::printSettings(std::ostream& out) const +void ModelBasedStateSpace::printSettings(std::ostream& out) const { out << "ModelBasedStateSpace '" << getName() << "' at " << this << '\n'; } -void ompl_interface::ModelBasedStateSpace::printState(const ompl::base::State* state, std::ostream& out) const +void ModelBasedStateSpace::printState(const ompl::base::State* state, std::ostream& out) const { for (const moveit::core::JointModel* j : joint_model_vector_) { @@ -338,25 +340,22 @@ void ompl_interface::ModelBasedStateSpace::printState(const ompl::base::State* s out << "Tag: " << state->as()->tag << '\n'; } -void ompl_interface::ModelBasedStateSpace::copyToRobotState(moveit::core::RobotState& rstate, - const ompl::base::State* state) const +void ModelBasedStateSpace::copyToRobotState(moveit::core::RobotState& rstate, const ompl::base::State* state) const { rstate.setJointGroupPositions(spec_.joint_model_group_, state->as()->values); rstate.update(); } -void ompl_interface::ModelBasedStateSpace::copyToOMPLState(ompl::base::State* state, - const moveit::core::RobotState& rstate) const +void ModelBasedStateSpace::copyToOMPLState(ompl::base::State* state, const moveit::core::RobotState& rstate) const { rstate.copyJointGroupPositions(spec_.joint_model_group_, state->as()->values); // clear any cached info (such as validity known or not) state->as()->clearKnownInformation(); } -void ompl_interface::ModelBasedStateSpace::copyJointToOMPLState(ompl::base::State* state, - const moveit::core::RobotState& robot_state, - const moveit::core::JointModel* joint_model, - int ompl_state_joint_index) const +void ModelBasedStateSpace::copyJointToOMPLState(ompl::base::State* state, const moveit::core::RobotState& robot_state, + const moveit::core::JointModel* joint_model, + int ompl_state_joint_index) const { // Copy one joint (multiple variables possibly) memcpy(getValueAddressAtIndex(state, ompl_state_joint_index), @@ -366,3 +365,5 @@ void ompl_interface::ModelBasedStateSpace::copyJointToOMPLState(ompl::base::Stat // clear any cached info (such as validity known or not) state->as()->clearKnownInformation(); } + +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index 6e2223da26..58bb0468c6 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp @@ -36,18 +36,23 @@ #include #include +#include #include namespace ompl_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.pose_model_state_space"); -} // namespace ompl_interface +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ompl_pose_model_state_space"); +} +} // namespace -const std::string ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel"; +const std::string PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel"; -ompl_interface::PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec) - : ModelBasedStateSpace(spec) +PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec) : ModelBasedStateSpace(spec) { jump_factor_ = 3; // \todo make this a param @@ -63,8 +68,8 @@ ompl_interface::PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSp } if (poses_.empty()) { - RCLCPP_ERROR(LOGGER, "No kinematics solvers specified. Unable to construct a " - "PoseModelStateSpace"); + RCLCPP_ERROR(getLogger(), "No kinematics solvers specified. Unable to construct a " + "PoseModelStateSpace"); } else { @@ -73,10 +78,9 @@ ompl_interface::PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSp setName(getName() + "_" + PARAMETERIZATION_TYPE); } -ompl_interface::PoseModelStateSpace::~PoseModelStateSpace() = default; +PoseModelStateSpace::~PoseModelStateSpace() = default; -double ompl_interface::PoseModelStateSpace::distance(const ompl::base::State* state1, - const ompl::base::State* state2) const +double PoseModelStateSpace::distance(const ompl::base::State* state1, const ompl::base::State* state2) const { double total = 0; for (std::size_t i = 0; i < poses_.size(); ++i) @@ -84,7 +88,7 @@ double ompl_interface::PoseModelStateSpace::distance(const ompl::base::State* st return total; } -double ompl_interface::PoseModelStateSpace::getMaximumExtent() const +double PoseModelStateSpace::getMaximumExtent() const { double total = 0.0; for (const auto& pose : poses_) @@ -92,7 +96,7 @@ double ompl_interface::PoseModelStateSpace::getMaximumExtent() const return total; } -ompl::base::State* ompl_interface::PoseModelStateSpace::allocState() const +ompl::base::State* PoseModelStateSpace::allocState() const { auto* state = new StateType(); state->values = @@ -103,7 +107,7 @@ ompl::base::State* ompl_interface::PoseModelStateSpace::allocState() const return state; } -void ompl_interface::PoseModelStateSpace::freeState(ompl::base::State* state) const +void PoseModelStateSpace::freeState(ompl::base::State* state) const { for (std::size_t i = 0; i < poses_.size(); ++i) poses_[i].state_space_->freeState(state->as()->poses[i]); @@ -111,8 +115,7 @@ void ompl_interface::PoseModelStateSpace::freeState(ompl::base::State* state) co ModelBasedStateSpace::freeState(state); } -void ompl_interface::PoseModelStateSpace::copyState(ompl::base::State* destination, - const ompl::base::State* source) const +void PoseModelStateSpace::copyState(ompl::base::State* destination, const ompl::base::State* source) const { // copy the state data ModelBasedStateSpace::copyState(destination, source); @@ -124,14 +127,14 @@ void ompl_interface::PoseModelStateSpace::copyState(ompl::base::State* destinati computeStateK(destination); } -void ompl_interface::PoseModelStateSpace::sanityChecks() const +void PoseModelStateSpace::sanityChecks() const { ModelBasedStateSpace::sanityChecks(std::numeric_limits::epsilon(), std::numeric_limits::epsilon(), ~ompl::base::StateSpace::STATESPACE_TRIANGLE_INEQUALITY); } -void ompl_interface::PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to, - const double t, ompl::base::State* state) const +void PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t, + ompl::base::State* state) const { // we want to interpolate in Cartesian space; we do not have a guarantee that from and to // have their poses computed, but this is very unlikely to happen (depends how the planner gets its input states) @@ -171,8 +174,7 @@ void ompl_interface::PoseModelStateSpace::interpolate(const ompl::base::State* f } } -void ompl_interface::PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, - double minZ, double maxZ) +void PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ) { ModelBasedStateSpace::setPlanningVolume(minX, maxX, minY, maxY, minZ, maxZ); ompl::base::RealVectorBounds b(3); @@ -186,8 +188,8 @@ void ompl_interface::PoseModelStateSpace::setPlanningVolume(double minX, double pose.state_space_->as()->setBounds(b); } -ompl_interface::PoseModelStateSpace::PoseComponent::PoseComponent( - const moveit::core::JointModelGroup* subgroup, const moveit::core::JointModelGroup::KinematicsSolver& k) +PoseModelStateSpace::PoseComponent::PoseComponent(const moveit::core::JointModelGroup* subgroup, + const moveit::core::JointModelGroup::KinematicsSolver& k) : subgroup_(subgroup), kinematics_solver_(k.allocator_(subgroup)), bijection_(k.bijection_) { state_space_ = std::make_shared(); @@ -197,7 +199,7 @@ ompl_interface::PoseModelStateSpace::PoseComponent::PoseComponent( fk_link_[0] = fk_link_[0].substr(1); } -bool ompl_interface::PoseModelStateSpace::PoseComponent::computeStateFK(StateType* full_state, unsigned int idx) const +bool PoseModelStateSpace::PoseComponent::computeStateFK(StateType* full_state, unsigned int idx) const { // read the values from the joint state, in the order expected by the kinematics solver std::vector values(bijection_.size()); @@ -221,7 +223,7 @@ bool ompl_interface::PoseModelStateSpace::PoseComponent::computeStateFK(StateTyp return true; } -bool ompl_interface::PoseModelStateSpace::PoseComponent::computeStateIK(StateType* full_state, unsigned int idx) const +bool PoseModelStateSpace::PoseComponent::computeStateIK(StateType* full_state, unsigned int idx) const { // read the values from the joint state, in the order expected by the kinematics solver; use these as the seed std::vector seed_values(bijection_.size()); @@ -264,7 +266,7 @@ bool ompl_interface::PoseModelStateSpace::PoseComponent::computeStateIK(StateTyp return true; } -bool ompl_interface::PoseModelStateSpace::computeStateFK(ompl::base::State* state) const +bool PoseModelStateSpace::computeStateFK(ompl::base::State* state) const { if (state->as()->poseComputed()) return true; @@ -280,7 +282,7 @@ bool ompl_interface::PoseModelStateSpace::computeStateFK(ompl::base::State* stat return true; } -bool ompl_interface::PoseModelStateSpace::computeStateIK(ompl::base::State* state) const +bool PoseModelStateSpace::computeStateIK(ompl::base::State* state) const { if (state->as()->jointsComputed()) return true; @@ -296,7 +298,7 @@ bool ompl_interface::PoseModelStateSpace::computeStateIK(ompl::base::State* stat return true; } -bool ompl_interface::PoseModelStateSpace::computeStateK(ompl::base::State* state) const +bool PoseModelStateSpace::computeStateK(ompl::base::State* state) const { if (state->as()->jointsComputed() && !state->as()->poseComputed()) return computeStateFK(state); @@ -308,7 +310,7 @@ bool ompl_interface::PoseModelStateSpace::computeStateK(ompl::base::State* state return false; } -ompl::base::StateSamplerPtr ompl_interface::PoseModelStateSpace::allocDefaultStateSampler() const +ompl::base::StateSamplerPtr PoseModelStateSpace::allocDefaultStateSampler() const { class PoseModelStateSampler : public ompl::base::StateSampler { @@ -351,15 +353,12 @@ ompl::base::StateSamplerPtr ompl_interface::PoseModelStateSpace::allocDefaultSta new PoseModelStateSampler(this, ModelBasedStateSpace::allocDefaultStateSampler()))); } -void ompl_interface::PoseModelStateSpace::copyToOMPLState(ompl::base::State* state, - const moveit::core::RobotState& rstate) const +void PoseModelStateSpace::copyToOMPLState(ompl::base::State* state, const moveit::core::RobotState& rstate) const { ModelBasedStateSpace::copyToOMPLState(state, rstate); state->as()->setJointsComputed(true); state->as()->setPoseComputed(false); computeStateFK(state); - /* - std::cout << "COPY STATE IN:\n"; - printState(state, std::cout); - std::cout << "---------- COPY STATE IN\n"; */ } + +} // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index ba2d5949b7..bb435c6a4c 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -36,6 +36,7 @@ #include #include +#include #include @@ -81,7 +82,13 @@ using namespace std::placeholders; namespace ompl_interface { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.planning_context_manager"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ompl_planning_context_manager"); +} +} // namespace struct PlanningContextManager::CachedContexts { @@ -96,8 +103,8 @@ MultiQueryPlannerAllocator::~MultiQueryPlannerAllocator() { ob::PlannerData data(planners_[entry.first]->getSpaceInformation()); planners_[entry.first]->getPlannerData(data); - RCLCPP_INFO_STREAM(LOGGER, "Storing planner data. NumEdges: " << data.numEdges() - << ", NumVertices: " << data.numVertices()); + RCLCPP_INFO_STREAM(getLogger(), "Storing planner data. NumEdges: " << data.numEdges() + << ", NumVertices: " << data.numVertices()); storage_.store(data, entry.second.c_str()); } } @@ -125,8 +132,8 @@ ompl::base::PlannerPtr MultiQueryPlannerAllocator::allocatePlanner(const ob::Spa { ob::PlannerData data(si); planner_map_it->second->getPlannerData(data); - RCLCPP_INFO_STREAM(LOGGER, "Reusing planner data. NumEdges: " << data.numEdges() - << ", NumVertices: " << data.numVertices()); + RCLCPP_INFO_STREAM(getLogger(), "Reusing planner data. NumEdges: " << data.numEdges() + << ", NumVertices: " << data.numVertices()); planners_[planner_map_it->first] = std::shared_ptr{ allocatePersistentPlanner(data) }; return planners_[planner_map_it->first]; } @@ -182,12 +189,12 @@ MultiQueryPlannerAllocator::allocatePlannerImpl(const ob::SpaceInformationPtr& s { ob::PlannerData data(si); storage_.load(file_path.c_str(), data); - RCLCPP_INFO_STREAM(LOGGER, "Loading planner data. NumEdges: " << data.numEdges() - << ", NumVertices: " << data.numVertices()); + RCLCPP_INFO_STREAM(getLogger(), "Loading planner data. NumEdges: " << data.numEdges() + << ", NumVertices: " << data.numVertices()); planner = std::shared_ptr{ allocatePersistentPlanner(data) }; if (!planner) { - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "Creating a '%s' planner from persistent data is not supported. Going to create a new instance.", new_name.c_str()); } @@ -271,7 +278,7 @@ ConfiguredPlannerAllocator PlanningContextManager::plannerSelector(const std::st } else { - RCLCPP_ERROR(LOGGER, "Unknown planner: '%s'", planner.c_str()); + RCLCPP_ERROR(getLogger(), "Unknown planner: '%s'", planner.c_str()); return ConfiguredPlannerAllocator(); } } @@ -348,7 +355,7 @@ PlanningContextManager::getPlanningContext(const planning_interface::PlannerConf { if (cached_context.unique()) { - RCLCPP_DEBUG(LOGGER, "Reusing cached planning context"); + RCLCPP_DEBUG(getLogger(), "Reusing cached planning context"); context = cached_context; break; } @@ -368,7 +375,7 @@ PlanningContextManager::getPlanningContext(const planning_interface::PlannerConf if (factory->getType() == ConstrainedPlanningStateSpace::PARAMETERIZATION_TYPE) { - RCLCPP_DEBUG_STREAM(LOGGER, "planning_context_manager: Using OMPL's constrained state space for planning."); + RCLCPP_DEBUG_STREAM(getLogger(), "planning_context_manager: Using OMPL's constrained state space for planning."); // Select the correct type of constraints based on the path constraints in the planning request. ompl::base::ConstraintPtr ompl_constraint = @@ -391,7 +398,7 @@ PlanningContextManager::getPlanningContext(const planning_interface::PlannerConf context_spec.ompl_simple_setup_ = std::make_shared(context_spec.state_space_); } - RCLCPP_DEBUG(LOGGER, "Creating new planning context"); + RCLCPP_DEBUG(getLogger(), "Creating new planning context"); context = std::make_shared(config.name, context_spec); // Do not cache a constrained planning context, as the constraints could be changed @@ -426,12 +433,12 @@ const ModelBasedStateSpaceFactoryPtr& PlanningContextManager::getStateSpaceFacto auto f = factory_type.empty() ? state_space_factories_.begin() : state_space_factories_.find(factory_type); if (f != state_space_factories_.end()) { - RCLCPP_DEBUG(LOGGER, "Using '%s' parameterization for solving problem", factory_type.c_str()); + RCLCPP_DEBUG(getLogger(), "Using '%s' parameterization for solving problem", factory_type.c_str()); return f->second; } else { - RCLCPP_ERROR(LOGGER, "Factory of type '%s' was not found", factory_type.c_str()); + RCLCPP_ERROR(getLogger(), "Factory of type '%s' was not found", factory_type.c_str()); static const ModelBasedStateSpaceFactoryPtr EMPTY; return EMPTY; } @@ -456,14 +463,14 @@ PlanningContextManager::getStateSpaceFactory(const std::string& group, if (best == state_space_factories_.end()) { - RCLCPP_ERROR(LOGGER, "There are no known state spaces that can represent the given planning " - "problem"); + RCLCPP_ERROR(getLogger(), "There are no known state spaces that can represent the given planning " + "problem"); static const ModelBasedStateSpaceFactoryPtr EMPTY; return EMPTY; } else { - RCLCPP_DEBUG(LOGGER, "Using '%s' parameterization for solving problem", best->first.c_str()); + RCLCPP_DEBUG(getLogger(), "Using '%s' parameterization for solving problem", best->first.c_str()); return best->second; } } @@ -475,7 +482,7 @@ ModelBasedPlanningContextPtr PlanningContextManager::getPlanningContext( { if (req.group_name.empty()) { - RCLCPP_ERROR(LOGGER, "No group specified to plan for"); + RCLCPP_ERROR(getLogger(), "No group specified to plan for"); error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; return ModelBasedPlanningContextPtr(); } @@ -484,7 +491,7 @@ ModelBasedPlanningContextPtr PlanningContextManager::getPlanningContext( if (!planning_scene) { - RCLCPP_ERROR(LOGGER, "No planning scene supplied as input"); + RCLCPP_ERROR(getLogger(), "No planning scene supplied as input"); return ModelBasedPlanningContextPtr(); } @@ -497,7 +504,7 @@ ModelBasedPlanningContextPtr PlanningContextManager::getPlanningContext( req.planner_id); if (pc == planner_configs_.end()) { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "Cannot find planning configuration for group '%s' using planner '%s'. Will use defaults instead.", req.group_name.c_str(), req.planner_id.c_str()); } @@ -508,7 +515,7 @@ ModelBasedPlanningContextPtr PlanningContextManager::getPlanningContext( pc = planner_configs_.find(req.group_name); if (pc == planner_configs_.end()) { - RCLCPP_ERROR(LOGGER, "Cannot find planning configuration for group '%s'", req.group_name.c_str()); + RCLCPP_ERROR(getLogger(), "Cannot find planning configuration for group '%s'", req.group_name.c_str()); return ModelBasedPlanningContextPtr(); } } @@ -591,12 +598,12 @@ ModelBasedPlanningContextPtr PlanningContextManager::getPlanningContext( try { context->configure(node, use_constraints_approximation); - RCLCPP_DEBUG(LOGGER, "%s: New planning context is set.", context->getName().c_str()); + RCLCPP_DEBUG(getLogger(), "%s: New planning context is set.", context->getName().c_str()); error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; } catch (ompl::Exception& ex) { - RCLCPP_ERROR(LOGGER, "OMPL encountered an error: %s", ex.what()); + RCLCPP_ERROR(getLogger(), "OMPL encountered an error: %s", ex.what()); context.reset(); } } diff --git a/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp index 43c21f8f5b..7f96dacf0b 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -58,8 +59,10 @@ #include "load_test_robot.h" -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.ompl_planning.test.test_constrained_planning_state_space"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("test_constrained_planning_state_space"); +} /** \brief Dummy constraint for testing, always satisfied. We need this to create and OMPL ConstrainedStateSpace. **/ class DummyConstraint : public ompl::base::Constraint @@ -215,9 +218,9 @@ class TestConstrainedStateSpace : public ompl_interface_testing::LoadTestRobot, const moveit::core::JointModel* joint_model = joint_model_group_->getJointModel(joint_model_names[joint_index]); EXPECT_FALSE(joint_model == nullptr); - RCLCPP_DEBUG_STREAM(LOGGER, "Joint model: " << joint_model->getName() << " index: " << joint_index); - RCLCPP_DEBUG_STREAM(LOGGER, "first index: " << joint_model->getFirstVariableIndex() * sizeof(double)); - RCLCPP_DEBUG_STREAM(LOGGER, "width: " << joint_model->getVariableCount() * sizeof(double)); + RCLCPP_DEBUG_STREAM(getLogger(), "Joint model: " << joint_model->getName() << " index: " << joint_index); + RCLCPP_DEBUG_STREAM(getLogger(), "first index: " << joint_model->getFirstVariableIndex() * sizeof(double)); + RCLCPP_DEBUG_STREAM(getLogger(), "width: " << joint_model->getVariableCount() * sizeof(double)); moveit_state_space_->copyJointToOMPLState(ompl_state.get(), moveit_state, joint_model, joint_index); } diff --git a/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp index 7d00d2d862..a37943c042 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -55,8 +56,10 @@ /** \brief This flag sets the verbosity level for the state validity checker. **/ constexpr bool VERBOSE = false; -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.ompl_planning.test.test_constrained_state_validity_checker"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("test_constrained_state_validity_checker"); +} /** \brief Pretty print std:vectors **/ std::ostream& operator<<(std::ostream& os, const std::vector& v) @@ -116,7 +119,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p ->getState() ->as(); - RCLCPP_DEBUG_STREAM(LOGGER, + RCLCPP_DEBUG_STREAM(getLogger(), std::vector(state->values, state->values + joint_model_group_->getVariableCount())); // assume the default position in not in self-collision @@ -128,7 +131,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p double distance = 0.0; result = checker->isValid(ompl_state.get(), distance); - RCLCPP_DEBUG(LOGGER, "Distance from the isValid function '%f': ", distance); + RCLCPP_DEBUG(getLogger(), "Distance from the isValid function '%f': ", distance); EXPECT_TRUE(result); EXPECT_GT(distance, 0.0); @@ -136,7 +139,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p state->values[0] = std::numeric_limits::max(); state->clearKnownInformation(); // make sure the validity checker does not use the cached value - RCLCPP_DEBUG_STREAM(LOGGER, + RCLCPP_DEBUG_STREAM(getLogger(), std::vector(state->values, state->values + joint_model_group_->getVariableCount())); bool result_2 = checker->isValid(ompl_state.get()); @@ -172,7 +175,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p // ompl_state.reals() throws a segmentation fault for this state type // use a more involved conversion to std::vector for logging - RCLCPP_DEBUG_STREAM(LOGGER, + RCLCPP_DEBUG_STREAM(getLogger(), std::vector(state->values, state->values + joint_model_group_->getVariableCount())); // the given state is known to be in self-collision, we check it here @@ -238,7 +241,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p planning_context_->setPlanningScene(planning_scene_); planning_context_->setCompleteInitialState(*initial_robot_state_); - RCLCPP_DEBUG(LOGGER, "Planning context with name '%s' is ready (but not configured).", + RCLCPP_DEBUG(getLogger(), "Planning context with name '%s' is ready (but not configured).", planning_context_->getName().c_str()); } diff --git a/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp index 1d14d76439..1042c69a5c 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp @@ -56,13 +56,17 @@ #include #include #include +#include #include #include #include #include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_ompl_constraints"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("test_ompl_constraints"); +} /** \brief Number of times to run a test that uses randomly generated input. **/ constexpr int NUM_RANDOM_TESTS = 10; @@ -163,7 +167,7 @@ class TestOMPLConstraints : public ompl_interface_testing::LoadTestRobot, public { std::string different_link = joint_model_group_->getLinkModelNames().at(num_dofs_ - DIFFERENT_LINK_OFFSET); - RCLCPP_DEBUG_STREAM(LOGGER, different_link); + RCLCPP_DEBUG_STREAM(getLogger(), different_link); moveit_msgs::msg::Constraints constraint_msgs; constraint_msgs.position_constraints.push_back(createPositionConstraint(base_link_name_, different_link)); @@ -207,10 +211,10 @@ class TestOMPLConstraints : public ompl_interface_testing::LoadTestRobot, public Eigen::MatrixXd jac_approx = numericalJacobianPosition(q, constraint_->getLinkName()); - RCLCPP_DEBUG_STREAM(LOGGER, "Analytical jacobian:"); - RCLCPP_DEBUG_STREAM(LOGGER, jac_exact); - RCLCPP_DEBUG_STREAM(LOGGER, "Finite difference jacobian:"); - RCLCPP_DEBUG_STREAM(LOGGER, jac_approx); + RCLCPP_DEBUG_STREAM(getLogger(), "Analytical jacobian:"); + RCLCPP_DEBUG_STREAM(getLogger(), jac_exact); + RCLCPP_DEBUG_STREAM(getLogger(), "Finite difference jacobian:"); + RCLCPP_DEBUG_STREAM(getLogger(), jac_approx); total_error = (jac_exact - jac_approx).lpNorm<1>(); EXPECT_LT(total_error, JAC_ERROR_TOLERANCE); @@ -255,7 +259,7 @@ class TestOMPLConstraints : public ompl_interface_testing::LoadTestRobot, public } catch (ompl::Exception& ex) { - RCLCPP_ERROR(LOGGER, "Sanity checks did not pass: %s", ex.what()); + RCLCPP_ERROR(getLogger(), "Sanity checks did not pass: %s", ex.what()); } } diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp index c16cc909ed..87a0d7b275 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -64,8 +64,7 @@ #include #include #include - -// static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_planning_context_manager"); +#include /** \brief Generic implementation of the tests that can be executed on different robots. **/ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public testing::Test @@ -74,6 +73,7 @@ class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public TestPlanningContext(const std::string& robot_name, const std::string& group_name) : LoadTestRobot(robot_name, group_name), node_(std::make_shared("planning_context_manager_test")) { + moveit::setNodeLoggerName(node_->get_name()); } void testSimpleRequest(const std::vector& start, const std::vector& goal) diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp index 5e54df17d3..cd62ba78f3 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp @@ -44,10 +44,14 @@ #include #include #include +#include #include #include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_state_space"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("test_state_space"); +} constexpr double EPSILON = std::numeric_limits::epsilon(); @@ -83,7 +87,7 @@ TEST_F(LoadPlanningModelsPr2, StateSpace) } catch (ompl::Exception& ex) { - RCLCPP_ERROR(LOGGER, "Sanity checks did not pass: %s", ex.what()); + RCLCPP_ERROR(getLogger(), "Sanity checks did not pass: %s", ex.what()); } EXPECT_TRUE(passed); } @@ -126,7 +130,7 @@ TEST_F(LoadPlanningModelsPr2, StateSpaceCopy) } catch (ompl::Exception& ex) { - RCLCPP_ERROR(LOGGER, "Sanity checks did not pass: %s", ex.what()); + RCLCPP_ERROR(getLogger(), "Sanity checks did not pass: %s", ex.what()); } EXPECT_TRUE(passed); @@ -207,7 +211,7 @@ TEST(TestDiffDrive, TestStateSpace) } catch (ompl::Exception& ex) { - RCLCPP_ERROR(LOGGER, "Sanity checks did not pass: %s", ex.what()); + RCLCPP_ERROR(getLogger(), "Sanity checks did not pass: %s", ex.what()); } EXPECT_TRUE(passed); } diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp index ff907f50f9..2a774e6e7b 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp @@ -59,13 +59,17 @@ #include #include #include +#include #include /** \brief This flag sets the verbosity level for the state validity checker. **/ constexpr bool VERBOSE = false; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_state_validity_checker"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("test_state_validity_checker"); +} /** \brief Pretty print std:vectors **/ std::ostream& operator<<(std::ostream& os, const std::vector& v) @@ -108,7 +112,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p ompl::base::ScopedState<> ompl_state(state_space_); state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); - RCLCPP_DEBUG_STREAM(LOGGER, ompl_state.reals()); + RCLCPP_DEBUG_STREAM(getLogger(), ompl_state.reals()); // assume the given position is not in self-collision // and there are no collision objects or path constraints so this state should be valid @@ -118,7 +122,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p ompl_state->as()->values[0] = std::numeric_limits::max(); ompl_state->as()->clearKnownInformation(); - RCLCPP_DEBUG_STREAM(LOGGER, ompl_state.reals()); + RCLCPP_DEBUG_STREAM(getLogger(), ompl_state.reals()); EXPECT_FALSE(checker->isValid(ompl_state.get())); } @@ -139,7 +143,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p ompl::base::ScopedState<> ompl_state(state_space_); state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); - RCLCPP_DEBUG_STREAM(LOGGER, ompl_state.reals()); + RCLCPP_DEBUG_STREAM(getLogger(), ompl_state.reals()); // the given state is known to be in self-collision, we check it here EXPECT_FALSE(checker->isValid(ompl_state.get())); @@ -175,7 +179,7 @@ class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, p ompl::base::ScopedState<> ompl_state(state_space_); state_space_->copyToOMPLState(ompl_state.get(), *robot_state_); - RCLCPP_DEBUG_STREAM(LOGGER, ompl_state.reals()); + RCLCPP_DEBUG_STREAM(getLogger(), ompl_state.reals()); EXPECT_TRUE(checker->isValid(ompl_state.get())); diff --git a/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp b/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp index 483d43a5ef..fd6711682d 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp @@ -42,8 +42,6 @@ The skeleton of this test was taken from test_state_validity_checker.cpp by Jero #include #include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_thread_safe_storage"); - /** \brief Generic implementation of the tests that can be executed on different robots. **/ class TestThreadSafeStateStorage : public ompl_interface_testing::LoadTestRobot, public testing::Test { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index d8fe742772..9443116576 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -42,6 +42,7 @@ #include #include +#include #include "cartesian_limits_parameters.hpp" #include @@ -54,7 +55,10 @@ namespace pilz_industrial_motion_planner namespace { const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.command_list_manager"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_command_list_manager"); +} } // namespace CommandListManager::CommandListManager(const rclcpp::Node::SharedPtr& node, @@ -194,16 +198,16 @@ bool CommandListManager::isInvalidBlendRadii(const moveit::core::RobotModel& mod // No blending between different groups if (item_A.req.group_name != item_B.req.group_name) { - RCLCPP_WARN_STREAM(LOGGER, "Blending between different groups (in this case: \"" - << item_A.req.group_name << "\" and \"" << item_B.req.group_name - << "\") not allowed"); + RCLCPP_WARN_STREAM(getLogger(), "Blending between different groups (in this case: \"" + << item_A.req.group_name << "\" and \"" << item_B.req.group_name + << "\") not allowed"); return true; } // No blending for groups without solver if (!hasSolver(model.getJointModelGroup(item_A.req.group_name))) { - RCLCPP_WARN_STREAM(LOGGER, "Blending for groups without solver not allowed"); + RCLCPP_WARN_STREAM(getLogger(), "Blending for groups without solver not allowed"); return true; } @@ -219,8 +223,8 @@ CommandListManager::extractBlendRadii(const moveit::core::RobotModel& model, { if (isInvalidBlendRadii(model, req_list.items.at(i), req_list.items.at(i + 1))) { - RCLCPP_WARN_STREAM(LOGGER, "Invalid blend radii between commands: [" << i << "] and [" << i + 1 - << "] => Blend radii set to zero"); + RCLCPP_WARN_STREAM(getLogger(), "Invalid blend radii between commands: [" << i << "] and [" << i + 1 + << "] => Blend radii set to zero"); continue; } radii.at(i) = req_list.items.at(i).blend_radius; @@ -244,7 +248,7 @@ CommandListManager::solveSequenceItems(const planning_scene::PlanningSceneConstP planning_interface::MotionPlanResponse res; if (!planning_pipeline->generatePlan(planning_scene, req, res)) { - RCLCPP_ERROR(LOGGER, "Generating a plan with planning pipeline failed."); + RCLCPP_ERROR(getLogger(), "Generating a plan with planning pipeline failed."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; } if (res.error_code.val != res.error_code.SUCCESS) @@ -254,7 +258,7 @@ CommandListManager::solveSequenceItems(const planning_scene::PlanningSceneConstP throw PlanningPipelineException(os.str(), res.error_code.val); } motion_plan_responses.emplace_back(res); - RCLCPP_DEBUG_STREAM(LOGGER, "Solved [" << ++curr_req_index << '/' << num_req << ']'); + RCLCPP_DEBUG_STREAM(getLogger(), "Solved [" << ++curr_req_index << '/' << num_req << ']'); } return motion_plan_responses; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp index feb0a49525..be1ead4996 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp @@ -37,12 +37,16 @@ #include #include +#include #include namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.joint_limits_aggregator"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_joint_limits_aggregator"); } +} // namespace pilz_industrial_motion_planner::JointLimitsContainer pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( @@ -51,7 +55,7 @@ pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits( { JointLimitsContainer container; - RCLCPP_INFO_STREAM(LOGGER, "Reading limits from namespace " << param_namespace); + RCLCPP_INFO_STREAM(getLogger(), "Reading limits from namespace " << param_namespace); // Iterate over all joint models and generate the map for (auto joint_model : joint_models) @@ -115,7 +119,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF { // LCOV_EXCL_START case 0: - RCLCPP_WARN_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName()); + RCLCPP_WARN_STREAM(getLogger(), "no bounds set for joint " << joint_model->getName()); break; // LCOV_EXCL_STOP case 1: @@ -125,7 +129,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF break; // LCOV_EXCL_START default: - RCLCPP_WARN_STREAM(LOGGER, "Multi-DOF-Joint '" << joint_model->getName() << "' not supported."); + RCLCPP_WARN_STREAM(getLogger(), "Multi-DOF-Joint '" << joint_model->getName() << "' not supported."); joint_limit.has_position_limits = true; joint_limit.min_position = 0; joint_limit.max_position = 0; @@ -133,8 +137,8 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF // LCOV_EXCL_STOP } - RCLCPP_DEBUG_STREAM(LOGGER, "Limit(" << joint_model->getName() << " min:" << joint_limit.min_position - << " max:" << joint_limit.max_position); + RCLCPP_DEBUG_STREAM(getLogger(), "Limit(" << joint_model->getName() << " min:" << joint_limit.min_position + << " max:" << joint_limit.max_position); } void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitFromJointModel( @@ -144,7 +148,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF { // LCOV_EXCL_START case 0: - RCLCPP_WARN_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName()); + RCLCPP_WARN_STREAM(getLogger(), "no bounds set for joint " << joint_model->getName()); break; // LCOV_EXCL_STOP case 1: @@ -153,7 +157,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF break; // LCOV_EXCL_START default: - RCLCPP_WARN_STREAM(LOGGER, "Multi-DOF-Joint '" << joint_model->getName() << "' not supported."); + RCLCPP_WARN_STREAM(getLogger(), "Multi-DOF-Joint '" << joint_model->getName() << "' not supported."); joint_limit.has_velocity_limits = true; joint_limit.max_velocity = 0; break; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp index 79a9584387..bcde37c223 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp @@ -38,25 +38,29 @@ #include #include #include +#include namespace pilz_industrial_motion_planner { namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.joint_limits_container"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_joint_limits_container"); } +} // namespace bool JointLimitsContainer::addLimit(const std::string& joint_name, pilz_industrial_motion_planner::JointLimit joint_limit) { if (joint_limit.has_deceleration_limits && joint_limit.max_deceleration >= 0) { - RCLCPP_ERROR_STREAM(LOGGER, "joint_limit.max_deceleration MUST be negative!"); + RCLCPP_ERROR_STREAM(getLogger(), "joint_limit.max_deceleration MUST be negative!"); return false; } const auto& insertion_result{ container_.insert(std::pair(joint_name, joint_limit)) }; if (!insertion_result.second) { - RCLCPP_ERROR_STREAM(LOGGER, "joint_limit for joint " << joint_name << " already contained."); + RCLCPP_ERROR_STREAM(getLogger(), "joint_limit for joint " << joint_name << " already contained."); return false; } return true; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp index 4784ef145a..537e487e36 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp @@ -34,13 +34,17 @@ #include #include +#include namespace pilz_industrial_motion_planner { namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("pilz_industrial_motion_planner.limits_container"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_limits_container"); } +} // namespace LimitsContainer::LimitsContainer() : has_joint_limits_(false), has_cartesian_limits_(false) { @@ -64,7 +68,7 @@ const JointLimitsContainer& LimitsContainer::getJointLimitContainer() const void LimitsContainer::printCartesianLimits() const { - RCLCPP_DEBUG(LOGGER, + RCLCPP_DEBUG(getLogger(), "Pilz Cartesian Limits - Max Trans Vel : %f, Max Trans Acc : %f, Max Trans Dec : %f, Max Rot Vel : %f", cartesian_limits_.max_trans_vel, cartesian_limits_.max_trans_acc, cartesian_limits_.max_trans_dec, cartesian_limits_.max_rot_vel); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp index 691aa22dbc..c78fcf2be0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include @@ -55,8 +56,11 @@ namespace pilz_industrial_motion_planner { namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.move_group_sequence_action"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_move_group_sequence_action"); } +} // namespace MoveGroupSequenceAction::MoveGroupSequenceAction() : MoveGroupCapability("SequenceAction") @@ -67,23 +71,23 @@ MoveGroupSequenceAction::MoveGroupSequenceAction() void MoveGroupSequenceAction::initialize() { // start the move action server - RCLCPP_INFO_STREAM(LOGGER, "initialize move group sequence action"); + RCLCPP_INFO_STREAM(getLogger(), "initialize move group sequence action"); action_callback_group_ = context_->moveit_cpp_->getNode()->create_callback_group(rclcpp::CallbackGroupType::Reentrant); move_action_server_ = rclcpp_action::create_server( context_->moveit_cpp_->getNode(), "sequence_move_group", [](const rclcpp_action::GoalUUID& /* unused */, const std::shared_ptr& /* unused */) { - RCLCPP_DEBUG(LOGGER, "Received action goal"); + RCLCPP_DEBUG(getLogger(), "Received action goal"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, [this](const std::shared_ptr& /* unused goal_handle */) { - RCLCPP_DEBUG(LOGGER, "Canceling action goal"); + RCLCPP_DEBUG(getLogger(), "Canceling action goal"); preemptMoveCallback(); return rclcpp_action::CancelResponse::ACCEPT; }, [this](const std::shared_ptr& goal_handle) { - RCLCPP_DEBUG(LOGGER, "Accepting new action goal"); + RCLCPP_DEBUG(getLogger(), "Accepting new action goal"); executeSequenceCallback(goal_handle); }, rcl_action_server_get_default_options(), action_callback_group_); @@ -103,7 +107,7 @@ void MoveGroupSequenceAction::executeSequenceCallback(const std::shared_ptrrequest.items.empty()) { - RCLCPP_WARN(LOGGER, "Received empty request. That's ok but maybe not what you intended."); + RCLCPP_WARN(getLogger(), "Received empty request. That's ok but maybe not what you intended."); setMoveState(move_group::IDLE); const auto action_res = std::make_shared(); action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; @@ -122,7 +126,7 @@ void MoveGroupSequenceAction::executeSequenceCallback(const std::shared_ptrplanning_options.plan_only) { - RCLCPP_WARN(LOGGER, "Only plan will be calculated, although plan_only == false."); // LCOV_EXCL_LINE + RCLCPP_WARN(getLogger(), "Only plan will be calculated, although plan_only == false."); // LCOV_EXCL_LINE } executeMoveCallbackPlanOnly(goal, action_res); } @@ -152,7 +156,7 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal, const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& action_res) { - RCLCPP_INFO(LOGGER, "Combined planning and execution request received for MoveGroupSequenceAction."); + RCLCPP_INFO(getLogger(), "Combined planning and execution request received for MoveGroupSequenceAction."); plan_execution::PlanExecution::Options opt; const moveit_msgs::msg::PlanningScene& planning_scene_diff = @@ -180,7 +184,7 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute( } catch (std::out_of_range&) { - RCLCPP_WARN(LOGGER, "Can not determine start state from empty sequence."); + RCLCPP_WARN(getLogger(), "Can not determine start state from empty sequence."); } action_res->response.error_code = plan.error_code; } @@ -201,7 +205,7 @@ void MoveGroupSequenceAction::executeMoveCallbackPlanOnly( const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal, const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& action_res) { - RCLCPP_INFO(LOGGER, "Planning request received for MoveGroupSequenceAction action."); + RCLCPP_INFO(getLogger(), "Planning request received for MoveGroupSequenceAction action."); // lock the scene so that it does not modify the world representation while // diff() is called @@ -222,7 +226,7 @@ void MoveGroupSequenceAction::executeMoveCallbackPlanOnly( resolvePlanningPipeline(goal->request.items[0].req.pipeline_id); if (!planning_pipeline) { - RCLCPP_ERROR_STREAM(LOGGER, "Could not load planning pipeline " << goal->request.items[0].req.pipeline_id); + RCLCPP_ERROR_STREAM(getLogger(), "Could not load planning pipeline " << goal->request.items[0].req.pipeline_id); action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return; } @@ -231,15 +235,15 @@ void MoveGroupSequenceAction::executeMoveCallbackPlanOnly( } catch (const MoveItErrorCodeException& ex) { - RCLCPP_ERROR_STREAM(LOGGER, "> Planning pipeline threw an exception (error code: " << ex.getErrorCode() - << "): " << ex.what()); + RCLCPP_ERROR_STREAM(getLogger(), "> Planning pipeline threw an exception (error code: " << ex.getErrorCode() + << "): " << ex.what()); action_res->response.error_code.val = ex.getErrorCode(); return; } // LCOV_EXCL_START // Keep moveit up even if lower parts throw catch (const std::exception& ex) { - RCLCPP_ERROR(LOGGER, "Planning pipeline threw an exception: %s", ex.what()); + RCLCPP_ERROR(getLogger(), "Planning pipeline threw an exception: %s", ex.what()); action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return; } @@ -259,7 +263,7 @@ void MoveGroupSequenceAction::executeMoveCallbackPlanOnly( } catch (std::out_of_range&) { - RCLCPP_WARN(LOGGER, "Can not determine start state from empty sequence."); + RCLCPP_WARN(getLogger(), "Can not determine start state from empty sequence."); } action_res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; @@ -281,7 +285,7 @@ bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::msg::M resolvePlanningPipeline(req.items[0].req.pipeline_id); if (!planning_pipeline) { - RCLCPP_ERROR_STREAM(LOGGER, "Could not load planning pipeline " << req.items[0].req.pipeline_id); + RCLCPP_ERROR_STREAM(getLogger(), "Could not load planning pipeline " << req.items[0].req.pipeline_id); return false; } @@ -289,15 +293,15 @@ bool MoveGroupSequenceAction::planUsingSequenceManager(const moveit_msgs::msg::M } catch (const MoveItErrorCodeException& ex) { - RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline threw an exception (error code: " << ex.getErrorCode() - << "): " << ex.what()); + RCLCPP_ERROR_STREAM(getLogger(), "Planning pipeline threw an exception (error code: " << ex.getErrorCode() + << "): " << ex.what()); plan.error_code.val = ex.getErrorCode(); return false; } // LCOV_EXCL_START // Keep MoveIt up even if lower parts throw catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(LOGGER, "Planning pipeline threw an exception: " << ex.what()); + RCLCPP_ERROR_STREAM(getLogger(), "Planning pipeline threw an exception: " << ex.what()); plan.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp index 44d7405511..0004b40cfb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp @@ -43,12 +43,17 @@ #include #include +#include + namespace pilz_industrial_motion_planner { namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.move_group_sequence_service"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_move_group_sequence_service"); } +} // namespace MoveGroupSequenceService::MoveGroupSequenceService() : MoveGroupCapability("SequenceService") { } @@ -74,7 +79,7 @@ bool MoveGroupSequenceService::plan(const moveit_msgs::srv::GetMotionSequence::R // Handle empty requests if (req->request.items.empty()) { - RCLCPP_WARN(LOGGER, "Received empty request. That's ok but maybe not what you intended."); + RCLCPP_WARN(getLogger(), "Received empty request. That's ok but maybe not what you intended."); res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; return true; } @@ -93,7 +98,7 @@ bool MoveGroupSequenceService::plan(const moveit_msgs::srv::GetMotionSequence::R resolvePlanningPipeline(req->request.items[0].req.pipeline_id); if (!planning_pipeline) { - RCLCPP_ERROR_STREAM(LOGGER, "Could not load planning pipeline " << req->request.items[0].req.pipeline_id); + RCLCPP_ERROR_STREAM(getLogger(), "Could not load planning pipeline " << req->request.items[0].req.pipeline_id); res->response.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; return false; } @@ -102,14 +107,15 @@ bool MoveGroupSequenceService::plan(const moveit_msgs::srv::GetMotionSequence::R } catch (const MoveItErrorCodeException& ex) { - RCLCPP_ERROR_STREAM(LOGGER, "Planner threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); + RCLCPP_ERROR_STREAM(getLogger(), + "Planner threw an exception (error code: " << ex.getErrorCode() << "): " << ex.what()); res->response.error_code.val = ex.getErrorCode(); return true; } // LCOV_EXCL_START // Keep moveit up even if lower parts throw catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(LOGGER, "Planner threw an exception: " << ex.what()); + RCLCPP_ERROR_STREAM(getLogger(), "Planner threw an exception: " << ex.what()); // If 'FALSE' then no response will be sent to the caller. return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp index cdadff31f2..f57118636c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp @@ -48,13 +48,17 @@ #include #include +#include namespace pilz_industrial_motion_planner { namespace { const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_industrial_motion_planner"); +} } // namespace bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, const rclcpp::Node::SharedPtr& node, @@ -88,12 +92,12 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c ss << factory << ' '; } - RCLCPP_INFO_STREAM(LOGGER, "Available plugins: " << ss.str()); + RCLCPP_INFO_STREAM(getLogger(), "Available plugins: " << ss.str()); // Load each factory for (const auto& factory : factories) { - RCLCPP_INFO_STREAM(LOGGER, "About to load: " << factory); + RCLCPP_INFO_STREAM(getLogger(), "About to load: " << factory); PlanningContextLoaderPtr loader_pointer(planner_context_loader_->createSharedInstance(factory)); pilz_industrial_motion_planner::LimitsContainer limits; @@ -130,14 +134,14 @@ CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr& moveit_msgs::msg::MoveItErrorCodes& error_code) const { // TODO(henningkayser): print req - // RCLCPP_DEBUG_STREAM(LOGGER, "Loading PlanningContext for request\n\n" << req << "\n"); - RCLCPP_DEBUG(LOGGER, "Loading PlanningContext"); + // RCLCPP_DEBUG_STREAM(getLogger(), "Loading PlanningContext for request\n\n" << req << "\n"); + RCLCPP_DEBUG(getLogger(), "Loading PlanningContext"); // Check that a loaded for this request exists if (!canServiceRequest(req)) { - RCLCPP_ERROR_STREAM(LOGGER, "No ContextLoader for planner_id '" << req.planner_id.c_str() - << "' found. Planning not possible."); + RCLCPP_ERROR_STREAM(getLogger(), "No ContextLoader for planner_id '" << req.planner_id.c_str() + << "' found. Planning not possible."); return nullptr; } @@ -145,7 +149,8 @@ CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr& if (context_loader_map_.at(req.planner_id)->loadContext(planning_context, req.planner_id, req.group_name)) { - RCLCPP_DEBUG_STREAM(LOGGER, "Found planning context loader for " << req.planner_id << " group:" << req.group_name); + RCLCPP_DEBUG_STREAM(getLogger(), + "Found planning context loader for " << req.planner_id << " group:" << req.group_name); planning_context->setMotionPlanRequest(req); planning_context->setPlanningScene(planning_scene); return planning_context; @@ -161,34 +166,36 @@ bool CommandPlanner::canServiceRequest(const moveit_msgs::msg::MotionPlanRequest { if (context_loader_map_.find(req.planner_id) == context_loader_map_.end()) { - RCLCPP_ERROR(LOGGER, "Cannot service planning request because planner ID '%s' does not exist.", + RCLCPP_ERROR(getLogger(), "Cannot service planning request because planner ID '%s' does not exist.", req.planner_id.c_str()); return false; } if (req.group_name.empty()) { - RCLCPP_ERROR(LOGGER, "Cannot service planning request because group name is not specified."); + RCLCPP_ERROR(getLogger(), "Cannot service planning request because group name is not specified."); return false; } auto joint_mode_group_ptr = model_->getJointModelGroup(req.group_name); if (joint_mode_group_ptr == nullptr) { - RCLCPP_ERROR(LOGGER, "Cannot service planning request because group '%s' does not exist.", req.group_name.c_str()); + RCLCPP_ERROR(getLogger(), "Cannot service planning request because group '%s' does not exist.", + req.group_name.c_str()); return false; } if (joint_mode_group_ptr->getSolverInstance() == nullptr) { - RCLCPP_ERROR(LOGGER, "Cannot service planning request because group '%s' does have an IK solver instance.", + RCLCPP_ERROR(getLogger(), "Cannot service planning request because group '%s' does have an IK solver instance.", req.group_name.c_str()); return false; } if (!req.trajectory_constraints.constraints.empty()) { - RCLCPP_ERROR(LOGGER, "Cannot service planning request because PILZ does not support 'trajectory constraints'."); + RCLCPP_ERROR(getLogger(), + "Cannot service planning request because PILZ does not support 'trajectory constraints'."); return false; } @@ -202,7 +209,7 @@ void CommandPlanner::registerContextLoader( if (context_loader_map_.find(planning_context_loader->getAlgorithm()) == context_loader_map_.end()) { context_loader_map_[planning_context_loader->getAlgorithm()] = planning_context_loader; - RCLCPP_INFO_STREAM(LOGGER, "Registered Algorithm [" << planning_context_loader->getAlgorithm() << ']'); + RCLCPP_INFO_STREAM(getLogger(), "Registered Algorithm [" << planning_context_loader->getAlgorithm() << ']'); } else { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp index dfe23638c4..ac72891536 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp @@ -39,13 +39,17 @@ #include #include #include +#include #include namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_circ"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_planning_context_loader_circ"); } +} // namespace pilz_industrial_motion_planner::PlanningContextLoaderCIRC::PlanningContextLoaderCIRC() { @@ -68,11 +72,12 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderCIRC::loadContext( { if (!limits_set_) { - RCLCPP_ERROR_STREAM(LOGGER, "Limits are not defined. Cannot load planning context. Call setLimits loadContext"); + RCLCPP_ERROR_STREAM(getLogger(), + "Limits are not defined. Cannot load planning context. Call setLimits loadContext"); } if (!model_set_) { - RCLCPP_ERROR_STREAM(LOGGER, "Robot model was not set"); + RCLCPP_ERROR_STREAM(getLogger(), "Robot model was not set"); } return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp index 08c25e1a28..0e0c11b859 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp @@ -36,13 +36,17 @@ #include #include #include +#include #include namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_lin"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_planning_context_loader_lin"); } +} // namespace pilz_industrial_motion_planner::PlanningContextLoaderLIN::PlanningContextLoaderLIN() { @@ -65,11 +69,12 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderLIN::loadContext( { if (!limits_set_) { - RCLCPP_ERROR_STREAM(LOGGER, "Limits are not defined. Cannot load planning context. Call setLimits loadContext"); + RCLCPP_ERROR_STREAM(getLogger(), + "Limits are not defined. Cannot load planning context. Call setLimits loadContext"); } if (!model_set_) { - RCLCPP_ERROR_STREAM(LOGGER, "Robot model was not set"); + RCLCPP_ERROR_STREAM(getLogger(), "Robot model was not set"); } return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp index 4eda1c472a..21d11e1b95 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp @@ -35,13 +35,17 @@ #include #include #include +#include #include namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_loader_ptp"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_planning_context_loader_ptp"); } +} // namespace pilz_industrial_motion_planner::PlanningContextLoaderPTP::PlanningContextLoaderPTP() { @@ -64,12 +68,12 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderPTP::loadContext( { if (!limits_set_) { - RCLCPP_ERROR_STREAM(LOGGER, + RCLCPP_ERROR_STREAM(getLogger(), "Joint Limits are not defined. Cannot load planning context. Call setLimits loadContext"); } if (!model_set_) { - RCLCPP_ERROR_STREAM(LOGGER, "Robot model was not set"); + RCLCPP_ERROR_STREAM(getLogger(), "Robot model was not set"); } return false; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index b4c0f0145a..e3a14310bb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -39,24 +39,27 @@ #include #include #include +#include namespace { -const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_blender_transition_window"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_trajectory_blender_transition_window"); } +} // namespace bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( const planning_scene::PlanningSceneConstPtr& planning_scene, const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, pilz_industrial_motion_planner::TrajectoryBlendResponse& res) { - RCLCPP_INFO(LOGGER, "Start trajectory blending using transition window."); + RCLCPP_INFO(getLogger(), "Start trajectory blending using transition window."); double sampling_time = 0.; if (!validateRequest(req, sampling_time, res.error_code)) { - RCLCPP_ERROR(LOGGER, "Trajectory blend request is not valid."); + RCLCPP_ERROR(getLogger(), "Trajectory blend request is not valid."); return false; } @@ -67,7 +70,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( std::size_t second_intersection_index; if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index)) { - RCLCPP_ERROR(LOGGER, "Blend radius to large."); + RCLCPP_ERROR(getLogger(), "Blend radius to large."); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -100,7 +103,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( blend_joint_trajectory, error_code, true)) { // LCOV_EXCL_START - RCLCPP_INFO(LOGGER, "Failed to generate joint trajectory for blending trajectory."); + RCLCPP_INFO(getLogger(), "Failed to generate joint trajectory for blending trajectory."); res.error_code.val = error_code.val; return false; // LCOV_EXCL_STOP @@ -142,12 +145,12 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time, moveit_msgs::msg::MoveItErrorCodes& error_code) const { - RCLCPP_DEBUG(LOGGER, "Validate the trajectory blend request."); + RCLCPP_DEBUG(getLogger(), "Validate the trajectory blend request."); // check planning group if (!req.first_trajectory->getRobotModel()->hasJointModelGroup(req.group_name)) { - RCLCPP_ERROR_STREAM(LOGGER, "Unknown planning group: " << req.group_name); + RCLCPP_ERROR_STREAM(getLogger(), "Unknown planning group: " << req.group_name); error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME; return false; } @@ -156,14 +159,14 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate if (!req.first_trajectory->getRobotModel()->hasLinkModel(req.link_name) && !req.first_trajectory->getLastWayPoint().hasAttachedBody(req.link_name)) { - RCLCPP_ERROR_STREAM(LOGGER, "Unknown link name: " << req.link_name); + RCLCPP_ERROR_STREAM(getLogger(), "Unknown link name: " << req.link_name); error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME; return false; } if (req.blend_radius <= 0) { - RCLCPP_ERROR(LOGGER, "Blending radius must be positive"); + RCLCPP_ERROR(getLogger(), "Blending radius must be positive"); error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -173,8 +176,8 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate if (!pilz_industrial_motion_planner::isRobotStateEqual( req.first_trajectory->getLastWayPoint(), req.second_trajectory->getFirstWayPoint(), req.group_name, EPSILON)) { - RCLCPP_ERROR_STREAM( - LOGGER, "During blending the last point of the preceding and the first point of the succeeding trajectory"); + RCLCPP_ERROR_STREAM(getLogger(), "During blending the last point of the preceding and the first point of the " + "succeeding trajectory"); error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -195,7 +198,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate !pilz_industrial_motion_planner::isRobotStateStationary(req.second_trajectory->getFirstWayPoint(), req.group_name, EPSILON)) { - RCLCPP_ERROR(LOGGER, "Intersection point of the blending trajectories has non-zero velocities/accelerations."); + RCLCPP_ERROR(getLogger(), "Intersection point of the blending trajectories has non-zero velocities/accelerations."); error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -265,7 +268,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIn const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, std::size_t& first_interse_index, std::size_t& second_interse_index) const { - RCLCPP_INFO(LOGGER, "Search for start and end point of blending trajectory."); + RCLCPP_INFO(getLogger(), "Search for start and end point of blending trajectory."); // compute the position of the center of the blend sphere // (last point of the first trajectory, first point of the second trajectory) @@ -275,19 +278,19 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIn if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.first_trajectory, true, first_interse_index)) { - RCLCPP_ERROR_STREAM(LOGGER, "Intersection point of first trajectory not found."); + RCLCPP_ERROR_STREAM(getLogger(), "Intersection point of first trajectory not found."); return false; } - RCLCPP_INFO_STREAM(LOGGER, "Intersection point of first trajectory found, index: " << first_interse_index); + RCLCPP_INFO_STREAM(getLogger(), "Intersection point of first trajectory found, index: " << first_interse_index); if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.second_trajectory, false, second_interse_index)) { - RCLCPP_ERROR_STREAM(LOGGER, "Intersection point of second trajectory not found."); + RCLCPP_ERROR_STREAM(getLogger(), "Intersection point of second trajectory not found."); return false; } - RCLCPP_INFO_STREAM(LOGGER, "Intersection point of second trajectory found, index: " << second_interse_index); + RCLCPP_INFO_STREAM(getLogger(), "Intersection point of second trajectory found, index: " << second_interse_index); return true; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 9f1e5f795c..555421a685 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -39,11 +39,15 @@ #include #include #include +#include namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_functions"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_trajectory_functions"); } +} // namespace bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name, const std::string& link_name, @@ -55,20 +59,21 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); if (!robot_model->hasJointModelGroup(group_name)) { - RCLCPP_ERROR_STREAM(LOGGER, "Robot model has no planning group named as " << group_name); + RCLCPP_ERROR_STREAM(getLogger(), "Robot model has no planning group named as " << group_name); return false; } if (!robot_model->getJointModelGroup(group_name)->canSetStateFromIK(link_name)) { - RCLCPP_ERROR_STREAM(LOGGER, "No valid IK solver exists for " << link_name << " in planning group " << group_name); + RCLCPP_ERROR_STREAM(getLogger(), + "No valid IK solver exists for " << link_name << " in planning group " << group_name); return false; } if (frame_id != robot_model->getModelFrame()) { - RCLCPP_ERROR_STREAM(LOGGER, "Given frame (" << frame_id << ") is unequal to model frame(" - << robot_model->getModelFrame() << ')'); + RCLCPP_ERROR_STREAM(getLogger(), "Given frame (" << frame_id << ") is unequal to model frame(" + << robot_model->getModelFrame() << ')'); return false; } @@ -95,9 +100,9 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin } else { - RCLCPP_ERROR(LOGGER, "Unable to find IK solution."); + RCLCPP_ERROR(getLogger(), "Unable to find IK solution."); // TODO(henning): Re-enable logging error - // RCLCPP_ERROR_STREAM(LOGGER, "Inverse kinematics for pose \n" << pose.translation() << " has no solution."); + // RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics for pose \n" << pose.translation() << " has no solution."); return false; } } @@ -125,7 +130,7 @@ bool pilz_industrial_motion_planner::computeLinkFK(const planning_scene::Plannin // check the reference frame of the target pose if (!rstate.knowsFrameTransform(link_name)) { - RCLCPP_ERROR_STREAM(LOGGER, "The target link " << link_name << " is not known by robot."); + RCLCPP_ERROR_STREAM(getLogger(), "The target link " << link_name << " is not known by robot."); return false; } @@ -146,7 +151,7 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( const double epsilon = 10e-6; if (duration_current <= epsilon) { - RCLCPP_ERROR(LOGGER, "Sample duration too small, cannot compute the velocity"); + RCLCPP_ERROR(getLogger(), "Sample duration too small, cannot compute the velocity"); return false; } @@ -158,10 +163,11 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( if (!joint_limits.verifyVelocityLimit(pos.first, velocity_current)) { - RCLCPP_ERROR_STREAM(LOGGER, "Joint velocity limit of " - << pos.first << " violated. Set the velocity scaling factor lower!" - << " Actual joint velocity is " << velocity_current << ", while the limit is " - << joint_limits.getLimit(pos.first).max_velocity << ". "); + RCLCPP_ERROR_STREAM(getLogger(), "Joint velocity limit of " + << pos.first << " violated. Set the velocity scaling factor lower!" + << " Actual joint velocity is " << velocity_current + << ", while the limit is " << joint_limits.getLimit(pos.first).max_velocity + << ". "); return false; } @@ -171,11 +177,11 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( { if (!joint_limits.verifyAccelerationLimit(pos.first, acceleration_current)) { - RCLCPP_ERROR_STREAM(LOGGER, "Joint acceleration limit of " - << pos.first << " violated. Set the acceleration scaling factor lower!" - << " Actual joint acceleration is " << acceleration_current - << ", while the limit is " << joint_limits.getLimit(pos.first).max_acceleration - << ". "); + RCLCPP_ERROR_STREAM(getLogger(), "Joint acceleration limit of " + << pos.first << " violated. Set the acceleration scaling factor lower!" + << " Actual joint acceleration is " << acceleration_current + << ", while the limit is " + << joint_limits.getLimit(pos.first).max_acceleration << ". "); return false; } } @@ -184,11 +190,11 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( { if (!joint_limits.verifyDecelerationLimit(pos.first, acceleration_current)) { - RCLCPP_ERROR_STREAM(LOGGER, "Joint deceleration limit of " - << pos.first << " violated. Set the acceleration scaling factor lower!" - << " Actual joint deceleration is " << acceleration_current - << ", while the limit is " << joint_limits.getLimit(pos.first).max_deceleration - << ". "); + RCLCPP_ERROR_STREAM(getLogger(), "Joint deceleration limit of " + << pos.first << " violated. Set the acceleration scaling factor lower!" + << " Actual joint deceleration is " << acceleration_current + << ", while the limit is " + << joint_limits.getLimit(pos.first).max_deceleration << ". "); return false; } } @@ -205,7 +211,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision) { - RCLCPP_DEBUG(LOGGER, "Generate joint trajectory from a Cartesian trajectory."); + RCLCPP_DEBUG(getLogger(), "Generate joint trajectory from a Cartesian trajectory."); const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); rclcpp::Clock clock; @@ -237,7 +243,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( if (!computePoseIK(scene, group_name, link_name, pose_sample, robot_model->getModelFrame(), ik_solution_last, ik_solution, check_self_collision)) { - RCLCPP_ERROR(LOGGER, "Failed to compute inverse kinematics solution for sampled Cartesian pose."); + RCLCPP_ERROR(getLogger(), "Failed to compute inverse kinematics solution for sampled Cartesian pose."); error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; joint_trajectory.points.clear(); return false; @@ -260,9 +266,9 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, sampling_time, duration_current_sample, joint_limits)) { - RCLCPP_ERROR_STREAM(LOGGER, "Inverse kinematics solution at " - << *time_iter - << "s violates the joint velocity/acceleration/deceleration limits."); + RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics solution at " + << *time_iter + << "s violates the joint velocity/acceleration/deceleration limits."); error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; joint_trajectory.points.clear(); return false; @@ -307,9 +313,9 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; double duration_ms = (clock.now() - generation_begin).seconds() * 1000; - RCLCPP_DEBUG_STREAM(LOGGER, "Generate trajectory (N-Points: " - << joint_trajectory.points.size() << ") took " << duration_ms << " ms | " - << duration_ms / joint_trajectory.points.size() << " ms per Point"); + RCLCPP_DEBUG_STREAM(getLogger(), "Generate trajectory (N-Points: " + << joint_trajectory.points.size() << ") took " << duration_ms << " ms | " + << duration_ms / joint_trajectory.points.size() << " ms per Point"); return true; } @@ -323,7 +329,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision) { - RCLCPP_DEBUG(LOGGER, "Generate joint trajectory from a Cartesian trajectory."); + RCLCPP_DEBUG(getLogger(), "Generate joint trajectory from a Cartesian trajectory."); const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel(); rclcpp::Clock clock; @@ -345,8 +351,8 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( if (!computePoseIK(scene, group_name, link_name, trajectory.points.at(i).pose, robot_model->getModelFrame(), ik_solution_last, ik_solution, check_self_collision)) { - RCLCPP_ERROR(LOGGER, "Failed to compute inverse kinematics solution for sampled " - "Cartesian pose."); + RCLCPP_ERROR(getLogger(), "Failed to compute inverse kinematics solution for sampled " + "Cartesian pose."); error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; joint_trajectory.points.clear(); return false; @@ -371,10 +377,10 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( // overload generateJointTrajectory(..., // KDL::Trajectory, ...) // TODO: refactor to avoid code duplication. - RCLCPP_ERROR_STREAM(LOGGER, "Inverse kinematics solution of the " - << i - << "th sample violates the joint " - "velocity/acceleration/deceleration limits."); + RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics solution of the " + << i + << "th sample violates the joint " + "velocity/acceleration/deceleration limits."); error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; joint_trajectory.points.clear(); return false; @@ -404,9 +410,9 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; double duration_ms = (clock.now() - generation_begin).seconds() * 1000; - RCLCPP_DEBUG_STREAM(LOGGER, "Generate trajectory (N-Points: " - << joint_trajectory.points.size() << ") took " << duration_ms << " ms | " - << duration_ms / joint_trajectory.points.size() << " ms per Point"); + RCLCPP_DEBUG_STREAM(getLogger(), "Generate trajectory (N-Points: " + << joint_trajectory.points.size() << ") took " << duration_ms << " ms | " + << duration_ms / joint_trajectory.points.size() << " ms per Point"); return true; } @@ -421,7 +427,7 @@ bool pilz_industrial_motion_planner::determineAndCheckSamplingTime( std::size_t n2 = second_trajectory->getWayPointCount() - 1; if ((n1 < 2) && (n2 < 2)) { - RCLCPP_ERROR_STREAM(LOGGER, "Both trajectories do not have enough points to determine sampling time."); + RCLCPP_ERROR_STREAM(getLogger(), "Both trajectories do not have enough points to determine sampling time."); return false; } @@ -440,9 +446,9 @@ bool pilz_industrial_motion_planner::determineAndCheckSamplingTime( { if (fabs(sampling_time - first_trajectory->getWayPointDurationFromPrevious(i)) > epsilon) { - RCLCPP_ERROR_STREAM(LOGGER, "First trajectory violates sampline time " << sampling_time << " between points " - << (i - 1) << "and " << i - << " (indices)."); + RCLCPP_ERROR_STREAM(getLogger(), "First trajectory violates sampline time " + << sampling_time << " between points " << (i - 1) << "and " << i + << " (indices)."); return false; } } @@ -451,9 +457,9 @@ bool pilz_industrial_motion_planner::determineAndCheckSamplingTime( { if (fabs(sampling_time - second_trajectory->getWayPointDurationFromPrevious(i)) > epsilon) { - RCLCPP_ERROR_STREAM(LOGGER, "Second trajectory violates sampline time " << sampling_time << " between points " - << (i - 1) << "and " << i - << " (indices)."); + RCLCPP_ERROR_STREAM(getLogger(), "Second trajectory violates sampline time " + << sampling_time << " between points " << (i - 1) << "and " << i + << " (indices)."); return false; } } @@ -473,8 +479,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_position_1 - joint_position_2).norm() > epsilon) { - RCLCPP_DEBUG_STREAM(LOGGER, "Joint positions of the two states are different. state1: " - << joint_position_1 << " state2: " << joint_position_2); + RCLCPP_DEBUG_STREAM(getLogger(), "Joint positions of the two states are different. state1: " + << joint_position_1 << " state2: " << joint_position_2); return false; } @@ -485,8 +491,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_velocity_1 - joint_velocity_2).norm() > epsilon) { - RCLCPP_DEBUG_STREAM(LOGGER, "Joint velocities of the two states are different. state1: " - << joint_velocity_1 << " state2: " << joint_velocity_2); + RCLCPP_DEBUG_STREAM(getLogger(), "Joint velocities of the two states are different. state1: " + << joint_velocity_1 << " state2: " << joint_velocity_2); return false; } @@ -497,8 +503,8 @@ bool pilz_industrial_motion_planner::isRobotStateEqual(const moveit::core::Robot if ((joint_acc_1 - joint_acc_2).norm() > epsilon) { - RCLCPP_DEBUG_STREAM(LOGGER, "Joint accelerations of the two states are different. state1: " - << joint_acc_1 << " state2: " << joint_acc_2); + RCLCPP_DEBUG_STREAM(getLogger(), "Joint accelerations of the two states are different. state1: " + << joint_acc_1 << " state2: " << joint_acc_2); return false; } @@ -512,13 +518,13 @@ bool pilz_industrial_motion_planner::isRobotStateStationary(const moveit::core:: state.copyJointGroupVelocities(group, joint_variable); if (joint_variable.norm() > EPSILON) { - RCLCPP_DEBUG(LOGGER, "Joint velocities are not zero."); + RCLCPP_DEBUG(getLogger(), "Joint velocities are not zero."); return false; } state.copyJointGroupAccelerations(group, joint_variable); if (joint_variable.norm() > EPSILON) { - RCLCPP_DEBUG(LOGGER, "Joint accelerations are not zero."); + RCLCPP_DEBUG(getLogger(), "Joint accelerations are not zero."); return false; } return true; @@ -529,7 +535,7 @@ bool pilz_industrial_motion_planner::linearSearchIntersectionPoint(const std::st const robot_trajectory::RobotTrajectoryPtr& traj, bool inverseOrder, std::size_t& index) { - RCLCPP_DEBUG(LOGGER, "Start linear search for intersection point."); + RCLCPP_DEBUG(getLogger(), "Start linear search for intersection point."); const size_t waypoint_num = traj->getWayPointCount(); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 24df7e4672..494acabaea 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -42,6 +42,7 @@ #include #include +#include #include @@ -49,8 +50,11 @@ namespace pilz_industrial_motion_planner { namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_trajectory_generator"); } +} // namespace sensor_msgs::msg::JointState TrajectoryGenerator::filterGroupValues(const sensor_msgs::msg::JointState& robot_state, const std::string& group) const @@ -307,7 +311,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, double sampling_time) { - RCLCPP_INFO_STREAM(LOGGER, "Generating " << req.planner_id << " trajectory..."); + RCLCPP_INFO_STREAM(getLogger(), "Generating " << req.planner_id << " trajectory..."); rclcpp::Time planning_begin = clock_->now(); res.planner_id = req.planner_id; @@ -317,7 +321,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& } catch (const MoveItErrorCodeException& ex) { - RCLCPP_ERROR_STREAM(LOGGER, ex.what()); + RCLCPP_ERROR_STREAM(getLogger(), ex.what()); res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return; @@ -329,7 +333,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& } catch (const MoveItErrorCodeException& ex) { - RCLCPP_ERROR_STREAM(LOGGER, ex.what()); + RCLCPP_ERROR_STREAM(getLogger(), ex.what()); res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return; @@ -342,7 +346,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& } catch (const MoveItErrorCodeException& ex) { - RCLCPP_ERROR_STREAM(LOGGER, ex.what()); + RCLCPP_ERROR_STREAM(getLogger(), ex.what()); res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return; @@ -355,7 +359,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& } catch (const MoveItErrorCodeException& ex) { - RCLCPP_ERROR_STREAM(LOGGER, ex.what()); + RCLCPP_ERROR_STREAM(getLogger(), ex.what()); res.error_code.val = ex.getErrorCode(); setFailureResponse(planning_begin, res); return; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index f8afbbdfff..61e4cb0142 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -48,13 +48,17 @@ #include #include #include +#include namespace pilz_industrial_motion_planner { namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_circ"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_trajectory_generator_circ"); } +} // namespace TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, const LimitsContainer& planner_limits, const std::string& /*group_name*/) @@ -90,7 +94,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni const planning_interface::MotionPlanRequest& req, TrajectoryGenerator::MotionPlanInfo& info) const { - RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request."); + RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request."); info.group_name = req.group_name; std::string frame_id{ robot_model_->getModelFrame() }; @@ -129,8 +133,8 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) { - RCLCPP_WARN(LOGGER, "Frame id is not set in position/orientation constraints of " - "goal. Use model frame as default"); + RCLCPP_WARN(getLogger(), "Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); frame_id = robot_model_->getModelFrame(); } else @@ -216,7 +220,7 @@ void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& std::unique_ptr TrajectoryGeneratorCIRC::setPathCIRC(const MotionPlanInfo& info) const { - RCLCPP_DEBUG(LOGGER, "Set Cartesian path for CIRC command."); + RCLCPP_DEBUG(getLogger(), "Set Cartesian path for CIRC command."); KDL::Frame start_pose, goal_pose; tf2::transformEigenToKDL(info.start_pose, start_pose); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 8147f64af6..9c9b251ecd 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -49,13 +49,17 @@ #include #include #include +#include namespace pilz_industrial_motion_planner { namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_lin"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_trajectory_generator_lin"); } +} // namespace TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, const LimitsContainer& planner_limits, const std::string& /*group_name*/) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) @@ -67,7 +71,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin const planning_interface::MotionPlanRequest& req, TrajectoryGenerator::MotionPlanInfo& info) const { - RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request."); + RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request."); info.group_name = req.group_name; std::string frame_id{ robot_model_->getModelFrame() }; @@ -104,8 +108,8 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) { - RCLCPP_WARN(LOGGER, "Frame id is not set in position/orientation constraints of " - "goal. Use model frame as default"); + RCLCPP_WARN(getLogger(), "Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); frame_id = robot_model_->getModelFrame(); } else @@ -178,7 +182,7 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s std::unique_ptr TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affine3d& start_pose, const Eigen::Affine3d& goal_pose) const { - RCLCPP_DEBUG(LOGGER, "Set Cartesian path for LIN command."); + RCLCPP_DEBUG(getLogger(), "Set Cartesian path for LIN command."); KDL::Frame kdl_start_pose, kdl_goal_pose; tf2::transformEigenToKDL(start_pose, kdl_start_pose); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index b0344350ed..75145e902f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -34,6 +34,7 @@ #include #include +#include #include #include @@ -48,8 +49,11 @@ namespace pilz_industrial_motion_planner { namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_ptp"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("pilz_trajectory_generator_ptp"); } +} // namespace TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model, const LimitsContainer& planner_limits, const std::string& group_name) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) @@ -87,7 +91,7 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelCon } } - RCLCPP_INFO(LOGGER, "Initialized Point-to-Point Trajectory Generator."); + RCLCPP_INFO(getLogger(), "Initialized Point-to-Point Trajectory Generator."); } void TrajectoryGeneratorPTP::planPTP(const std::map& start_pos, @@ -114,7 +118,7 @@ void TrajectoryGeneratorPTP::planPTP(const std::map& start_ } if (goal_reached) { - RCLCPP_INFO_STREAM(LOGGER, "Goal already reached, set one goal point explicitly."); + RCLCPP_INFO_STREAM(getLogger(), "Goal already reached, set one goal point explicitly."); if (joint_trajectory.points.empty()) { trajectory_msgs::msg::JointTrajectoryPoint point; diff --git a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp index 82b964065a..2c4ffdbda9 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -46,7 +47,13 @@ namespace stomp_moveit { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("stomp_moveit"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("stomp_moveit"); +} +} // namespace using namespace planning_interface; @@ -109,13 +116,13 @@ class StompPlannerManager : public PlannerManager { if (req.goal_constraints.empty()) { - RCLCPP_ERROR(LOGGER, "Invalid goal constraints"); + RCLCPP_ERROR(getLogger(), "Invalid goal constraints"); return false; } if (req.group_name.empty() || !robot_model_->hasJointModelGroup(req.group_name)) { - RCLCPP_ERROR(LOGGER, "Invalid joint group '%s'", req.group_name.c_str()); + RCLCPP_ERROR(getLogger(), "Invalid joint group '%s'", req.group_name.c_str()); return false; } return true; diff --git a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp index bc459a63d3..f5895ed0d3 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp @@ -51,10 +51,17 @@ #include #include +#include namespace stomp_moveit { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("stomp_moveit"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("stomp_moveit"); +} +} // namespace // @brief Run a planning attempt with STOMP, either providing start and goal states or an optional seed trajectory bool solveWithStomp(const std::shared_ptr& stomp, const moveit::core::RobotState& start_state, @@ -105,7 +112,7 @@ bool extractSeedTrajectory(const planning_interface::MotionPlanRequest& req, auto n = constraints[i].joint_constraints.size(); if (n != dof) { // first test to ensure that dimensionality is correct - RCLCPP_WARN(LOGGER, "Seed trajectory index %lu does not have %lu constraints (has %lu instead).", i, dof, n); + RCLCPP_WARN(getLogger(), "Seed trajectory index %lu does not have %lu constraints (has %lu instead).", i, dof, n); return false; } @@ -116,8 +123,9 @@ bool extractSeedTrajectory(const planning_interface::MotionPlanRequest& req, const auto& c = constraints[i].joint_constraints[j]; if (c.joint_name != names[j]) { - RCLCPP_WARN(LOGGER, "Seed trajectory (index %lu, joint %lu) joint name '%s' does not match expected name '%s'", - i, j, c.joint_name.c_str(), names[j].c_str()); + RCLCPP_WARN(getLogger(), + "Seed trajectory (index %lu, joint %lu) joint name '%s' does not match expected name '%s'", i, j, + c.joint_name.c_str(), names[j].c_str()); return false; } joint_pt.positions.push_back(c.position); @@ -272,7 +280,7 @@ void StompPlanningContext::solve(planning_interface::MotionPlanResponse& res) void StompPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& /*res*/) { // TODO(#2168): implement this function - RCLCPP_ERROR(LOGGER, + RCLCPP_ERROR(getLogger(), "StompPlanningContext::solve(planning_interface::MotionPlanDetailedResponse&) is not implemented!"); return; } 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 c6c316915e..e5fff81211 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 @@ -54,13 +54,21 @@ #include #include #include +#include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.plugins.ros_control_interface"); static const rclcpp::Duration CONTROLLER_INFORMATION_VALIDITY_AGE = rclcpp::Duration::from_seconds(1.0); static const double SERVICE_CALL_TIMEOUT = 1.0; namespace moveit_ros_control_interface { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("ros_control_interface"); +} +} // namespace + /** * \brief Get joint name from resource name reported by ros2_control, since claimed_interfaces return by ros2_control * will have the interface name as suffix joint_name/INTERFACE_TYPE @@ -135,8 +143,8 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan // Skip if controller stamp is too new for new discovery, enforce update if force==true if (!force && ((node_->now() - controllers_stamp_) < CONTROLLER_INFORMATION_VALIDITY_AGE)) { - RCLCPP_WARN_STREAM(LOGGER, "Controller information from " << list_controllers_service_->get_service_name() - << " is out of date, skipping discovery"); + RCLCPP_WARN_STREAM(getLogger(), "Controller information from " << list_controllers_service_->get_service_name() + << " is out of date, skipping discovery"); return; } @@ -146,9 +154,9 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan auto result_future = list_controllers_service_->async_send_request(request); if (result_future.wait_for(std::chrono::duration(SERVICE_CALL_TIMEOUT)) == std::future_status::timeout) { - RCLCPP_WARN_STREAM(LOGGER, "Failed to read controllers from " << list_controllers_service_->get_service_name() - << " within " << SERVICE_CALL_TIMEOUT - << " seconds"); + RCLCPP_WARN_STREAM(getLogger(), "Failed to read controllers from " + << list_controllers_service_->get_service_name() << " within " + << SERVICE_CALL_TIMEOUT << " seconds"); return; } @@ -253,7 +261,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan "ros_control_namespace parameter.")]] Ros2ControlManager(const std::string& ns) : ns_(ns), loader_("moveit_ros_control_interface", "moveit_ros_control_interface::ControllerHandleAllocator") { - RCLCPP_INFO_STREAM(LOGGER, "Started moveit_ros_control_interface::Ros2ControlManager for namespace " << ns_); + RCLCPP_INFO_STREAM(getLogger(), "Started moveit_ros_control_interface::Ros2ControlManager for namespace " << ns_); } void initialize(const rclcpp::Node::SharedPtr& node) override @@ -267,7 +275,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan else { node_->get_parameter("ros_control_namespace", ns_); - RCLCPP_INFO_STREAM(LOGGER, "Namespace for controller manager was specified, namespace: " << ns_); + RCLCPP_INFO_STREAM(getLogger(), "Namespace for controller manager was specified, namespace: " << ns_); } list_controllers_service_ = node_->create_client( @@ -474,9 +482,9 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan auto result_future = switch_controller_service_->async_send_request(request); if (result_future.wait_for(std::chrono::duration(SERVICE_CALL_TIMEOUT)) == std::future_status::timeout) { - RCLCPP_ERROR_STREAM(LOGGER, "Couldn't switch controllers at " << switch_controller_service_->get_service_name() - << " within " << SERVICE_CALL_TIMEOUT - << " seconds"); + RCLCPP_ERROR_STREAM(getLogger(), "Couldn't switch controllers at " + << switch_controller_service_->get_service_name() << " within " + << SERVICE_CALL_TIMEOUT << " seconds"); return false; } discover(true); @@ -500,8 +508,9 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan { if (controller.chain_connections.size() > 1) { - RCLCPP_ERROR_STREAM(LOGGER, "Controller with name %s chains to more than one controller. Chaining to more than " - "one controller is not supported."); + RCLCPP_ERROR_STREAM(getLogger(), + "Controller with name %s chains to more than one controller. Chaining to more than " + "one controller is not supported."); return false; } dependency_map_[controller.name].clear(); @@ -559,7 +568,7 @@ class Ros2ControlMultiManager : public moveit_controller_manager::MoveItControll std::string ns = service_name.substr(0, found); if (controller_managers_.find(ns) == controller_managers_.end()) { // create Ros2ControlManager if it does not exist - RCLCPP_INFO_STREAM(LOGGER, "Adding controller_manager interface for node at namespace " << ns); + RCLCPP_INFO_STREAM(getLogger(), "Adding controller_manager interface for node at namespace " << ns); auto controller_manager = std::make_shared(ns); controller_manager->initialize(node_); controller_managers_.insert(std::make_pair(ns, controller_manager)); diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp index c2989141f8..acbfe27598 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp @@ -47,6 +47,7 @@ #include #include #include +#include namespace { @@ -82,7 +83,13 @@ std::string makeParameterName(T... strings) namespace moveit_simple_controller_manager { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.plugins.moveit_simple_controller_manager"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit_simple_controller_manager"); +} +} // namespace static const std::string PARAM_BASE_NAME = "moveit_simple_controller_manager"; class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItControllerManager { @@ -96,14 +103,14 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo node_ = node; if (!node_->has_parameter(makeParameterName(PARAM_BASE_NAME, "controller_names"))) { - RCLCPP_ERROR_STREAM(LOGGER, "No controller_names specified."); + RCLCPP_ERROR_STREAM(getLogger(), "No controller_names specified."); return; } rclcpp::Parameter controller_names_param; node_->get_parameter(makeParameterName(PARAM_BASE_NAME, "controller_names"), controller_names_param); if (controller_names_param.get_type() != rclcpp::ParameterType::PARAMETER_STRING_ARRAY) { - RCLCPP_ERROR(LOGGER, "Parameter controller_names should be specified as a string array"); + RCLCPP_ERROR(getLogger(), "Parameter controller_names should be specified as a string array"); return; } std::vector controller_names = controller_names_param.as_string_array(); @@ -116,15 +123,15 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo const std::string& action_ns_param = makeParameterName(PARAM_BASE_NAME, controller_name, "action_ns"); if (!node_->get_parameter(action_ns_param, action_ns)) { - RCLCPP_ERROR_STREAM(LOGGER, "No action namespace specified for controller `" - << controller_name << "` through parameter `" << action_ns_param << '`'); + RCLCPP_ERROR_STREAM(getLogger(), "No action namespace specified for controller `" + << controller_name << "` through parameter `" << action_ns_param << '`'); continue; } std::string type; if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name, "type"), type)) { - RCLCPP_ERROR_STREAM(LOGGER, "No type specified for controller " << controller_name); + RCLCPP_ERROR_STREAM(getLogger(), "No type specified for controller " << controller_name); continue; } @@ -132,7 +139,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo if (!node_->get_parameter(makeParameterName(PARAM_BASE_NAME, controller_name, "joints"), controller_joints) || controller_joints.empty()) { - RCLCPP_ERROR_STREAM(LOGGER, "No joints specified for controller " << controller_name); + RCLCPP_ERROR_STREAM(getLogger(), "No joints specified for controller " << controller_name); continue; } @@ -143,7 +150,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo const std::string& max_effort_param = makeParameterName(PARAM_BASE_NAME, controller_name, "max_effort"); if (!node->get_parameter(max_effort_param, max_effort)) { - RCLCPP_INFO_STREAM(LOGGER, "Max effort set to 0.0"); + RCLCPP_INFO_STREAM(getLogger(), "Max effort set to 0.0"); max_effort = 0.0; } @@ -153,8 +160,8 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo { if (controller_joints.size() != 2) { - RCLCPP_ERROR_STREAM(LOGGER, "Parallel Gripper requires exactly two joints, " << controller_joints.size() - << " are specified"); + RCLCPP_ERROR_STREAM(getLogger(), "Parallel Gripper requires exactly two joints, " + << controller_joints.size() << " are specified"); continue; } static_cast(new_handle.get()) @@ -173,18 +180,18 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo node_->get_parameter_or(makeParameterName(PARAM_BASE_NAME, "allow_failure"), allow_failure, false); static_cast(new_handle.get())->allowFailure(allow_failure); - RCLCPP_INFO_STREAM(LOGGER, "Added GripperCommand controller for " << controller_name); + RCLCPP_INFO_STREAM(getLogger(), "Added GripperCommand controller for " << controller_name); controllers_[controller_name] = new_handle; } else if (type == "FollowJointTrajectory") { new_handle = std::make_shared(node_, controller_name, action_ns); - RCLCPP_INFO_STREAM(LOGGER, "Added FollowJointTrajectory controller for " << controller_name); + RCLCPP_INFO_STREAM(getLogger(), "Added FollowJointTrajectory controller for " << controller_name); controllers_[controller_name] = new_handle; } else { - RCLCPP_ERROR_STREAM(LOGGER, "Unknown controller type: " << type); + RCLCPP_ERROR_STREAM(getLogger(), "Unknown controller type: " << type); continue; } if (!controllers_[controller_name]) @@ -205,7 +212,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo } catch (...) { - RCLCPP_ERROR_STREAM(LOGGER, "Caught unknown exception while parsing controller information"); + RCLCPP_ERROR_STREAM(getLogger(), "Caught unknown exception while parsing controller information"); } } } @@ -221,7 +228,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo } else { - RCLCPP_FATAL_STREAM(LOGGER, "No such controller: " << name); + RCLCPP_FATAL_STREAM(getLogger(), "No such controller: " << name); } return moveit_controller_manager::MoveItControllerHandlePtr(); } @@ -234,7 +241,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo for (std::map::const_iterator it = controllers_.begin(); it != controllers_.end(); ++it) names.push_back(it->first); - RCLCPP_INFO_STREAM(LOGGER, "Returned " << names.size() << " controllers in list"); + RCLCPP_INFO_STREAM(getLogger(), "Returned " << names.size() << " controllers in list"); } /* @@ -266,7 +273,7 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo } else { - RCLCPP_WARN(LOGGER, + RCLCPP_WARN(getLogger(), "The joints for controller '%s' are not known. Perhaps the controller configuration is " "not loaded on the param server?", name.c_str()); diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp index a436bb535a..eaf8eee2fa 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -35,11 +35,17 @@ /* Author: Peter David Fagan */ #include "moveit_cpp.h" +#include namespace moveit_py { namespace bind_moveit_cpp { +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit_cpp_initializer"); +} + std::shared_ptr getPlanningComponent(std::shared_ptr& moveit_cpp_ptr, const std::string& planning_component) { @@ -56,8 +62,6 @@ void initMoveitPy(py::module& m) .def(py::init([](const std::string& node_name, const std::vector& launch_params_filepaths, const py::object& config_dict, bool provide_planning_service) { - static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_initializer"); - // This section is used to load the appropriate node parameters before spinning a moveit_cpp instance // Priority is given to parameters supplied directly via a config_dict, followed by launch parameters // and finally no supplied parameters. @@ -83,7 +87,7 @@ void initMoveitPy(py::module& m) // Initialize ROS, pass launch arguments with rclcpp::init() if (!rclcpp::ok()) { - RCLCPP_INFO(LOGGER, "Initialize rclcpp"); + RCLCPP_INFO(getLogger(), "Initialize rclcpp"); std::vector chars; chars.reserve(launch_arguments.size()); for (const auto& arg : launch_arguments) @@ -95,18 +99,18 @@ void initMoveitPy(py::module& m) } // Build NodeOptions - RCLCPP_INFO(LOGGER, "Initialize node parameters"); + RCLCPP_INFO(getLogger(), "Initialize node parameters"); rclcpp::NodeOptions node_options; node_options.allow_undeclared_parameters(true) .automatically_declare_parameters_from_overrides(true) .arguments(launch_arguments); - RCLCPP_INFO(LOGGER, "Initialize node and executor"); + RCLCPP_INFO(getLogger(), "Initialize node and executor"); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(node_name, "", node_options); std::shared_ptr executor = std::make_shared(); - RCLCPP_INFO(LOGGER, "Spin separate thread"); + RCLCPP_INFO(getLogger(), "Spin separate thread"); auto spin_node = [node, executor]() { executor->add_node(node); executor->spin(); diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp index d72ec0358c..48c1892ecb 100644 --- a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp @@ -40,7 +40,6 @@ namespace moveit_py { namespace bind_planning_scene_monitor { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_py.bind_planning_scene_monitor"); bool processCollisionObject(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, moveit_msgs::msg::CollisionObject& collision_object_msg) diff --git a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp index a995ca01ef..2cdfeb0356 100644 --- a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp +++ b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp @@ -40,8 +40,6 @@ namespace moveit_py { namespace bind_trajectory_execution_manager { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_py.bind_trajectory_execution_manager"); - void initTrajectoryExecutionManager(py::module& m) { py::class_ #include -namespace -{ -const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager"); -} - namespace moveit::hybrid_planning { ReactionResult ReplanInvalidatedTrajectory::react(const std::string& event) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp index f0306aa3b3..4d0ce03c3e 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp @@ -34,11 +34,6 @@ #include -namespace -{ -const rclcpp::Logger LOGGER = rclcpp::get_logger("hybrid_planning_manager"); -} - namespace moveit::hybrid_planning { bool SinglePlanExecution::initialize(const std::shared_ptr& hybrid_planning_manager) diff --git a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp index 5e7b375e0a..647b7558e9 100644 --- a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp +++ b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp @@ -56,6 +56,7 @@ #include #include #include +#include #if RCLCPP_VERSION_GTE(20, 0, 0) #include #else @@ -71,7 +72,10 @@ using namespace std::chrono_literals; namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("test_hybrid_planning_client"); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("test_hybrid_planning_client"); +} } // namespace class HybridPlanningDemo @@ -88,7 +92,7 @@ class HybridPlanningDemo } else { - RCLCPP_ERROR(LOGGER, "hybrid_planning_action_name parameter was not defined"); + RCLCPP_ERROR(getLogger(), "hybrid_planning_action_name parameter was not defined"); std::exit(EXIT_FAILURE); } hp_action_client_ = @@ -147,14 +151,14 @@ class HybridPlanningDemo void run() { - RCLCPP_INFO(LOGGER, "Initialize Planning Scene Monitor"); + RCLCPP_INFO(getLogger(), "Initialize Planning Scene Monitor"); tf_buffer_ = std::make_shared(node_->get_clock()); planning_scene_monitor_ = std::make_shared(node_, "robot_description", "planning_scene_monitor"); if (!planning_scene_monitor_->getPlanningScene()) { - RCLCPP_ERROR(LOGGER, "The planning scene was not retrieved!"); + RCLCPP_ERROR(getLogger(), "The planning scene was not retrieved!"); return; } else @@ -169,13 +173,13 @@ class HybridPlanningDemo if (!planning_scene_monitor_->waitForCurrentRobotState(node_->now(), 5)) { - RCLCPP_ERROR(LOGGER, "Timeout when waiting for /joint_states updates. Is the robot running?"); + RCLCPP_ERROR(getLogger(), "Timeout when waiting for /joint_states updates. Is the robot running?"); return; } if (!hp_action_client_->wait_for_action_server(20s)) { - RCLCPP_ERROR(LOGGER, "Hybrid planning action server not available after waiting"); + RCLCPP_ERROR(getLogger(), "Hybrid planning action server not available after waiting"); return; } @@ -194,7 +198,7 @@ class HybridPlanningDemo scene->processCollisionObjectMsg(collision_object_1_); } // Unlock PlanningScene - RCLCPP_INFO(LOGGER, "Wait 2s for the collision object"); + RCLCPP_INFO(getLogger(), "Wait 2s for the collision object"); rclcpp::sleep_for(2s); // Setup motion planning goal taken from motion_planning_api tutorial @@ -253,27 +257,27 @@ class HybridPlanningDemo switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - RCLCPP_INFO(LOGGER, "Hybrid planning goal succeeded"); + RCLCPP_INFO(getLogger(), "Hybrid planning goal succeeded"); break; case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(LOGGER, "Hybrid planning goal was aborted"); + RCLCPP_ERROR(getLogger(), "Hybrid planning goal was aborted"); return; case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(LOGGER, "Hybrid planning goal was canceled"); + RCLCPP_ERROR(getLogger(), "Hybrid planning goal was canceled"); return; default: - RCLCPP_ERROR(LOGGER, "Unknown hybrid planning result code"); + RCLCPP_ERROR(getLogger(), "Unknown hybrid planning result code"); return; - RCLCPP_INFO(LOGGER, "Hybrid planning result received"); + RCLCPP_INFO(getLogger(), "Hybrid planning result received"); } }; send_goal_options.feedback_callback = [](const rclcpp_action::ClientGoalHandle::SharedPtr& /*unused*/, const std::shared_ptr& feedback) { - RCLCPP_INFO_STREAM(LOGGER, feedback->feedback); + RCLCPP_INFO_STREAM(getLogger(), feedback->feedback); }; - RCLCPP_INFO(LOGGER, "Sending hybrid planning goal"); + RCLCPP_INFO(getLogger(), "Sending hybrid planning goal"); // Ask server to achieve some goal and wait until it's accepted auto goal_handle_future = hp_action_client_->async_send_goal(goal_action_request, send_goal_options); } diff --git a/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp b/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp index 89a5d7fb07..5c1b9dc99d 100644 --- a/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp +++ b/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp @@ -37,8 +37,6 @@ namespace collision_detection { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_plugin_loader"); - CollisionPluginLoader::CollisionPluginLoader() : logger_(moveit::getLogger("collision_plugin_loader")) { } diff --git a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp index 52b79968d4..0118986c5c 100644 --- a/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp +++ b/moveit_ros/planning/planning_components_tools/src/display_random_state.cpp @@ -40,8 +40,6 @@ using namespace std::chrono_literals; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("display_random_state"); - int main(int argc, char** argv) { rclcpp::init(argc, argv); diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index bac47ee51a..cbc3d20356 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -40,7 +40,13 @@ namespace { -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ros_planning.planning_pipeline"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("planning_pipeline"); +} +} // namespace /** * @brief Transform a joint trajectory into a vector of joint constraints @@ -146,7 +152,7 @@ void PlanningPipeline::configure() catch (pluginlib::PluginlibException& ex) { std::string classes_str = fmt::format("{}", fmt::join(planner_plugin_loader_->getDeclaredClasses(), ", ")); - RCLCPP_FATAL(LOGGER, + RCLCPP_FATAL(getLogger(), "Exception while loading planner '%s': %s" "Available plugins: %s", planner_name.c_str(), ex.what(), classes_str.c_str()); @@ -165,7 +171,7 @@ void PlanningPipeline::configure() { throw std::runtime_error("Unable to initialize planning plugin " + planner_name); } - RCLCPP_INFO(LOGGER, "Successfully loaded planner '%s'", planner_instance->getDescription().c_str()); + RCLCPP_INFO(getLogger(), "Successfully loaded planner '%s'", planner_instance->getDescription().c_str()); planner_map_.insert(std::make_pair(planner_name, planner_instance)); } diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp index 7267e6f149..f6a7683f2b 100644 --- a/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/test/test_execution_manager.cpp @@ -110,8 +110,6 @@ TEST_F(MoveItCppTest, PushExecuteAndWaitTest) } // namespace moveit_cpp -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.trajectory_execution_manager.test_app"); - int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp index a20d1e5cb2..b896492ac8 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_param_widget.cpp @@ -44,7 +44,6 @@ namespace mpi = moveit::planning_interface; namespace moveit_rviz_plugin { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_visualization.motion_planning_param_widget"); MotionPlanningParamWidget::MotionPlanningParamWidget(QWidget* parent) : rviz_common::properties::PropertyTreeWidget(parent) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp index 2af820fa44..41b29d94dc 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp @@ -40,10 +40,18 @@ #include #include #include +#include namespace moveit_rviz_plugin { -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.rviz_plugin_render_tools.robot_state_visualization"); +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("rviz_plugin_render_tools.robot_state_visualization"); +} +} // namespace + RobotStateVisualization::RobotStateVisualization(Ogre::SceneNode* root_node, rviz_common::DisplayContext* context, const std::string& name, rviz_common::properties::Property* parent_property) @@ -134,7 +142,7 @@ void RobotStateVisualization::updateHelper(const moveit::core::RobotStateConstPt rviz_default_plugins::robot::RobotLink* link = robot_.getLink(attached_body->getAttachedLinkName()); if (!link) { - RCLCPP_ERROR_STREAM(LOGGER, "Link " << attached_body->getAttachedLinkName() << " not found in rviz::Robot"); + RCLCPP_ERROR_STREAM(getLogger(), "Link " << attached_body->getAttachedLinkName() << " not found in rviz::Robot"); continue; } Ogre::ColourValue rcolor(color.r, color.g, color.b); diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/test/test_perception.cpp b/moveit_setup_assistant/moveit_setup_app_plugins/test/test_perception.cpp index 00c0474763..3597f261df 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/test/test_perception.cpp +++ b/moveit_setup_assistant/moveit_setup_app_plugins/test/test_perception.cpp @@ -36,8 +36,6 @@ #include #include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_perception"); - using moveit_setup::expectYamlEquivalence; using moveit_setup::getSharePath; using moveit_setup::app::PerceptionConfig; diff --git a/moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp index 48babb454c..d6c0eeb245 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp +++ b/moveit_setup_assistant/moveit_setup_assistant/src/collisions_updater.cpp @@ -90,7 +90,6 @@ int main(int argc, char* argv[]) rclcpp::init(argc, argv); rclcpp::Node::SharedPtr node = std::make_shared("collision_updater"); - static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_updater"); moveit_setup::DataWarehousePtr config_data = std::make_shared(node); moveit_setup::srdf_setup::DefaultCollisions setup_step; @@ -105,18 +104,18 @@ int main(int argc, char* argv[]) } catch (const std::runtime_error& e) { - RCLCPP_ERROR_STREAM(LOGGER, "Could not load config at '" << config_pkg_path << "'. " << e.what()); + RCLCPP_ERROR_STREAM(node->get_logger(), "Could not load config at '" << config_pkg_path << "'. " << e.what()); return 1; } } else if (urdf_path.empty() || srdf_path.empty()) { - RCLCPP_ERROR_STREAM(LOGGER, "Please provide config package or URDF and SRDF path"); + RCLCPP_ERROR_STREAM(node->get_logger(), "Please provide config package or URDF and SRDF path"); return 1; } else if (rdf_loader::RDFLoader::isXacroFile(srdf_path) && output_path.empty()) { - RCLCPP_ERROR_STREAM(LOGGER, "Please provide a different output file for SRDF xacro input file"); + RCLCPP_ERROR_STREAM(node->get_logger(), "Please provide a different output file for SRDF xacro input file"); return 1; } @@ -150,12 +149,12 @@ int main(int argc, char* argv[]) { if (thread_progress - last_progress > 10) { - RCLCPP_INFO(LOGGER, "%d%% complete...", thread_progress); + RCLCPP_INFO(node->get_logger(), "%d%% complete...", thread_progress); last_progress = thread_progress; } } setup_step.joinGenerationThread(); - RCLCPP_INFO(LOGGER, "100%% complete..."); + RCLCPP_INFO(node->get_logger(), "100%% complete..."); size_t skip_mask = 0; if (!include_default) diff --git a/moveit_setup_assistant/moveit_setup_controllers/test/test_controllers.cpp b/moveit_setup_assistant/moveit_setup_controllers/test/test_controllers.cpp index f2233b033c..24398e5d9e 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/test/test_controllers.cpp +++ b/moveit_setup_assistant/moveit_setup_controllers/test/test_controllers.cpp @@ -38,8 +38,6 @@ #include #include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_controllers"); - using moveit_setup::expectYamlEquivalence; using moveit_setup::getSharePath; using moveit_setup::controllers::ControllerInfo; diff --git a/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp b/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp index 37ff291e99..3343b90cf9 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp @@ -37,11 +37,18 @@ #include #include #include // for string find and replace in templates +#include namespace moveit_setup { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit_setup.templates"); +} +} // namespace std::vector TemplatedGeneratedFile::variables; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_setup.templates"); bool TemplatedGeneratedFile::write() { @@ -50,7 +57,7 @@ bool TemplatedGeneratedFile::write() // Error check file if (!std::filesystem::is_regular_file(template_path)) { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to find template file " << template_path.string()); + RCLCPP_ERROR_STREAM(getLogger(), "Unable to find template file " << template_path.string()); return false; } @@ -58,7 +65,7 @@ bool TemplatedGeneratedFile::write() std::ifstream template_stream(template_path); if (!template_stream.good()) // File not found { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to load file " << template_path.string()); + RCLCPP_ERROR_STREAM(getLogger(), "Unable to load file " << template_path.string()); return false; } @@ -84,7 +91,7 @@ bool TemplatedGeneratedFile::write() std::ofstream output_stream(file_path, std::ios_base::trunc); if (!output_stream.good()) { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to open file for writing " << file_path.string()); + RCLCPP_ERROR_STREAM(getLogger(), "Unable to open file for writing " << file_path.string()); return false; } diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp index 7fc7a7da62..7e72724d84 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp @@ -40,11 +40,19 @@ #include #include #include +#include namespace moveit_setup { namespace srdf_setup { +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("collision_updater"); +} +} // namespace // ****************************************************************************************** // Custom Types, Enums and Structs // ****************************************************************************************** @@ -56,9 +64,6 @@ const std::unordered_map REASONS_TO_STRING = boost: const std::unordered_map REASONS_FROM_STRING = boost::assign::map_list_of("Never", NEVER)( "Default", DEFAULT)("Adjacent", ADJACENT)("Always", ALWAYS)("User", USER)("Not Disabled", NOT_DISABLED); -// Used for logging -static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_updater"); - // Unique set of pairs of links in string-based form typedef std::set > StringPairSet; @@ -189,8 +194,6 @@ LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr // LinkGraph is a custom type of a map with a LinkModel as key and a set of LinkModels as second LinkGraph link_graph; - // RCLCPP_INFO_STREAM_STREAM(LOGGER, "Initial allowed Collision Matrix Size = " << scene.getAllowedCollisions().getSize() ); - // 0. GENERATE ALL POSSIBLE LINK PAIRS ------------------------------------------------------------- // Generate a list of unique link pairs for all links with geometry. Order pairs alphabetically. // There should be n choose 2 pairs @@ -231,7 +234,7 @@ LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr // Compute the links that are always in collision unsigned int num_always = disableAlwaysInCollision(*scene, link_pairs, req, links_seen_colliding, min_collision_fraction); - // RCLCPP_INFO_STREAM(LOGGER, "Links seen colliding total = %d", int(links_seen_colliding.size())); + // RCLCPP_INFO_STREAM(getLogger(), "Links seen colliding total = %d", int(links_seen_colliding.size())); *progress = 8; // Progress bar feedback boost::this_thread::interruption_point(); @@ -243,8 +246,6 @@ LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr num_never = disableNeverInCollision(num_trials, *scene, link_pairs, req, links_seen_colliding, progress); } - // RCLCPP_INFO_STREAM(LOGGER, "Link pairs seen colliding ever: %d", int(links_seen_colliding.size())); - if (verbose) { // Calculate number of disabled links: @@ -255,26 +256,20 @@ LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr ++num_disabled; } - RCLCPP_INFO_STREAM(LOGGER, "-------------------------------------------------------------------------------"); - RCLCPP_INFO_STREAM(LOGGER, "Statistics:"); + RCLCPP_INFO_STREAM(getLogger(), "-------------------------------------------------------------------------------"); + RCLCPP_INFO_STREAM(getLogger(), "Statistics:"); unsigned int num_links = int(link_graph.size()); double num_possible = boost::math::binomial_coefficient(num_links, 2); // n choose 2 unsigned int num_sometimes = num_possible - num_disabled; - RCLCPP_INFO_STREAM(LOGGER, "Total Links : " + std::to_string(num_links)); - RCLCPP_INFO_STREAM(LOGGER, "Total possible collisions : " + std::to_string(num_possible)); - RCLCPP_INFO_STREAM(LOGGER, "Always in collision : " + std::to_string(num_always)); - RCLCPP_INFO_STREAM(LOGGER, "Never in collision : " + std::to_string(num_never)); - RCLCPP_INFO_STREAM(LOGGER, "Default in collision : " + std::to_string(num_default)); - RCLCPP_INFO_STREAM(LOGGER, "Adjacent links disabled : " + std::to_string(num_adjacent)); - RCLCPP_INFO_STREAM(LOGGER, "Sometimes in collision : " + std::to_string(num_sometimes)); - RCLCPP_INFO_STREAM(LOGGER, "TOTAL DISABLED : " + std::to_string(num_disabled)); - - /*ROS_INFO("Copy to Spreadsheet:"); - ROS_INFO_STREAM(num_links << "\t" << num_possible << "\t" << num_always << "\t" << num_never - << "\t" << num_default << "\t" << num_adjacent << "\t" << num_sometimes - << "\t" << num_disabled); - */ + RCLCPP_INFO_STREAM(getLogger(), "Total Links : " + std::to_string(num_links)); + RCLCPP_INFO_STREAM(getLogger(), "Total possible collisions : " + std::to_string(num_possible)); + RCLCPP_INFO_STREAM(getLogger(), "Always in collision : " + std::to_string(num_always)); + RCLCPP_INFO_STREAM(getLogger(), "Never in collision : " + std::to_string(num_never)); + RCLCPP_INFO_STREAM(getLogger(), "Default in collision : " + std::to_string(num_default)); + RCLCPP_INFO_STREAM(getLogger(), "Adjacent links disabled : " + std::to_string(num_adjacent)); + RCLCPP_INFO_STREAM(getLogger(), "Sometimes in collision : " + std::to_string(num_sometimes)); + RCLCPP_INFO_STREAM(getLogger(), "TOTAL DISABLED : " + std::to_string(num_disabled)); } *progress = 100; // end the status bar @@ -387,7 +382,6 @@ void computeConnectionGraph(const moveit::core::LinkModel* start_link, LinkGraph } } } - // RCLCPP_INFO_STREAM(LOGGER, "Generated connection graph with %d links", int(link_graph.size())); } // ****************************************************************************************** @@ -412,7 +406,7 @@ void computeConnectionGraphRec(const moveit::core::LinkModel* start_link, LinkGr } else { - RCLCPP_ERROR_STREAM(LOGGER, "Joint exists in URDF with no link!"); + RCLCPP_ERROR_STREAM(getLogger(), "Joint exists in URDF with no link!"); } } @@ -428,8 +422,6 @@ unsigned int disableAdjacentLinks(planning_scene::PlanningScene& scene, LinkGrap for (std::set::const_iterator adj_it = link_graph_it->second.begin(); adj_it != link_graph_it->second.end(); ++adj_it) { - // RCLCPP_INFO_STREAM(LOGGER, "Disabled %s to %s", link_graph_it->first->getName().c_str(), (*adj_it)->getName().c_str() ); - // Check if either of the links have no geometry. If so, do not add (are we sure?) if (!link_graph_it->first->getShapes().empty() && !(*adj_it)->getShapes().empty()) // both links have geometry { @@ -440,7 +432,6 @@ unsigned int disableAdjacentLinks(planning_scene::PlanningScene& scene, LinkGrap } } } - // RCLCPP_INFO_STREAM(LOGGER, "Disabled %d adjacent link pairs from collision checking", num_disabled); return num_disabled; } @@ -468,8 +459,6 @@ unsigned int disableDefaultCollisions(planning_scene::PlanningScene& scene, Link scene.getAllowedCollisionMatrixNonConst().setEntry(it->first.first, it->first.second, true); } - // RCLCPP_INFO_STREAM(LOGGER, "Disabled %d link pairs that are in collision in default state from collision checking", num_disabled); - return num_disabled; } @@ -515,7 +504,6 @@ unsigned int disableAlwaysInCollision(planning_scene::PlanningScene& scene, Link if (nc >= req.max_contacts) { req.max_contacts *= 2; // double the max contacts that the CollisionRequest checks for - // RCLCPP_INFO_STREAM(LOGGER, "Doubling max_contacts to %d", int(req.max_contacts)); } } @@ -542,8 +530,6 @@ unsigned int disableAlwaysInCollision(planning_scene::PlanningScene& scene, Link // if no updates were made to the collision matrix, we stop if (found == 0) done = true; - - // RCLCPP_INFO_STREAM(LOGGER, "Disabled %u link pairs that are always in collision from collision checking", found); } return num_disabled; @@ -561,8 +547,6 @@ unsigned int disableNeverInCollision(const unsigned int num_trials, planning_sce std::mutex lock; // used for sharing the same data structures int num_threads = std::thread::hardware_concurrency(); // how many cores does this computer have? - // RCLCPP_INFO_STREAM_STREAM(LOGGER, "Performing " << num_trials << " trials for 'always in collision' checking on " << - // num_threads << " threads..."); for (int i = 0; i < num_threads; ++i) { @@ -593,7 +577,6 @@ unsigned int disableNeverInCollision(const unsigned int num_trials, planning_sce } } } - // RCLCPP_INFO_STREAM(LOGGER, "Disabled %d link pairs that are never in collision", num_disabled); return num_disabled; } @@ -603,8 +586,6 @@ unsigned int disableNeverInCollision(const unsigned int num_trials, planning_sce // ****************************************************************************************** void disableNeverInCollisionThread(ThreadComputation tc) { - // RCLCPP_INFO_STREAM_STREAM(LOGGER, "Thread " << tc.thread_id_ << " running " << tc.num_trials_ << " trials"); - // User feedback vars const unsigned int progress_interval = tc.num_trials_ / 20; // show progress update every 5% diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/test/test_srdf.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/test/test_srdf.cpp index 299653b3e4..0d5215448c 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/test/test_srdf.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/test/test_srdf.cpp @@ -38,8 +38,6 @@ #include #include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_srdf"); - using moveit_setup::getSharePath; using moveit_setup::SRDFConfig; using moveit_setup::srdf_setup::PlanningGroups; From 87d594540308d682b4b20ff4c0e967db5e7b7739 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Thu, 14 Dec 2023 01:37:29 -0800 Subject: [PATCH 14/17] Make `moveit_servo` listen to Octomap updates (#2601) * Start servo's world geometry monitor * Typo fix --------- Co-authored-by: Amal Nanavati --- moveit_ros/moveit_servo/src/utils/common.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index 2a4a4817b0..dd74f91e72 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -400,6 +400,7 @@ planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const planning_scene_monitor->startStateMonitor(servo_params.joint_topic); planning_scene_monitor->startSceneMonitor(servo_params.monitored_planning_scene_topic); + planning_scene_monitor->startWorldGeometryMonitor(); planning_scene_monitor->setPlanningScenePublishingFrequency(25); planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true); planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, From 08cb716d4e47b9abbc03e5f9f4016589ae29e76b Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 15 Dec 2023 15:55:22 +0100 Subject: [PATCH 15/17] Fix warning and cleanup unneeded placeholders (#2566) * Fix warning and cleanup unneeded placeholders * Make clang-tidy happy * Remove print statement --- .../src/controller_manager_plugin.cpp | 2 -- .../execute_trajectory_action_capability.cpp | 3 --- 2 files changed, 5 deletions(-) diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp index e5fff81211..65d8310b2a 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 @@ -143,8 +143,6 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan // Skip if controller stamp is too new for new discovery, enforce update if force==true if (!force && ((node_->now() - controllers_stamp_) < CONTROLLER_INFORMATION_VALIDITY_AGE)) { - RCLCPP_WARN_STREAM(getLogger(), "Controller information from " << list_controllers_service_->get_service_name() - << " is out of date, skipping discovery"); return; } diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp index 582ec20696..1866128e5e 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp @@ -59,9 +59,6 @@ MoveGroupExecuteTrajectoryAction::MoveGroupExecuteTrajectoryAction() : MoveGroup void MoveGroupExecuteTrajectoryAction::initialize() { - using std::placeholders::_1; - using std::placeholders::_2; - auto node = context_->moveit_cpp_->getNode(); // start the move action server execute_action_server_ = rclcpp_action::create_server( From a20676aaa35e211fa9727ad87f77a3d8826b5e1d Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 15 Dec 2023 19:30:10 +0100 Subject: [PATCH 16/17] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20actions/up?= =?UTF-8?q?load-artifact=20from=203=20to=204=20(#2609)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bumps [actions/upload-artifact](https://github.com/actions/upload-artifact) from 3 to 4. - [Release notes](https://github.com/actions/upload-artifact/releases) - [Commits](https://github.com/actions/upload-artifact/compare/v3...v4) --- updated-dependencies: - dependency-name: actions/upload-artifact dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index a5977f405b..05dbc402f6 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -141,7 +141,7 @@ jobs: run: | testspace "[ ${{ matrix.env.IMAGE }} ]${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml" - name: Upload test artifacts (on failure) - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results) with: name: test-results-${{ matrix.env.IMAGE }} From a84e71ac317510ac72292ad35347cfbb35b2db0a Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 15 Dec 2023 19:32:15 +0100 Subject: [PATCH 17/17] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20actions/se?= =?UTF-8?q?tup-python=20from=204=20to=205=20(#2585)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4 to 5. - [Release notes](https://github.com/actions/setup-python/releases) - [Commits](https://github.com/actions/setup-python/compare/v4...v5) --- updated-dependencies: - dependency-name: actions/setup-python dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Sebastian Jahr --- .github/workflows/format.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 0be2212627..b8c178f046 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -16,7 +16,7 @@ jobs: runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4 + - uses: actions/setup-python@v5 with: python-version: '3.10' - name: Install clang-format-14