From 98288b13ce8e84f5745bc93a31b63982537cb50a Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Thu, 21 Oct 2021 12:11:12 +0200
Subject: [PATCH 001/114] Modernize: std::make_shared

---
 .../src/planning_scene_display.cpp                         | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
index bea40fafa0..44cc719a6f 100644
--- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
+++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
@@ -170,8 +170,7 @@ PlanningSceneDisplay::~PlanningSceneDisplay()
   planning_scene_render_.reset();
   if (context_ && context_->getSceneManager() && planning_scene_node_)
     context_->getSceneManager()->destroySceneNode(planning_scene_node_);
-  if (planning_scene_robot_)
-    planning_scene_robot_.reset();
+  planning_scene_robot_.reset();
   planning_scene_monitor_.reset();
 }
 
@@ -193,8 +192,8 @@ void PlanningSceneDisplay::onInitialize()
 
   if (robot_category_)
   {
-    planning_scene_robot_.reset(
-        new RobotStateVisualization(planning_scene_node_, context_, "Planning Scene", robot_category_));
+    planning_scene_robot_ =
+        std::make_shared<RobotStateVisualization>(planning_scene_node_, context_, "Planning Scene", robot_category_);
     planning_scene_robot_->setVisible(true);
     planning_scene_robot_->setVisualVisible(scene_robot_visual_enabled_property_->getBool());
     planning_scene_robot_->setCollisionVisible(scene_robot_collision_enabled_property_->getBool());

From 27e5b32804b856999f33f6a3a2064ed62ffde46f Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Thu, 21 Oct 2021 12:23:52 +0200
Subject: [PATCH 002/114] Fix ClassLoader: SEVERE WARNING

Clear all references to RobotModel before destroying the corresponding
RobotModelLoader.
---
 .../motion_planning_display.h                     |  1 +
 .../motion_planning_frame.h                       |  1 +
 .../motion_planning_frame_joints_widget.h         |  1 +
 .../src/motion_planning_display.cpp               | 15 +++++++++++++++
 .../src/motion_planning_frame.cpp                 |  7 +++++++
 .../src/motion_planning_frame_joints_widget.cpp   | 12 ++++++++----
 .../planning_scene_display.h                      |  2 +-
 .../src/planning_scene_display.cpp                |  4 ++--
 8 files changed, 36 insertions(+), 7 deletions(-)

diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h
index 0fd36a4088..22bea0bf54 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.h
@@ -196,6 +196,7 @@ private Q_SLOTS:
     OUTSIDE_BOUNDS_LINK
   };
 
+  void clearRobotModel() override;
   void onRobotModelLoaded() override;
   void onNewPlanningSceneState() override;
   void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override;
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h
index 455568c14f..4c58e2fefd 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h
@@ -105,6 +105,7 @@ class MotionPlanningFrame : public QWidget
   MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::DisplayContext* context, QWidget* parent = nullptr);
   ~MotionPlanningFrame() override;
 
+  void clearRobotModel();
   void changePlanningGroup();
   void enable();
   void disable();
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h
index e2de501104..679cf7d1a8 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h
@@ -126,6 +126,7 @@ class MotionPlanningFrameJointsWidget : public QWidget
   MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr);
   ~MotionPlanningFrameJointsWidget() override;
 
+  void clearRobotModel();
   void changePlanningGroup(const std::string& group_name,
                            const robot_interaction::InteractionHandlerPtr& start_state_handler,
                            const robot_interaction::InteractionHandlerPtr& goal_state_handler);
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
index dc16f4c7ac..9a9d65fecc 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
@@ -35,6 +35,7 @@
 /* Author: Ioan Sucan, Dave Coleman, Adam Leeper, Sachin Chitta */
 
 #include <moveit/motion_planning_rviz_plugin/motion_planning_display.h>
+#include <moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h>
 #include <moveit/robot_interaction/kinematic_options_map.h>
 #include <moveit/rviz_plugin_render_tools/planning_link_updater.h>
 #include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
@@ -1139,6 +1140,20 @@ void MotionPlanningDisplay::populateMenuHandler(std::shared_ptr<interactive_mark
   //               boost::bind(&MotionPlanningDisplay::changePlanningGroup, this, group_names[i]));
 }
 
+void MotionPlanningDisplay::clearRobotModel()
+{
+  // Invalidate all references to the RobotModel ...
+  if (frame_)
+    frame_->clearRobotModel();
+  previous_state_.reset();
+  query_start_state_.reset();
+  query_goal_state_.reset();
+  kinematics_metrics_.reset();
+  robot_interaction_.reset();
+  // ... before calling the parent's method, which finally destroys the creating RobotModelLoader.
+  PlanningSceneDisplay::clearRobotModel();
+}
+
 void MotionPlanningDisplay::onRobotModelLoaded()
 {
   PlanningSceneDisplay::onRobotModelLoaded();
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp
index 34082ce602..2006089f07 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp
@@ -425,6 +425,13 @@ void MotionPlanningFrame::changePlanningGroupHelper()
   }
 }
 
+void MotionPlanningFrame::clearRobotModel()
+{
+  ui_->planner_param_treeview->setMoveGroup(moveit::planning_interface::MoveGroupInterfacePtr());
+  joints_tab_->clearRobotModel();
+  move_group_.reset();
+}
+
 void MotionPlanningFrame::changePlanningGroup()
 {
   planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::changePlanningGroupHelper, this),
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp
index 652c2e0e30..fdd7f0fe9f 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp
@@ -198,15 +198,19 @@ MotionPlanningFrameJointsWidget::~MotionPlanningFrameJointsWidget()
   delete ui_;
 }
 
-void MotionPlanningFrameJointsWidget::changePlanningGroup(
-    const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler,
-    const robot_interaction::InteractionHandlerPtr& goal_state_handler)
+void MotionPlanningFrameJointsWidget::clearRobotModel()
 {
-  // release previous models (if any)
   ui_->joints_view_->setModel(nullptr);
   start_state_model_.reset();
   goal_state_model_.reset();
+}
 
+void MotionPlanningFrameJointsWidget::changePlanningGroup(
+    const std::string& group_name, const robot_interaction::InteractionHandlerPtr& start_state_handler,
+    const robot_interaction::InteractionHandlerPtr& goal_state_handler)
+{
+  // release previous models (if any)
+  clearRobotModel();
   // create new models
   start_state_handler_ = start_state_handler;
   goal_state_handler_ = goal_state_handler;
diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h
index 94cc27ff32..959aca2d4f 100644
--- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h
+++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.h
@@ -142,7 +142,7 @@ protected Q_SLOTS:
 
   /// This function is used by loadRobotModel() and should only be called in the MainLoop
   /// You probably should not call this function directly
-  void clearRobotModel();
+  virtual void clearRobotModel();
 
   /// This function constructs a new planning scene. Probably this should be called in a background thread
   /// as it may take some time to complete its execution
diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
index 44cc719a6f..8aa6630e7b 100644
--- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
+++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
@@ -516,8 +516,8 @@ planning_scene_monitor::PlanningSceneMonitorPtr PlanningSceneDisplay::createPlan
 void PlanningSceneDisplay::clearRobotModel()
 {
   planning_scene_render_.reset();
-  planning_scene_monitor_.reset();  // this so that the destructor of the PlanningSceneMonitor gets called before a new
-                                    // instance of a scene monitor is constructed
+  // Ensure old PSM is destroyed before we attempt to create a new one
+  planning_scene_monitor_.reset();
 }
 
 void PlanningSceneDisplay::loadRobotModel()

From 8d2f96120662730c69b3de2c829e72b6f6259815 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Wed, 3 Nov 2021 18:25:53 +0100
Subject: [PATCH 003/114] Reset markers on display_contacts topic after a new
 planning attempt

---
 .../planning/planning_pipeline/src/planning_pipeline.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
index 0558a605ab..07cb9874c1 100644
--- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
+++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
@@ -264,6 +264,11 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
     ROS_DEBUG_STREAM("Motion planner reported a solution path with " << state_count << " states");
     if (check_solution_paths_)
     {
+      visualization_msgs::MarkerArray arr;
+      visualization_msgs::Marker m;
+      m.action = visualization_msgs::Marker::DELETEALL;
+      arr.markers.push_back(m);
+
       std::vector<std::size_t> index;
       if (!planning_scene->isPathValid(*res.trajectory_, req.path_constraints, req.group_name, false, &index))
       {
@@ -301,7 +306,6 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
                              << private_nh_.resolveName(MOTION_CONTACTS_TOPIC));
 
             // call validity checks in verbose mode for the problematic states
-            visualization_msgs::MarkerArray arr;
             for (std::size_t it : index)
             {
               // check validity with verbose on
@@ -325,8 +329,6 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
               }
             }
             ROS_ERROR_STREAM("Completed listing of explanations for invalid states.");
-            if (!arr.markers.empty())
-              contacts_publisher_.publish(arr);
           }
         }
         else
@@ -335,6 +337,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
       }
       else
         ROS_DEBUG("Planned path was found to be valid when rechecked");
+      contacts_publisher_.publish(arr);
     }
   }
 

From 776ff5366a5f6adec021d8d3e480b7118502e689 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 5 Nov 2021 16:56:57 +0100
Subject: [PATCH 004/114] Enable compiler warnings

---
 .github/workflows/ci.yaml | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index 9909cc1b62..08d756148d 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -27,6 +27,8 @@ jobs:
           - IMAGE: master-ci # ROS Melodic with all dependencies required for master
             CATKIN_LINT: true
     env:
+      CXXFLAGS: >-
+        -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
       CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
       DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }}
       UPSTREAM_WORKSPACE: .github/workflows/upstream.rosinstall
@@ -37,7 +39,8 @@ jobs:
       AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src
       TARGET_CMAKE_ARGS: >
         -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}}
-        -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}"
+        -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}$CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}"
+      UPSTREAM_CMAKE_ARGS: "-DCMAKE_CXX_FLAGS=''"
       DOWNSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-Wall -Wextra"
       CCACHE_DIR: ${{ github.workspace }}/.ccache
       BASEDIR: ${{ github.workspace }}/.work

From 5795c6ce52338b963fe8fcf5505f01cf1a7ecfde Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 5 Nov 2021 17:02:11 +0100
Subject: [PATCH 005/114] Use catkin_tools_devel builder from industrial_ci

https://github.com/ros-industrial/industrial_ci/pull/707
---
 .github/workflows/catkin_tools_devel.sh | 15 ---------------
 .github/workflows/ci.yaml               |  2 +-
 2 files changed, 1 insertion(+), 16 deletions(-)
 delete mode 100644 .github/workflows/catkin_tools_devel.sh

diff --git a/.github/workflows/catkin_tools_devel.sh b/.github/workflows/catkin_tools_devel.sh
deleted file mode 100644
index fa1c4f597e..0000000000
--- a/.github/workflows/catkin_tools_devel.sh
+++ /dev/null
@@ -1,15 +0,0 @@
-#!/bin/bash
-
-# Custome BUILDER to build in devel-space only
-
-BUILDER=catkin_tools ici_source_builder
-
-function ici_extend_space {
-  echo "$1/devel"
-}
-
-function _catkin_config {
-    local extend=$1; shift
-    local ws=$1; shift
-    ici_exec_in_workspace "$extend" "$ws" catkin config --init
-}
diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index 08d756148d..4d80800a55 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -19,7 +19,7 @@ jobs:
         env:
           - IMAGE: noetic-ci
             CCOV: true
-            BUILDER: .github/workflows/catkin_tools_devel.sh
+            BUILDER: catkin_tools_devel
           - IMAGE: noetic-ci-shadow-fixed
             IKFAST_TEST: true
             CATKIN_LINT: true

From 1e18c60e4680808276a4103138c6f9a2a471b175 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 5 Nov 2021 17:18:25 +0100
Subject: [PATCH 006/114] Add comments

---
 .github/workflows/ci.yaml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index 4d80800a55..3841c3d2fe 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -37,6 +37,7 @@ jobs:
       # Pull any updates to the upstream workspace (after restoring it from cache)
       AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src
       AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src
+      # Compile CCOV with Debug. Enable -Werror (except CLANG_TIDY=pedantic, which makes the clang-tidy step fail on warnings)
       TARGET_CMAKE_ARGS: >
         -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}}
         -DCMAKE_CXX_FLAGS="${{ matrix.env.CLANG_TIDY != 'pedantic' && '-Werror ' || '' }}$CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}"

From 0785f1ea46f3fa3fdda1273ac62bf718ce6cc8fe Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 5 Nov 2021 17:49:50 +0100
Subject: [PATCH 007/114] Use test_environment.launch in unittests

---
 .../test/rrbot_move_group.launch              |  5 ++-
 .../integrationtest_command_planning.test     | 14 +-------
 ...st_command_planning_frankaemika_panda.test | 14 +-------
 ...iontest_command_planning_with_gripper.test | 12 +------
 ...ationtest_sequence_service_capability.test | 14 +-------
 ..._service_capability_frankaemika_panda.test | 14 +-------
 ...uence_service_capability_with_gripper.test | 12 +------
 .../launch/moveit_planning_execution.launch   | 33 ++-----------------
 .../test/move_group_interface_cpp_test.test   | 24 +-------------
 .../test/move_group_pick_place_test.test      | 24 +-------------
 .../test/subframes_test.test                  | 24 +-------------
 11 files changed, 13 insertions(+), 177 deletions(-)

diff --git a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch
index ab54cf53fc..56100a65ff 100644
--- a/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch
+++ b/moveit_planners/chomp/chomp_interface/test/rrbot_move_group.launch
@@ -27,9 +27,8 @@
   <!-- Given the published joint states, publish tf for the robot links -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
 
- <!-- move_group settings -->
-  <arg name="allow_trajectory_execution" default="true"/>
-  <arg name="fake_execution" default="true"/>
+  <!-- move_group settings -->
+  <arg name="allow_trajectory_execution" default="true" />
   <arg name="max_safe_path_cost" default="1"/>
   <arg name="jiggle_fraction" default="0.05" />
   <arg name="publish_monitored_planning_scene" default="true"/>
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test
index ba73e8b56b..6d26187e4d 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning.test
@@ -34,19 +34,7 @@ POSSIBILITY OF SUCH DAMAGE.
 
 <launch>
 
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="/use_gui" value="false"/>
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-  </node>
-
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
-  <arg name="debug" default="false"/>
-  <include file="$(find moveit_resources_prbt_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/test_environment.launch">
     <arg name="pipeline" value="pilz_industrial_motion_planner" />
   </include>
 
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test
index d360fbcafa..44f7d54b3d 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_frankaemika_panda.test
@@ -34,19 +34,7 @@ POSSIBILITY OF SUCH DAMAGE.
 
 <launch>
 
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="/use_gui" value="false"/>
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-  </node>
-
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
-  <arg name="debug" default="false"/>
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch">
     <arg name="pipeline" value="pilz_industrial_motion_planner" />
   </include>
 
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test
index 64fb904bde..27212a2b92 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_planning_with_gripper.test
@@ -34,17 +34,7 @@ POSSIBILITY OF SUCH DAMAGE.
 
 <launch>
 
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="/use_gui" value="false"/>
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-  </node>
-
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
-  <include file="$(find moveit_resources_prbt_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/test_environment.launch">
     <arg name="gripper" value="pg70"/>
     <arg name="pipeline" value="pilz_industrial_motion_planner" />
   </include>
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test
index 9eec6f308b..049cb5a253 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.test
@@ -33,19 +33,7 @@ POSSIBILITY OF SUCH DAMAGE.
 -->
 
 <launch>
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="/use_gui" value="false"/>
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-  </node>
-
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
-  <arg name="debug" default="false"/>
-  <include file="$(find moveit_resources_prbt_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/test_environment.launch">
     <arg name="pipeline" value="pilz_industrial_motion_planner" />
   </include>
 
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test
index dbec926599..604d6ace07 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_frankaemika_panda.test
@@ -34,19 +34,7 @@ POSSIBILITY OF SUCH DAMAGE.
 
 <launch>
 
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="/use_gui" value="false"/>
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-  </node>
-
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
-  <arg name="debug" default="false"/>
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch">
     <arg name="pipeline" value="pilz_industrial_motion_planner" />
   </include>
 
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test
index f214d4baf7..0c516453ac 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability_with_gripper.test
@@ -33,17 +33,7 @@ POSSIBILITY OF SUCH DAMAGE.
 -->
 
 <launch>
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <param name="/use_gui" value="false"/>
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-  </node>
-
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
-
-  <include file="$(find moveit_resources_prbt_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
+  <include file="$(find moveit_resources_prbt_moveit_config)/launch/test_environment.launch">
     <arg name="gripper" value="pg70"/>
     <arg name="pipeline" value="pilz_industrial_motion_planner" />
   </include>
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch
index f8ccaa62a3..134a32ba3e 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch
+++ b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch
@@ -37,38 +37,9 @@ POSSIBILITY OF SUCH DAMAGE.
 <!-- simulated environment for testing pilz_industrial_motion_planner with a panda robot -->
 
 <launch>
-  <!-- The planning and execution components of MoveIt configured to run -->
-  <!-- using the ROS-Industrial interface. -->
 
-  <!-- Choose planning pipeline -->
-  <arg name="pipeline" default="ompl" />
-
-  <!-- Load robot description to parameter server. Can be set to false to let someone else load the model -->
-  <arg name="load_robot_description" default="true" />
-
-  <!-- load the robot_description parameter before launching ROS-I nodes -->
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch" >
-    <arg name="load_robot_description" value="$(arg load_robot_description)" />
-    <arg name="load_gripper" value="true" />
-  </include>
-
-  <!-- run the robot simulator and action interface nodes -->
-  <group>
-    <!-- publish the robot state (tf transforms) -->
-    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
-
-    <!-- We do not have a robot connected, so publish fake joint states -->
-    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-      <param name="/use_gui" value="false"/>
-      <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-    </node>
-  </group>
-
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/move_group.launch">
-    <arg name="publish_monitored_planning_scene" value="true" />
-    <arg name="fake_execution" value="true"/>
-    <arg name="pipeline" value="$(arg pipeline)" />
-    <arg name="load_gripper" value="true" />
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch">
+    <arg name="pipeline" value="ompl" />
   </include>
 
   <include file="$(find moveit_resources_panda_moveit_config)/launch/moveit_rviz.launch">
diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test
index 9975f257d2..02d1f7201c 100644
--- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test
+++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.test
@@ -1,27 +1,5 @@
 <launch>
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
-  <!-- If needed, broadcast static tf for robot root -->
-  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" />
-
-  <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-  </node>
-  <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
-
-  <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
-
-  <!-- Run the main MoveIt executable with fake trajectory execution -->
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
-  </include>
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch" />
 
   <!-- test -->
   <test pkg="moveit_ros_planning_interface" type="move_group_interface_cpp_test" test-name="move_group_interface_cpp_test"
diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.test b/moveit_ros/planning_interface/test/move_group_pick_place_test.test
index 9dd7dbdcc3..bf20f60b5d 100644
--- a/moveit_ros/planning_interface/test/move_group_pick_place_test.test
+++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.test
@@ -1,27 +1,5 @@
 <launch>
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
-  <!-- If needed, broadcast static tf for robot root -->
-  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" />
-
-  <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-  </node>
-  <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
-
-  <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
-
-  <!-- Run the main MoveIt executable with fake trajectory execution -->
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
-  </include>
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch" />
 
   <!-- test -->
   <test pkg="moveit_ros_planning_interface" type="move_group_pick_place_test" test-name="move_group_pick_place_test"
diff --git a/moveit_ros/planning_interface/test/subframes_test.test b/moveit_ros/planning_interface/test/subframes_test.test
index 761b8210ad..d1fca4b53a 100644
--- a/moveit_ros/planning_interface/test/subframes_test.test
+++ b/moveit_ros/planning_interface/test/subframes_test.test
@@ -1,27 +1,5 @@
 <launch>
-  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
-  <!-- If needed, broadcast static tf for robot root -->
-  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" />
-
-  <!-- We do not have a robot connected, so publish fake joint states -->
-  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
-    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
-  </node>
-  <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
-
-  <!-- Given the published joint states, publish tf for the robot links -->
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
-
-  <!-- Run the main MoveIt executable with fake trajectory execution -->
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="fake_execution" value="true"/>
-    <arg name="info" value="true"/>
-  </include>
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch" />
 
   <!-- Subframes test -->
   <test pkg="moveit_ros_planning_interface" type="subframes_test" test-name="subframes_test"

From 98767ef5a7b9e0215be8ea7c774a17a605414fc9 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 5 Nov 2021 18:07:30 +0100
Subject: [PATCH 008/114] Remove unused moveit_planning_execution.launch

---
 .../acceptance_tests/acceptance_test_lin.md   |  2 +-
 .../acceptance_tests/acceptance_test_ptp.md   |  2 +-
 .../launch/moveit_planning_execution.launch   | 48 -------------------
 3 files changed, 2 insertions(+), 50 deletions(-)
 delete mode 100644 moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch

diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md
index 4f2abec293..867c7f6587 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md
+++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md
@@ -41,7 +41,7 @@ to be moved out of singularities.
 
 ## Test Sequence:
   1. Bringup can: `sudo ip link set can0 up type can bitrate 1000000`
-  2. Run `roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=False pipeline:=pilz_industrial_motion_planner`
+  2. Run `roslaunch moveit_resources_prbt_moveit_config demo.launch pipeline:=pilz_industrial_motion_planner`
   3. In the motion planing widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image)
 ![moveit_1](img/acceptance_test_lin_img1.png)
   4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle to select a singularity free position. Click on "plan and execute".
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md
index 732ec8ec4f..be2dce5489 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md
+++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_ptp.md
@@ -40,7 +40,7 @@ This test checks that the real robot system is able to perform a PTP Motion to a
 
 ## Test Sequence:
   1. Bringup can: `sudo ip link set can0 up type can bitrate 1000000`
-  2. Run `roslaunch moveit_resources_prbt_moveit_config moveit_planning_execution.launch sim:=False pipeline:=pilz_industrial_motion_planner`
+  2. Run `roslaunch moveit_resources_prbt_moveit_config demo.launch pipeline:=pilz_industrial_motion_planner`
   3. The motion planning widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image)
 ![moveit_1](img/acceptance_test_ptp_img1.png)
   4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle the select goal pose. Click on "plan and execute".
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch b/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch
deleted file mode 100644
index 134a32ba3e..0000000000
--- a/moveit_planners/pilz_industrial_motion_planner/test/test_robots/frankaemika_panda/launch/moveit_planning_execution.launch
+++ /dev/null
@@ -1,48 +0,0 @@
-<?xml version="1.0"?>
-
-<!--
-Software License Agreement (BSD License)
-
-Copyright (c) 2018 Pilz GmbH & Co. KG
-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 Pilz GmbH & Co. KG 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.
--->
-
-<!-- simulated environment for testing pilz_industrial_motion_planner with a panda robot -->
-
-<launch>
-
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch">
-    <arg name="pipeline" value="ompl" />
-  </include>
-
-  <include file="$(find moveit_resources_panda_moveit_config)/launch/moveit_rviz.launch">
-  </include>
-
-</launch>

From e30c6fa853dbc699c0cc84e420626000eefe3212 Mon Sep 17 00:00:00 2001
From: Tom Noble <tom@additiveautomations.com>
Date: Sat, 6 Nov 2021 00:22:51 +0100
Subject: [PATCH 009/114] Fix orientation of subframe offset in Pilz planners
 (#2890)

Fix #2879 by reorienting the subframe offset applied to a goal pose in the PTP planner,
so that it is applied in the correct direction.
Also add the same offset computation to LIN and CIRC generators,
thus allowing them to work with subframes.

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>
---
 .../trajectory_functions.h                    | 17 +++++++++++
 .../src/trajectory_functions.cpp              | 23 +++++++++++++++
 .../src/trajectory_generator_circ.cpp         | 29 +++++++++++--------
 .../src/trajectory_generator_lin.cpp          |  7 +----
 .../src/trajectory_generator_ptp.cpp          | 17 ++---------
 5 files changed, 61 insertions(+), 32 deletions(-)

diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h
index fc78cf994b..75be0fa56b 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h
@@ -220,3 +220,20 @@ bool isStateColliding(const bool test_for_self_collision, const planning_scene::
 }  // namespace pilz_industrial_motion_planner
 
 void normalizeQuaternion(geometry_msgs::Quaternion& quat);
+
+/**
+ * @brief Adapt goal pose, defined by position+orientation, to consider offset
+ * @param constraint to apply offset to
+ * @param offset to apply to the constraint
+ * @param orientation to apply to the offset
+ * @return final goal pose
+ */
+Eigen::Isometry3d getConstraintPose(const geometry_msgs::Point& position, const geometry_msgs::Quaternion& orientation,
+                                    const geometry_msgs::Vector3& offset);
+
+/**
+ * @brief Conviencency method, passing args from a goal constraint
+ * @param goal goal constraint
+ * @return final goal pose
+ */
+Eigen::Isometry3d getConstraintPose(const moveit_msgs::Constraints& goal);
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 3810fca709..93ba27e133 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp
@@ -585,3 +585,26 @@ void normalizeQuaternion(geometry_msgs::Quaternion& quat)
   tf2::fromMsg(quat, q);
   quat = tf2::toMsg(q.normalize());
 }
+
+Eigen::Isometry3d getConstraintPose(const geometry_msgs::Point& position, const geometry_msgs::Quaternion& orientation,
+                                    const geometry_msgs::Vector3& offset)
+{
+  Eigen::Quaterniond quat;
+  tf2::fromMsg(orientation, quat);
+  quat.normalize();
+  Eigen::Vector3d v;
+  tf2::fromMsg(position, v);
+
+  Eigen::Isometry3d pose = Eigen::Translation3d(v) * quat;
+
+  tf2::fromMsg(offset, v);
+  pose.translation() -= quat * v;
+  return pose;
+}
+
+Eigen::Isometry3d getConstraintPose(const moveit_msgs::Constraints& goal)
+{
+  return getConstraintPose(goal.position_constraints.front().constraint_region.primitive_poses.front().position,
+                           goal.orientation_constraints.front().orientation,
+                           goal.position_constraints.front().target_point_offset);
+}
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 44115f6e2e..c949793b54 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
@@ -142,12 +142,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
     {
       frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
     }
-    geometry_msgs::Pose goal_pose_msg;
-    goal_pose_msg.position =
-        req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position;
-    goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation;
-    normalizeQuaternion(goal_pose_msg.orientation);
-    tf2::fromMsg(goal_pose_msg, info.goal_pose);
+    info.goal_pose = getConstraintPose(req.goal_constraints.front());
   }
 
   assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size());
@@ -179,13 +174,23 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
     // LCOV_EXCL_STOP // not able to trigger here since lots of checks before
     // are in place
   }
-
-  Eigen::Vector3d circ_path_point;
-  tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
-               circ_path_point);
-
   info.circ_path_point.first = req.path_constraints.name;
-  info.circ_path_point.second = circ_path_point;
+  if (!req.goal_constraints.front().position_constraints.empty())
+  {
+    const moveit_msgs::Constraints& goal = req.goal_constraints.front();
+    info.circ_path_point.second =
+        getConstraintPose(
+            req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
+            goal.orientation_constraints.front().orientation, goal.position_constraints.front().target_point_offset)
+            .translation();
+  }
+  else
+  {
+    Eigen::Vector3d circ_path_point;
+    tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
+                 circ_path_point);
+    info.circ_path_point.second = circ_path_point;
+  }
 }
 
 void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene,
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 dede65781f..b2052ed8af 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
@@ -116,12 +116,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
     {
       frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
     }
-    geometry_msgs::Pose goal_pose_msg;
-    goal_pose_msg.position =
-        req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position;
-    goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation;
-    normalizeQuaternion(goal_pose_msg.orientation);
-    tf2::fromMsg(goal_pose_msg, info.goal_pose);
+    info.goal_pose = getConstraintPose(req.goal_constraints.front());
   }
 
   assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size());
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 d1dc58f1c2..ebb35c4b4f 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
@@ -221,23 +221,12 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin
       info.goal_joint_position[joint_constraint.joint_name] = joint_constraint.position;
     }
   }
-  // slove the ik
+  // solve the ik
   else
   {
-    geometry_msgs::Point p =
-        req.goal_constraints.at(0).position_constraints.at(0).constraint_region.primitive_poses.at(0).position;
-    p.x -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.x;
-    p.y -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.y;
-    p.z -= req.goal_constraints.at(0).position_constraints.at(0).target_point_offset.z;
-
-    geometry_msgs::Pose pose;
-    pose.position = p;
-    pose.orientation = req.goal_constraints.at(0).orientation_constraints.at(0).orientation;
-    Eigen::Isometry3d pose_eigen;
-    normalizeQuaternion(pose.orientation);
-    tf2::fromMsg(pose, pose_eigen);
+    Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front());
     if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name,
-                       pose_eigen, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position))
+                       goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position))
     {
       throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose");
     }

From 304ae6acdf9bb3994ad7f7315c23048ca6c72f7e Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sat, 6 Nov 2021 06:15:18 +0100
Subject: [PATCH 010/114] CI: rename cross_platform_ci -> robostack

---
 .github/{ci_cross_platform_env.yml => robostack_env.yaml}  | 0
 .../workflows/{cross_platform_ci.yml => robostack.yaml}    | 7 +++----
 2 files changed, 3 insertions(+), 4 deletions(-)
 rename .github/{ci_cross_platform_env.yml => robostack_env.yaml} (100%)
 rename .github/workflows/{cross_platform_ci.yml => robostack.yaml} (95%)

diff --git a/.github/ci_cross_platform_env.yml b/.github/robostack_env.yaml
similarity index 100%
rename from .github/ci_cross_platform_env.yml
rename to .github/robostack_env.yaml
diff --git a/.github/workflows/cross_platform_ci.yml b/.github/workflows/robostack.yaml
similarity index 95%
rename from .github/workflows/cross_platform_ci.yml
rename to .github/workflows/robostack.yaml
index c43ed62bd7..1840976190 100644
--- a/.github/workflows/cross_platform_ci.yml
+++ b/.github/workflows/robostack.yaml
@@ -1,4 +1,4 @@
-name: MoveIt core cross-platform RoboStack build
+name: RoboStack
 
 on:
   workflow_dispatch:
@@ -21,9 +21,9 @@ jobs:
     - uses: actions/checkout@v2
 
     - name: Set up Build Dependencies
-      uses: mamba-org/provision-with-micromamba@v10
+      uses: mamba-org/provision-with-micromamba@v11
       with:
-        environment-file: .github/ci_cross_platform_env.yml
+        environment-file: .github/robostack_env.yaml
         micromamba-version: 0.17.0
 
     - name: Set up MoveIt Core Dependencies on Unix
@@ -70,7 +70,6 @@ jobs:
       if: runner.os == 'Windows'
       shell: cmd
       run: |
-
         echo "Remove unnecessary / colliding things from PATH"
         set "PATH=%PATH:C:\ProgramData\Chocolatey\bin;=%"
         set "PATH=%PATH:C:\Program Files (x86)\sbt\bin;=%"

From 331aff5de359b50a21794fcaf781a0e238a2ee68 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sat, 6 Nov 2021 06:57:45 +0100
Subject: [PATCH 011/114] CI: prerelease.yaml: only allow manual triggering

---
 .github/workflows/prerelease.yaml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/.github/workflows/prerelease.yaml b/.github/workflows/prerelease.yaml
index 5c8b6b8f94..06a7214db8 100644
--- a/.github/workflows/prerelease.yaml
+++ b/.github/workflows/prerelease.yaml
@@ -5,7 +5,6 @@ name: pre-release
 
 on:
   workflow_dispatch:
-  push:
 
 jobs:
   default:

From 568154498ac04ce1911007e95d3790d080403557 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sat, 6 Nov 2021 07:54:00 +0100
Subject: [PATCH 012/114] README.md: Update badges

---
 README.md                             | 97 ++++++++++++---------------
 moveit/scripts/create_readme_table.py |  5 +-
 2 files changed, 47 insertions(+), 55 deletions(-)

diff --git a/README.md b/README.md
index 073957755b..65e99faa75 100644
--- a/README.md
+++ b/README.md
@@ -11,8 +11,8 @@
 
 ## Branches Policy
 
-- We develop latest features on ``master``.
-- The ``*-devel`` branches correspond to released and stable versions of MoveIt for specific distributions of ROS.
+- We develop latest features on `master`.
+- The `*-devel` branches correspond to released and stable versions of MoveIt for specific distributions of ROS. `noetic-devel` is synced to `master` currently.
 - Bug fixes occasionally get backported to these released versions of MoveIt.
 - For MoveIt 2 development, see [moveit2](https://github.com/ros-planning/moveit2).
 
@@ -20,65 +20,56 @@
 
 ### Continuous Integration
 
-service    | Kinetic | Melodic | Master
----------- | ------- | ------- | ------
-GitHub | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=kinetic-devel)](https://github.https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Akinetic-devel) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=kinetic-devel)](https://github.https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Akinetic-devel) | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=melodic-devel)](https://github.https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Amelodic-devel) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=melodic-devel)](https://github.https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Amelodic-devel) | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=master)](https://github.https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Amaster) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=master)](https://github.https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Amaster) |
-CodeCov | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/kinetic-devel/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/melodic-devel/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/master/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) |
-build farm | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__moveit__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdev__moveit__ubuntu_xenial_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mdev__moveit__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__moveit__ubuntu_bionic_amd64) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ndev__moveit__ubuntu_focal_amd64)](https://build.ros.org/job/Ndev__moveit__ubuntu_focal_amd64/) |
+service    | Melodic | Master
+---------- | ------- | ------
+GitHub | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=melodic-devel)](https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Amelodic-devel) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=melodic-devel)](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Amelodic-devel) | [![Format](https://github.com/ros-planning/moveit/actions/workflows/format.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit/actions/workflows/format.yaml?query=branch%3Amaster) [![CI](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit/actions/workflows/ci.yaml?query=branch%3Amaster) |
+CodeCov | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/melodic-devel/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) | [![codecov](https://codecov.io/gh/ros-planning/moveit/branch/master/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit) |
+build farm | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mdev__moveit__ubuntu_bionic_amd64)](https://build.ros.org/job/Mdev__moveit__ubuntu_bionic_amd64) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Ndev__moveit__ubuntu_focal_amd64)](https://build.ros.org/job/Ndev__moveit__ubuntu_focal_amd64/) |
+| [![Docker](https://img.shields.io/docker/stars/moveit/moveit.svg)](https://registry.hub.docker.com/moveit/moveit)<br>[![Pulls](https://img.shields.io/docker/pulls/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit) | [![automated build](https://img.shields.io/docker/automated/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit) | [![docker](https://github.com/ros-planning/moveit/actions/workflows/docker.yaml/badge.svg?branch=master)](https://github.com/ros-planning/moveit/actions/workflows/docker.yaml?query=branch%3Amaster) |
 
 ### Licenses
 
 [![FOSSA Status](https://app.fossa.com/api/projects/git%2Bgithub.com%2Fros-planning%2Fmoveit.svg?type=shield)](https://app.fossa.com/projects/git%2Bgithub.com%2Fros-planning%2Fmoveit?ref=badge_shield)
 
-### Docker Containers
-
-[![Docker Build](https://img.shields.io/docker/build/moveit/moveit.svg)](https://hub.docker.com/r/moveit/moveit/builds)
-[![Docker Automated build](https://img.shields.io/docker/automated/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit/) [![Docker Pulls](https://img.shields.io/docker/pulls/moveit/moveit.svg?maxAge=2592000)](https://hub.docker.com/r/moveit/moveit/) [![Docker Stars](https://img.shields.io/docker/stars/moveit/moveit.svg)](https://registry.hub.docker.com/moveit/moveit/)
 
 ### ROS Buildfarm
 
-MoveIt Package | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian | Noetic Source | Noetic Debian
--------------- | -------------- | -------------- | -------------- | -------------- | ------------- | -------------
-moveit | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit__ubuntu_focal_amd64__binary)
-moveit_chomp_optimizer_adapter | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary)
-moveit_commander | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_commander__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_commander__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_commander__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_commander__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_commander__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_commander__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary)
-moveit_core | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_core__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_core__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_core__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_core__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_core__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_core__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary)
-moveit_fake_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary)
-moveit_grasps |  |  |  |  | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_grasps__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_grasps__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_grasps__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_grasps__ubuntu_focal_amd64__binary)
-moveit_jog_arm |  |  |  |  | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_jog_arm__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_jog_arm__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_jog_arm__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_jog_arm__ubuntu_focal_amd64__binary)
-moveit_kinematics | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_kinematics__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_kinematics__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_kinematics__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_kinematics__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_kinematics__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_kinematics__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary)
-moveit_msgs | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_msgs__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_msgs__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_msgs__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_msgs__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_msgs__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_msgs__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_msgs__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_msgs__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary)
-moveit_planners | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary)
-moveit_planners_chomp | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary)
-moveit_planners_ompl | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary)
-moveit_plugins | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_plugins__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_plugins__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_plugins__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_plugins__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_plugins__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_plugins__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary)
-moveit_resources | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_resources__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_resources__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_resources__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_resources__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_resources__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_resources__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_resources__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_resources__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_resources__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_resources__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_resources__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_resources__ubuntu_focal_amd64__binary)
-moveit_ros | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary)
-moveit_ros_benchmarks | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary)
-moveit_ros_control_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary)
-moveit_ros_manipulation | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary)
-moveit_ros_move_group | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary)
-moveit_ros_occupancy_map_monitor |  |  | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary)
-moveit_ros_perception | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_perception__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_perception__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_perception__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_perception__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary)
-moveit_ros_planning | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary)
-moveit_ros_planning_interface | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary)
-moveit_ros_robot_interaction | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary)
-moveit_ros_visualization | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary)
-moveit_ros_warehouse | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary)
-moveit_runtime | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_runtime__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_runtime__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_runtime__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_runtime__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_runtime__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_runtime__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary)
-moveit_setup_assistant | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary)
-moveit_simple_controller_manager | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary)
-moveit_task_constructor_capabilities |  |  |  |  | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_capabilities__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_capabilities__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_capabilities__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_capabilities__ubuntu_focal_amd64__binary)
-moveit_task_constructor_core |  |  |  |  | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_core__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_core__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_core__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_core__ubuntu_focal_amd64__binary)
-moveit_task_constructor_demo |  |  |  |  | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_demo__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_demo__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_demo__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_demo__ubuntu_focal_amd64__binary)
-moveit_task_constructor_msgs |  |  |  |  | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_msgs__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_msgs__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_msgs__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_msgs__ubuntu_focal_amd64__binary)
-moveit_task_constructor_visualization |  |  |  |  | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_task_constructor_visualization__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_task_constructor_visualization__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_task_constructor_visualization__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_task_constructor_visualization__ubuntu_focal_amd64__binary)
-moveit_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_visual_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_visual_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary)
-chomp_motion_planner | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__chomp_motion_planner__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__chomp_motion_planner__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__chomp_motion_planner__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary)
-geometric_shapes | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__geometric_shapes__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__geometric_shapes__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__geometric_shapes__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__geometric_shapes__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__geometric_shapes__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__geometric_shapes__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)
-rviz_marker_tools |  |  |  |  | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__rviz_marker_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__rviz_marker_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__rviz_marker_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__rviz_marker_tools__ubuntu_focal_amd64__binary)
-rviz_visual_tools | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__rviz_visual_tools__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__rviz_visual_tools__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__rviz_visual_tools__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__rviz_visual_tools__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__rviz_visual_tools__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__rviz_visual_tools__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__rviz_visual_tools__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__rviz_visual_tools__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__rviz_visual_tools__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__rviz_visual_tools__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__rviz_visual_tools__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__rviz_visual_tools__ubuntu_focal_amd64__binary)
-srdfdom | [![Build Status](http://build.ros.org/buildStatus/icon?job=Ksrc_uX__srdfdom__ubuntu_xenial__source)](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__srdfdom__ubuntu_xenial__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary)](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Msrc_uB__srdfdom__ubuntu_bionic__source)](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__srdfdom__ubuntu_bionic__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary)](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nsrc_uF__srdfdom__ubuntu_focal__source)](http://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__srdfdom__ubuntu_focal__source) | [![Build Status](http://build.ros.org/buildStatus/icon?job=Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary)](http://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary)
+MoveIt Package | Melodic Source | Melodic Debian | Noetic Source | Noetic Debian
+-------------- | -------------- | -------------- | ------------- | -------------
+**moveit** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit__ubuntu_focal_amd64__binary)
+moveit_core | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_core__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_core__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_core__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_core__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_core__ubuntu_focal_amd64__binary)
+moveit_kinematics | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_kinematics__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_kinematics__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_kinematics__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_kinematics__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_kinematics__ubuntu_focal_amd64__binary)
+**moveit_planners** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners__ubuntu_focal_amd64__binary)
+moveit_planners_ompl | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_ompl__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_ompl__ubuntu_focal_amd64__binary)
+moveit_planners_chomp | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_planners_chomp__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_planners_chomp__ubuntu_focal_amd64__binary)
+chomp_motion_planner | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__chomp_motion_planner__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__chomp_motion_planner__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__chomp_motion_planner__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__chomp_motion_planner__ubuntu_focal_amd64__binary)
+moveit_chomp_optimizer_adapter | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_chomp_optimizer_adapter__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_chomp_optimizer_adapter__ubuntu_focal_amd64__binary)
+pilz_industrial_motion_planner | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__pilz_industrial_motion_planner__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__pilz_industrial_motion_planner__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__pilz_industrial_motion_planner__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__pilz_industrial_motion_planner__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__pilz_industrial_motion_planner__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__pilz_industrial_motion_planner__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__pilz_industrial_motion_planner__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__pilz_industrial_motion_planner__ubuntu_focal_amd64__binary)
+pilz_industrial_motion_planner_testutils | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__pilz_industrial_motion_planner_testutils__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__pilz_industrial_motion_planner_testutils__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__pilz_industrial_motion_planner_testutils__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__pilz_industrial_motion_planner_testutils__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__pilz_industrial_motion_planner_testutils__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__pilz_industrial_motion_planner_testutils__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__pilz_industrial_motion_planner_testutils__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__pilz_industrial_motion_planner_testutils__ubuntu_focal_amd64__binary)
+**moveit_plugins** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_plugins__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_plugins__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_plugins__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_plugins__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_plugins__ubuntu_focal_amd64__binary)
+moveit_fake_controller_manager | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_fake_controller_manager__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_fake_controller_manager__ubuntu_focal_amd64__binary)
+moveit_simple_controller_manager | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_simple_controller_manager__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_simple_controller_manager__ubuntu_focal_amd64__binary)
+moveit_ros_control_interface | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_control_interface__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_control_interface__ubuntu_focal_amd64__binary)
+**moveit_ros** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros__ubuntu_focal_amd64__binary)
+moveit_ros_planning | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning__ubuntu_focal_amd64__binary)
+moveit_ros_move_group | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_move_group__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_move_group__ubuntu_focal_amd64__binary)
+moveit_ros_planning_interface | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_planning_interface__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_planning_interface__ubuntu_focal_amd64__binary)
+moveit_ros_benchmarks | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_benchmarks__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_benchmarks__ubuntu_focal_amd64__binary)
+moveit_ros_perception | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_perception__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_perception__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_perception__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_perception__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_perception__ubuntu_focal_amd64__binary)
+moveit_ros_occupancy_map_monitor | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_occupancy_map_monitor__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_occupancy_map_monitor__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_occupancy_map_monitor__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_occupancy_map_monitor__ubuntu_focal_amd64__binary)
+moveit_ros_manipulation | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_manipulation__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_manipulation__ubuntu_focal_amd64__binary)
+moveit_ros_robot_interaction | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_robot_interaction__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_robot_interaction__ubuntu_focal_amd64__binary)
+moveit_ros_visualization | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_visualization__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_visualization__ubuntu_focal_amd64__binary)
+moveit_ros_warehouse | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_ros_warehouse__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_ros_warehouse__ubuntu_focal_amd64__binary)
+moveit_servo | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_servo__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_servo__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_servo__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_servo__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_servo__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_servo__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_servo__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_servo__ubuntu_focal_amd64__binary)
+**moveit_runtime** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_runtime__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_runtime__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_runtime__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_runtime__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_runtime__ubuntu_focal_amd64__binary)
+moveit_commander | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_commander__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_commander__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_commander__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_commander__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_commander__ubuntu_focal_amd64__binary)
+moveit_setup_assistant | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_setup_assistant__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_setup_assistant__ubuntu_focal_amd64__binary)
+**moveit_msgs** | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_msgs__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_msgs__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_msgs__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_msgs__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_msgs__ubuntu_focal_amd64__binary)
+geometric_shapes | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__geometric_shapes__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__geometric_shapes__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__geometric_shapes__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__geometric_shapes__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__geometric_shapes__ubuntu_focal_amd64__binary)
+srdfdom | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__srdfdom__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__srdfdom__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__srdfdom__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__srdfdom__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__srdfdom__ubuntu_focal_amd64__binary)
+moveit_visual_tools | [![Build Status](https://build.ros.org/buildStatus/icon?job=Msrc_uB__moveit_visual_tools__ubuntu_bionic__source)](https://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_visual_tools__ubuntu_bionic__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary)](https://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_visual_tools__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_visual_tools__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_visual_tools__ubuntu_focal_amd64__binary)
+moveit_tutorials |  |  | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nsrc_uF__moveit_tutorials__ubuntu_focal__source)](https://build.ros.org/view/Nsrc_uF/job/Nsrc_uF__moveit_tutorials__ubuntu_focal__source) | [![Build Status](https://build.ros.org/buildStatus/icon?job=Nbin_uF64__moveit_tutorials__ubuntu_focal_amd64__binary)](https://build.ros.org/view/Nbin_uF64/job/Nbin_uF64__moveit_tutorials__ubuntu_focal_amd64__binary)
 
 
 ## Stargazers over time
diff --git a/moveit/scripts/create_readme_table.py b/moveit/scripts/create_readme_table.py
index a7906fc02f..ab661c85c1 100644
--- a/moveit/scripts/create_readme_table.py
+++ b/moveit/scripts/create_readme_table.py
@@ -1,5 +1,7 @@
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
+# Usage: python moveit/moveit/scripts/create_readme_table.py > /tmp/table.md
+# First update supported_distro_ubuntu_dict below!
 
 from __future__ import print_function
 import os
@@ -46,7 +48,7 @@ def create_line(package, ros_ubuntu_dict):
             U=ubuntu[0].upper(),
             ubuntu=ubuntu.lower(),
             package=package,
-            base_url="http://build.ros.org",
+            base_url="https://build.ros.org",
         )
         for target in ["src", "bin"]:
             define_urls(target, params)
@@ -74,7 +76,6 @@ def create_moveit_buildfarm_table():
     # remove {"indigo":"trusty"} and add {"noetic":"fbuntu"} with "fbuntu"
     # being whatever the 20.04 distro is named
     supported_distro_ubuntu_dict = {
-        "kinetic": "xenial",
         "melodic": "bionic",
         "noetic": "focal",
     }

From 679d285595f89f4c0be4ea3fe1ab78e9425cf226 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sat, 6 Nov 2021 15:11:39 +0100
Subject: [PATCH 013/114] 1.1.6

---
 moveit/CHANGELOG.rst                          |  3 +
 moveit_commander/CHANGELOG.rst                |  7 ++
 moveit_core/CHANGELOG.rst                     | 59 +++++++++++++
 moveit_kinematics/CHANGELOG.rst               |  6 ++
 .../chomp/chomp_interface/CHANGELOG.rst       |  8 ++
 .../chomp/chomp_motion_planner/CHANGELOG.rst  |  7 ++
 .../chomp_optimizer_adapter/CHANGELOG.rst     |  6 ++
 moveit_planners/moveit_planners/CHANGELOG.rst |  3 +
 moveit_planners/ompl/CHANGELOG.rst            |  9 ++
 .../CHANGELOG.rst                             | 18 ++++
 .../CHANGELOG.rst                             |  5 ++
 .../CHANGELOG.rst                             |  6 ++
 moveit_plugins/moveit_plugins/CHANGELOG.rst   |  3 +
 .../CHANGELOG.rst                             |  6 ++
 .../CHANGELOG.rst                             |  7 ++
 moveit_ros/benchmarks/CHANGELOG.rst           |  7 ++
 moveit_ros/manipulation/CHANGELOG.rst         |  8 ++
 moveit_ros/move_group/CHANGELOG.rst           |  7 ++
 moveit_ros/moveit_ros/CHANGELOG.rst           |  3 +
 moveit_ros/moveit_servo/CHANGELOG.rst         | 10 +++
 .../occupancy_map_monitor/CHANGELOG.rst       |  8 ++
 moveit_ros/perception/CHANGELOG.rst           |  8 ++
 moveit_ros/planning/CHANGELOG.rst             | 17 ++++
 moveit_ros/planning_interface/CHANGELOG.rst   | 12 +++
 moveit_ros/robot_interaction/CHANGELOG.rst    |  6 ++
 moveit_ros/visualization/CHANGELOG.rst        | 20 +++++
 moveit_ros/warehouse/CHANGELOG.rst            |  6 ++
 moveit_runtime/CHANGELOG.rst                  |  3 +
 moveit_setup_assistant/CHANGELOG.rst          | 88 +++++++++++++++++++
 29 files changed, 356 insertions(+)

diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst
index e13afc099d..ba31fd2f92 100644
--- a/moveit/CHANGELOG.rst
+++ b/moveit/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst
index 4d142984f6..05021c05db 100644
--- a/moveit_commander/CHANGELOG.rst
+++ b/moveit_commander/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package moveit_commander
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use relative imports (`#2912 <https://github.com/ros-planning/moveit/issues/2912>`_)
+* Fix trajectory constraints (`#2429 <https://github.com/ros-planning/moveit/issues/2429>`_)
+* Fix ``get_planning_pipeline_id`` in Python MGI (`#2753 <https://github.com/ros-planning/moveit/issues/2753>`_)
+* Contributors: Felix von Drigalski, Kevin Chang, Michael Görner, Robert Haschke
+
 1.1.5 (2021-05-23)
 ------------------
 * Allow selecting planning pipeline in MotionSequenceAction (`#2657 <https://github.com/ros-planning/moveit/issues/2657>`_)
diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst
index fb575e9f96..f381b2f6ee 100644
--- a/moveit_core/CHANGELOG.rst
+++ b/moveit_core/CHANGELOG.rst
@@ -2,6 +2,65 @@
 Changelog for package moveit_core
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Silent warning about invalid ``virtual_joint`` in Gazebo setups
+* Add ``RobotState::getRigidlyConnectedParentLinkModel`` `#2918 <https://github.com/ros-planning/moveit/issues/2918>`_ (add RobotState::getRigidlyAttachedParentLink)
+* Normalize incoming transforms (`#2920 <https://github.com/ros-planning/moveit/issues/2920>`_)
+* Reworked compiler flags and fixed various warnings (`#2915 <https://github.com/ros-planning/moveit/issues/2915>`_)
+
+  * Remove unused arguments from global_adjustment_factor()
+  * Simplify API: Remove obviously unused arguments
+  * Introduced cmake macro ``moveit_build_options()`` in ``moveit_core`` to centrally define
+    common build options like ``CMAKE_CXX_STANDARD``, ``CMAKE_BUILD_TYPE``, and compiler warning flags
+* Fix uninitialized orientation in default shape pose (`#2896 <https://github.com/ros-planning/moveit/issues/2896>`_)
+* Drop the minimum velocity/acceleration limits for TOTG (`#2937 <https://github.com/ros-planning/moveit/issues/2937>`_)
+* Readability and consistency improvements in TOTG (`#2882 <https://github.com/ros-planning/moveit/issues/2882>`_)
+* Bullet collision: Consider ACM defaults using ``getAllowedCollision()`` (`#2871 <https://github.com/ros-planning/moveit/issues/2871>`_)
+* ``PlanningScene::getPlanningSceneDiffMsg()``: Do not list an object as destroyed when it got attached (`#2864 <https://github.com/ros-planning/moveit/issues/2864>`_)
+
+  The information in the diff is redundant because attaching implies the removal from the PlanningScene.
+  In the unlikely case, you relied on the ``REMOVE`` entry in the diff message,
+  use the newly attached collision object to indicate the same instead.
+* Fix Bullet collision: Register ``notify`` function to receive world updates (`#2830 <https://github.com/ros-planning/moveit/issues/2830>`_)
+* Split CollisionPluginLoader (`#2834 <https://github.com/ros-planning/moveit/issues/2834>`_)
+
+  To avoid circular dependencies, but enable reuse of the ``CollisionPluginLoader``, the non-ROS part was moved into ``moveit_core/moveit_collision_detection.so``
+  and the ROS part (reading the plugin name from the parameter server) into ``moveit_ros_planning/moveit_collision_plugin_loader.so`` (as before).
+* Use default copy constructor to clone attached objects (`#2855 <https://github.com/ros-planning/moveit/issues/2855>`_)
+* Fix pose-not-set-bug (`#2852 <https://github.com/ros-planning/moveit/issues/2852>`_)
+* Add API for passing a ``RNG`` to ``setToRandomPositionsNearBy()`` (`#2799 <https://github.com/ros-planning/moveit/issues/2799>`_)
+* Fix backwards compatibility for specifying poses for a single collision shape (`#2816 <https://github.com/ros-planning/moveit/issues/2816>`_)
+* Fix Bullet collision returning wrong contact type (`#2829 <https://github.com/ros-planning/moveit/issues/2829>`_)
+* Add ``RobotState::setToDefaultValues(string group)`` (`#2828 <https://github.com/ros-planning/moveit/issues/2828>`_)
+* Fix confusion of tolerance limits in JointConstraint (`#2815 <https://github.com/ros-planning/moveit/issues/2815>`_)
+* Fix RobotState constructor segfault (`#2790 <https://github.com/ros-planning/moveit/issues/2790>`_)
+* Preserve metadata (color, type) when detaching objects (`#2814 <https://github.com/ros-planning/moveit/issues/2814>`_)
+* Introduce a reference frame for collision objects (`#2037 <https://github.com/ros-planning/moveit/issues/2037>`_)
+
+  ``CollisionObject`` messages are now defined with a ``Pose``. Shapes and subframes are defined relative to that pose.
+  This makes it easier to place objects with subframes and multiple shapes in the scene.
+  This causes several changes:
+
+  - ``getFrameTransform()`` now returns this pose instead of the first shape's pose.
+  - The Rviz plugin's manipulation tab now uses the object's pose instead of the shape pose to evaluate if object's are in the region of interest.
+  - PlanningScene geometry text files (``.scene``) have changed format.
+
+    Add a line ``0 0 0 0 0 0 1`` under each line with an asterisk to upgrade old files if required.
+* Fix bullet plugin library path name (`#2783 <https://github.com/ros-planning/moveit/issues/2783>`_)
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* ``RobotTrajectory``: convenience constructor + chain setter support (`#2751 <https://github.com/ros-planning/moveit/issues/2751>`_)
+* Fix Windows build (`#2604 <https://github.com/ros-planning/moveit/issues/2604>`_, `#2776 <https://github.com/ros-planning/moveit/issues/2776>`_)
+* Allow axis-angle representation for orientation constraints (`#2402 <https://github.com/ros-planning/moveit/issues/2402>`_)
+* Optimization: ``reserve()`` vector in advance (`#2732 <https://github.com/ros-planning/moveit/issues/2732>`_)
+* Use same padding/scale for attached collision objects as for parent link (`#2721 <https://github.com/ros-planning/moveit/issues/2721>`_)
+* Optimize ``FCL distanceCallback()``: use thread_local vars, avoid copying (`#2698 <https://github.com/ros-planning/moveit/issues/2698>`_)
+* Remove octomap from catkin_packages ``LIBRARIES`` entry (`#2700 <https://github.com/ros-planning/moveit/issues/2700>`_)
+* Remove deprecated header ``deprecation.h`` (`#2693 <https://github.com/ros-planning/moveit/issues/2693>`_)
+* ``collision_detection_fcl``: Report link_names in correct order (`#2682 <https://github.com/ros-planning/moveit/issues/2682>`_)
+* Move ``OccMapTree`` to ``moveit_core/collision_detection`` (`#2684 <https://github.com/ros-planning/moveit/issues/2684>`_)
+* Contributors: 0Nel, AndyZe, Captain Yoshi, Felix von Drigalski, Jafar Abdi, Jeroen, Jochen Sprickerhof, John Stechschulte, Jonathan Grebe, Max Schwarz, Michael Görner, Michael Wiznitzer, Peter Mitrano, Robert Haschke, Silvio Traversaro, Simon Schmeisser, Tobias Fischer, Tyler Weaver, Wolf Vollprecht, Yuri Rocha, pvanlaar, toru-kuga, v4hn, werner291
+
 1.1.5 (2021-05-23)
 ------------------
 * Revert "Lock the octomap/octree while collision checking (`#2683 <https://github.com/ros-planning/moveit/issues/2683>`_)
diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst
index a4665cf89c..042f1dc1bb 100644
--- a/moveit_kinematics/CHANGELOG.rst
+++ b/moveit_kinematics/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_kinematics
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Contributors: Robert Haschke, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst
index ca1d44da19..7ce1926fe2 100644
--- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst
+++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst
@@ -2,6 +2,14 @@
 Changelog for package chomp_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use test_environment.launch in unittests (`#2949 <https://github.com/ros-planning/moveit/issues/2949>`_)
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* CHOMP: Rename param 'clearence' to 'clearance' (`#2702 <https://github.com/ros-planning/moveit/issues/2702>`_, `#2707 <https://github.com/ros-planning/moveit/issues/2707>`_)
+* Contributors: Martin Günther, Michael Görner, Robert Haschke, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst
index 33c95b2fd5..a0e95d0ff1 100644
--- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst
+++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package chomp_motion_planner
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* CHOMP: Rename param 'clearence' to 'clearance' (`#2702 <https://github.com/ros-planning/moveit/issues/2702>`_, `#2707 <https://github.com/ros-planning/moveit/issues/2707>`_)
+* Contributors: Martin Günther, Michael Görner, Robert Haschke, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst
index 2ce53076e6..f157e261ab 100644
--- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst
+++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_chomp_optimizer_adapter
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* CHOMP: Rename param 'clearence' to 'clearance' (`#2702 <https://github.com/ros-planning/moveit/issues/2702>`_, `#2707 <https://github.com/ros-planning/moveit/issues/2707>`_)
+* Contributors: Martin Günther, Michael Görner, Robert Haschke
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst
index 2a6b1726df..88bc0f818b 100644
--- a/moveit_planners/moveit_planners/CHANGELOG.rst
+++ b/moveit_planners/moveit_planners/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_planners
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst
index 4f92d41837..74380e604a 100644
--- a/moveit_planners/ompl/CHANGELOG.rst
+++ b/moveit_planners/ompl/CHANGELOG.rst
@@ -2,6 +2,15 @@
 Changelog for package moveit_planners_ompl
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Fix ConstrainedGoalSampler (`#2811 <https://github.com/ros-planning/moveit/issues/2811>`_): actually call ``sample()`` (`#2872 <https://github.com/ros-planning/moveit/issues/2872>`_)
+* Provide override for missing isValid method (`#2802 <https://github.com/ros-planning/moveit/issues/2802>`_)
+* Add missing dependencies to generated dynamic_reconfigure headers (`#2772 <https://github.com/ros-planning/moveit/issues/2772>`_)
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Contributors: Mathias Lüdtke, Michael Görner, Robert Haschke, pvanlaar, v4hn, werner291
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst
index e3325a5963..6db7860dbc 100644
--- a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst
+++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst
@@ -2,6 +2,24 @@
 Changelog for package pilz_industrial_motion_planner
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Fix calculation of subframe offset (`#2890 <https://github.com/ros-planning/moveit/issues/2890>`_)
+* Remove unused moveit_planning_execution.launch
+* Use test_environment.launch in unittests (`#2949 <https://github.com/ros-planning/moveit/issues/2949>`_)
+* Rename launch argument execution_type -> fake_execution_type
+* Improve error messages (`#2940 <https://github.com/ros-planning/moveit/issues/2940>`_)
+  * Remove deprecated xacro --inorder
+  * Don't complain about missing limits for irrelevant JMGs
+  * Avoid duplicate error messages
+  * Downgrade ERROR to WARN when checking joint limits, report affected joint name
+  * Quote (possibly empty) planner id
+* Consider attached bodies for planning (`#2773 <https://github.com/ros-planning/moveit/issues/2773>`_, `#2824 <https://github.com/ros-planning/moveit/issues/2824>`_, `#2878 <https://github.com/ros-planning/moveit/issues/2878>`_)
+* Fix collision detection: consider current PlanningScene (`#2803 <https://github.com/ros-planning/moveit/issues/2803>`_)
+* Add planning_pipeline_id to MotionSequence action + service (`#2755 <https://github.com/ros-planning/moveit/issues/2755>`_)
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Contributors: Felix von Drigalski, Leroy Rügemer, Michael Görner, Robert Haschke, Tom Noble, aa-tom, cambel, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 * Allow selecting planning pipeline in MotionSequenceAction (`#2657 <https://github.com/ros-planning/moveit/issues/2657>`_)
diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst
index c2d02e5f29..3a52f57b77 100644
--- a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst
+++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package pilz_industrial_motion_planner_testutils
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Contributors: Robert Haschke, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst
index a73125936b..bd397a6b35 100644
--- a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst
+++ b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_fake_controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Contributors: Robert Haschke, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst
index f278b749f5..1f4b2c84ee 100644
--- a/moveit_plugins/moveit_plugins/CHANGELOG.rst
+++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_plugins
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst
index ede525eb96..8fa38fd676 100644
--- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst
+++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_ros_control_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Fix reversed check in switchControllers (`#2726 <https://github.com/ros-planning/moveit/issues/2726>`_)
+* Contributors: Nathan Brooks, Robert Haschke
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst
index fa85618a51..f6dff94a2f 100644
--- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst
+++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package moveit_simple_controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Facilitate spotting of empty controller names, using quotes (`#2761 <https://github.com/ros-planning/moveit/issues/2761>`_)
+* Contributors: G.A. vd. Hoorn, Robert Haschke
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst
index 84b78ee71f..60ef11f3cf 100644
--- a/moveit_ros/benchmarks/CHANGELOG.rst
+++ b/moveit_ros/benchmarks/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package moveit_ros_benchmarks
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Fix predefined poses benchmark example (`#2718 <https://github.com/ros-planning/moveit/issues/2718>`_)
+* Contributors: Captain Yoshi, Robert Haschke
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_ros/manipulation/CHANGELOG.rst b/moveit_ros/manipulation/CHANGELOG.rst
index ed2222aee6..412b0702c4 100644
--- a/moveit_ros/manipulation/CHANGELOG.rst
+++ b/moveit_ros/manipulation/CHANGELOG.rst
@@ -2,6 +2,14 @@
 Changelog for package moveit_ros_manipulation
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Introduce a reference frame for collision objects (`#2037 <https://github.com/ros-planning/moveit/issues/2037>`_)
+* Add missing dependencies to generated dynamic_reconfigure headers (`#2772 <https://github.com/ros-planning/moveit/issues/2772>`_)
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Contributors: Felix von Drigalski, Mathias Lüdtke, Robert Haschke, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst
index f9dfde4142..2fd523bd35 100644
--- a/moveit_ros/move_group/CHANGELOG.rst
+++ b/moveit_ros/move_group/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package moveit_ros_move_group
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Introduce a reference frame for collision objects (`#2037 <https://github.com/ros-planning/moveit/issues/2037>`_)
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Contributors: Felix von Drigalski, Robert Haschke, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst
index b3a04d400b..0fc302e0a1 100644
--- a/moveit_ros/moveit_ros/CHANGELOG.rst
+++ b/moveit_ros/moveit_ros/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_ros/moveit_servo/CHANGELOG.rst b/moveit_ros/moveit_servo/CHANGELOG.rst
index 12986c8f2e..48af78cadf 100644
--- a/moveit_ros/moveit_servo/CHANGELOG.rst
+++ b/moveit_ros/moveit_servo/CHANGELOG.rst
@@ -2,6 +2,16 @@
 Changelog for package moveit_servo
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Backport position limit enforcement from MoveIt2 (`#2898 <https://github.com/ros-planning/moveit/issues/2898>`_)
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Minor fixups (`#2759 <https://github.com/ros-planning/moveit/issues/2759>`_)
+* Remove gtest include from non-testing source (`#2747 <https://github.com/ros-planning/moveit/issues/2747>`_)
+* Fix an off-by-one error in servo_calcs.cpp (`#2740 <https://github.com/ros-planning/moveit/issues/2740>`_)
+* Refactor ``moveit_servo::LowPassFilter`` to be assignable (`#2722 <https://github.com/ros-planning/moveit/issues/2722>`_)
+* Contributors: Griswald Brooks, Michael Görner, Michael Wiznitzer, Robert Haschke, luisrayas3, toru-kuga
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst
index bf39697d1c..8783014b85 100644
--- a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst
+++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst
@@ -2,6 +2,14 @@
 Changelog for package moveit_ros_occupancy_map_monitor
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Prefer ``std::make_shared`` over ``new`` operator (`#2756 <https://github.com/ros-planning/moveit/issues/2756>`_)
+* Add missing ``OCTOMAP_INCLUDE_DIRS`` (`#2671 <https://github.com/ros-planning/moveit/issues/2671>`_)
+* Move ``OccMapTree`` to ``moveit_core/collision_detection`` (`#2684 <https://github.com/ros-planning/moveit/issues/2684>`_)
+* Contributors: 0Nel, Michael Görner, Robert Haschke, Simon Schmeisser, Tyler Weaver
+
 1.1.5 (2021-05-23)
 ------------------
 * Revert "Lock the octomap/octree while collision checking (`#2683 <https://github.com/ros-planning/moveit/issues/2683>`_)
diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst
index 66cae07257..2a07ed4e95 100644
--- a/moveit_ros/perception/CHANGELOG.rst
+++ b/moveit_ros/perception/CHANGELOG.rst
@@ -2,6 +2,14 @@
 Changelog for package moveit_ros_perception
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Fix clipping of points: only considered points up to ``max_range`` (`#2848 <https://github.com/ros-planning/moveit/issues/2848>`_)
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Move ``OccMapTree`` to ``moveit_core/collision_detection`` (`#2684 <https://github.com/ros-planning/moveit/issues/2684>`_)
+* Contributors: Michael Görner, Robert Haschke, Simon Schmeisser, Tyler Weaver, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 * Revert "Lock the octomap/octree while collision checking (`#2683 <https://github.com/ros-planning/moveit/issues/2683>`_)
diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst
index 4bf14edb28..5d62ec94d7 100644
--- a/moveit_ros/planning/CHANGELOG.rst
+++ b/moveit_ros/planning/CHANGELOG.rst
@@ -2,6 +2,23 @@
 Changelog for package moveit_ros_planning
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Split ``CollisionPluginLoader`` (`#2834 <https://github.com/ros-planning/moveit/issues/2834>`_)
+
+  To avoid circular dependencies, but enable reuse of the ``CollisionPluginLoader``, the non-ROS part was moved into ``moveit_core/moveit_collision_detection.so``
+  and the ROS part (reading the plugin name from the parameter server) into ``moveit_ros_planning/moveit_collision_plugin_loader.so`` (as before).
+* Introduce a reference frame for collision objects (`#2037 <https://github.com/ros-planning/moveit/issues/2037>`_)
+* Fix RDFLoader: uninitialized member (`#2806 <https://github.com/ros-planning/moveit/issues/2806>`_)
+* Add missing dependencies to generated dynamic_reconfigure headers (`#2772 <https://github.com/ros-planning/moveit/issues/2772>`_)
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* PSM: Read padding parameters from correct namespace (`#2706 <https://github.com/ros-planning/moveit/issues/2706>`_)
+* Load ``max_safe_path_cost`` into namespace ``sense_for_plan`` (`#2703 <https://github.com/ros-planning/moveit/issues/2703>`_)
+* Fix order of member initialization (`#2687 <https://github.com/ros-planning/moveit/issues/2687>`_)
+* Move ``OccMapTree`` to ``moveit_core/collision_detection`` (`#2684 <https://github.com/ros-planning/moveit/issues/2684>`_)
+* Contributors: Felix von Drigalski, Martin Günther, Mathias Lüdtke, Michael Görner, Robert Haschke, Simon Schmeisser, Tyler Weaver, pvanlaar, werner291
+
 1.1.5 (2021-05-23)
 ------------------
 * Revert "Lock the octomap/octree while collision checking (`#2683 <https://github.com/ros-planning/moveit/issues/2683>`_)
diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst
index 04aabc8fe8..18f34c1b38 100644
--- a/moveit_ros/planning_interface/CHANGELOG.rst
+++ b/moveit_ros/planning_interface/CHANGELOG.rst
@@ -2,6 +2,18 @@
 Changelog for package moveit_ros_planning_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use test_environment.launch in unittests (`#2949 <https://github.com/ros-planning/moveit/issues/2949>`_)
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Add missing replan/look options to interface (`#2892 <https://github.com/ros-planning/moveit/issues/2892>`_)
+* PSI: get object.pose from new msg field (`#2877 <https://github.com/ros-planning/moveit/issues/2877>`_)
+* Introduce a reference frame for collision objects (`#2037 <https://github.com/ros-planning/moveit/issues/2037>`_)
+* Fix trajectory constraints for ``moveit_commander`` (`#2429 <https://github.com/ros-planning/moveit/issues/2429>`_)
+* ``MGI::setStartState``: Only fetch current state when new state is diff (`#2775 <https://github.com/ros-planning/moveit/issues/2775>`_)
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Contributors: Felix von Drigalski, Gauthier Hentz, Kevin Chang, Michael Görner, Robert Haschke, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 * Allow selecting planning pipeline in MotionSequenceAction (`#2657 <https://github.com/ros-planning/moveit/issues/2657>`_)
diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst
index 5d68f588b4..5e4d171fdf 100644
--- a/moveit_ros/robot_interaction/CHANGELOG.rst
+++ b/moveit_ros/robot_interaction/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_ros_robot_interaction
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Contributors: Robert Haschke, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst
index 84c7880dff..4d1935f035 100644
--- a/moveit_ros/visualization/CHANGELOG.rst
+++ b/moveit_ros/visualization/CHANGELOG.rst
@@ -2,6 +2,26 @@
 Changelog for package moveit_ros_visualization
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Re-initialize params, subscribers, and topics when the ``MoveGroupNS`` has changed (`#2922 <https://github.com/ros-planning/moveit/issues/2922>`_)
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Do not save/restore warehouse parameters (`#2865 <https://github.com/ros-planning/moveit/issues/2865>`_) but use the ROS parameters only
+* PSD: Correctly update robot's base pose (`#2876 <https://github.com/ros-planning/moveit/issues/2876>`_)
+* Fix Python2: convert keys() into list (`#2862 <https://github.com/ros-planning/moveit/issues/2862>`_)
+* MP panel: fix order of input widgets for shape size (`#2847 <https://github.com/ros-planning/moveit/issues/2847>`_)
+* Use relative topic name in trajectory visualization to allow namespacing (`#2835 <https://github.com/ros-planning/moveit/issues/2835>`_)
+* MotionPlanningFrame: Gracefully handle undefined parent widget, e.g. for use via ``librviz.so`` (`#2833 <https://github.com/ros-planning/moveit/issues/2833>`_)
+* Introduce a reference frame for collision objects (`#2037 <https://github.com/ros-planning/moveit/issues/2037>`_)
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Support arbitrary real-time factors in trajectory visualization (`#2745 <https://github.com/ros-planning/moveit/issues/2745>`_)
+
+  Replaced special value ``REALTIME`` to accept arbitrary real-time factors in the format ``<number>x``, e.g. ``3x``.
+* Joints tab: Fix handling of mimic + passive joints (`#2744 <https://github.com/ros-planning/moveit/issues/2744>`_)
+* Fix ``TrajectoryPanel``: Keep "Pause/Play" button in correct state (`#2737 <https://github.com/ros-planning/moveit/issues/2737>`_)
+* Fixed error: ``moveit_joy: RuntimeError: dictionary changed size during iteration`` (`#2625 <https://github.com/ros-planning/moveit/issues/2625>`_, `#2628 <https://github.com/ros-planning/moveit/issues/2628>`_)
+* Contributors: Felix von Drigalski, Michael Görner, Rick Staa, Robert Haschke, Yuri Rocha, lorepieri8, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst
index 153234b518..03a4958608 100644
--- a/moveit_ros/warehouse/CHANGELOG.rst
+++ b/moveit_ros/warehouse/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_ros_warehouse
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
+* Contributors: Robert Haschke, pvanlaar
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst
index 58a8e9bd94..87365e3bc5 100644
--- a/moveit_runtime/CHANGELOG.rst
+++ b/moveit_runtime/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_runtime
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+
 1.1.5 (2021-05-23)
 ------------------
 
diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/CHANGELOG.rst
index dd33bc3d32..9486988fc9 100644
--- a/moveit_setup_assistant/CHANGELOG.rst
+++ b/moveit_setup_assistant/CHANGELOG.rst
@@ -2,6 +2,94 @@
 Changelog for package moveit_setup_assistant
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.6 (2021-11-06)
+------------------
+* Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
+* Correctly state not-found package name
+* Finish setup_assistant.launch when MSA finished (`#2749 <https://github.com/ros-planning/moveit/issues/2749>`_)
+
+* **Enabling the following new features requires recreation of your robot's MoveIt config with the new MSA!**
+* Various improvements (`#2932 <https://github.com/ros-planning/moveit/issues/2932>`_)
+
+  * Define default planner for Pilz pipeline
+  * Allow checking/unchecking multiple files for generation
+  * ``moveit.rviz``: Use Orbit view controller
+  * Rename launch argument ``execution_type`` -> ``fake_execution_type`` to clarify that this parameter is only used for fake controllers
+  * ``demo.launch``: Start joint + robot-state publishers in fake mode only
+  * Modularize ``demo_gazebo.launch`` reusing ``demo.launch``
+  * ``gazebo.launch``
+
+    * Delay unpause to ensure that the robot's initial pose is actually held
+    * Allow initial_joint_positions
+    * Load URDF via xacro if neccessary
+
+  * Rework ``moveit_controller_manager`` handling
+
+    So far, ``move_group.launch`` distinguished between fake and real-robot operation only.
+    The boolean launch-file argument ``fake_execution`` was translated to ``moveit_controller_manager = [fake|robot]``
+    in ``move_group.launch`` and then further translated to the actual plugin name.
+
+    However, MoveIt provides 3 basic controller manager plugins:
+
+    - ``fake`` = ``moveit_fake_controller_manager::MoveItFakeControllerManager`` (default in ``demo.launch``)
+
+      Doesn't really control the robot. Provides these interpolation types (``fake_execution_type``):
+
+      - ``via points``: jumps to the via points
+      - ``interpolate``: linearly interpolates between via points (default)
+      - ``last point``: jumps to the final trajectory point (used for fast execution testing)
+    - ``ros_control`` = ``moveit_ros_control_interface::MoveItControllerManager``
+
+      Interfaces to ``ros_control`` controllers.
+    - ``simple`` = ``moveit_simple_controller_manager/MoveItSimpleControllerManager``
+      Interfaces to action servers for ``FollowJointTrajectory`` and/or ``GripperCommand``
+      that in turn interface to the low-level robot controllers (typically based on ros_control).
+
+    Now, the argument ``moveit_controller_manager`` allows for switching between these 3 variants using the given names.
+    Adding more ``*_moveit_controller_manager.launch`` files allows for further extension of this scheme.
+
+* Rework Controller Handling (`#2945 <https://github.com/ros-planning/moveit/issues/2945>`_)
+
+  * Write separate controller config files for different MoveIt controller managers:
+
+    - ``fake_controllers.yaml`` for use with ``MoveItFakeControllerManager``
+    - ``simple_moveit_controllers.yaml`` handles everything relevant for ``MoveItSimpleControllerManager``
+    - ``ros_controllers.yaml`` defines ``ros_control`` controllers
+    - ``gazebo_controllers.yaml`` lists controllers required for Gazebo
+
+  * Rework controller config generation
+
+    - Provide all types of ``JointTrajectoryController`` (position, velocity, and effort based)
+      as well as ``FollowJointTrajectory`` and ``GripperCommand`` (use by simple controller manager)
+    - Use ``effort_controllers/JointTrajectoryController`` as default
+    - Create ``FollowJointTrajectory`` entries for any ``JointTrajectoryController``
+    - Fix controller list generation: always write joint names as a list
+
+  * Code refactoring to clarify that controller widget handles all controllers, not only ``ros_control`` controllers
+
+    * Update widget texts to speak about generic controllers
+    * Rename ``ROSControllersWidget`` -> ``ControllersWidget``
+    * Rename files ``ros_controllers_widget.*`` -> ``controllers_widget.*``
+    * Rename ``ros_controllers_config_`` -> ``controller_configs_``
+    * Rename functions ``*ROSController*`` -> ``*Controller*``
+    * Rename ``ROSControlConfig`` -> ``ControllerConfig``
+
+* Fix sensor config handling (`#2708 <https://github.com/ros-planning/moveit/issues/2708>`_, `#2946 <https://github.com/ros-planning/moveit/issues/2946>`_)
+
+* Load planning pipelines into their own namespace (`#2888 <https://github.com/ros-planning/moveit/issues/2888>`_)
+* Add ``jiggle_fraction`` arg to trajopt template (`#2858 <https://github.com/ros-planning/moveit/issues/2858>`_)
+* Only define ``default`` values for input argumens in ``*_planning_pipeline.launch`` templates (`#2849 <https://github.com/ros-planning/moveit/issues/2849>`_)
+* Mention (optional) Gazebo deps in package.xml templates (`#2839 <https://github.com/ros-planning/moveit/issues/2839>`_)
+* Create ``static_transform_publisher`` for each virtual joint type (`#2769 <https://github.com/ros-planning/moveit/issues/2769>`_)
+* Use $(dirname) in launch files (`#2748 <https://github.com/ros-planning/moveit/issues/2748>`_)
+* CHOMP: Read parameters from proper namespace (`#2707 <https://github.com/ros-planning/moveit/issues/2707>`_)
+
+  * Pilz pipeline: remove unused arg ``start_state_max_bounds_error``
+  * Set ``jiggle_fraction`` per pipeline
+  * Rename param ``clearence`` to ``clearance``
+* Load ``max_safe_path_cost`` into namespace ``sense_for_plan`` (`#2703 <https://github.com/ros-planning/moveit/issues/2703>`_)
+* Contributors: David V. Lu!!, Martin Günther, Max Puig, Michael Görner, Rick Staa, Robert Haschke, pvanlaar, v4hn
+
 1.1.5 (2021-05-23)
 ------------------
 

From ccbfdac484534a2c6343f2a6666ee35bf556921f Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@users.noreply.github.com>
Date: Sun, 7 Nov 2021 18:27:49 +0100
Subject: [PATCH 014/114] GHA: auto-sync noetic-devel to master (#2952)

---
 .github/workflows/ci.yaml | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index 3841c3d2fe..dd9ffa6661 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -9,7 +9,6 @@ on:
   push:
     branches:
       - master
-      - "[kmn]*-devel"
 
 jobs:
   default:
@@ -150,3 +149,13 @@ jobs:
           sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete
           sudo rm -rf ${{ env.BASEDIR }}/target_ws/src
           du -sh ${{ env.BASEDIR }}/target_ws
+
+  # Sync noetic-devel branch with master on successful build
+  sync-noetic-devel:
+    runs-on: ubuntu-latest
+    needs: default # only trigger on success of default build job
+    if: github.event_name == 'push' && github.repository_owner == 'ros-planning' && github.ref == 'refs/heads/master'
+    steps:
+      - uses: actions/checkout@v2
+      - name: Fast-forward noetic-devel to sync with master
+        run: git push origin HEAD:noetic-devel

From 15c8a6e3a0c51ec0b27b13f6b84e54f520c75172 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sun, 7 Nov 2021 23:32:41 +0100
Subject: [PATCH 015/114] Fixup auto-sync noetic-devel to master

---
 .github/workflows/ci.yaml | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index dd9ffa6661..daaa7be1b4 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -158,4 +158,8 @@ jobs:
     steps:
       - uses: actions/checkout@v2
       - name: Fast-forward noetic-devel to sync with master
-        run: git push origin HEAD:noetic-devel
+        run: |
+          # Extend shallow copy from actions/checkout to connect noetic-devel..master
+          git fetch --depth=1 origin noetic-devel
+          git fetch --depth=200 origin master
+          git push origin master:noetic-devel

From fb9fc79a4a6faf6d6aa76e61a0d1c4dfcbc7800f Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@users.noreply.github.com>
Date: Mon, 8 Nov 2021 12:26:11 +0100
Subject: [PATCH 016/114] Upload controller_list for simple controller manager
 (#2954)

---
 .../launch/simple_moveit_controller_manager.launch.xml           | 1 +
 1 file changed, 1 insertion(+)

diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml
index dec8cff842..f05ca303f4 100644
--- a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml
+++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/simple_moveit_controller_manager.launch.xml
@@ -3,5 +3,6 @@
   <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
 
   <!-- Load controller list to the parameter server -->
+  <rosparam file="$(find [GENERATED_PACKAGE_NAME])/config/simple_moveit_controllers.yaml" />
   <rosparam file="$(find [GENERATED_PACKAGE_NAME])/config/ros_controllers.yaml" />
 </launch>

From 4db626d9c3b5d6296b012188a4a0bfe4bddf6bce Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Mon, 8 Nov 2021 13:43:52 +0100
Subject: [PATCH 017/114] Fixup auto-sync noetic-devel to master

Fix push permissions.
---
 .github/workflows/ci.yaml | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index daaa7be1b4..23274bd83e 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -159,6 +159,10 @@ jobs:
       - uses: actions/checkout@v2
       - name: Fast-forward noetic-devel to sync with master
         run: |
+          # Configure push user+url
+          git config --global user.name 'GHA sync bot'
+          git config --global user.email 'rhaschke@users.noreply.github.com'
+          git remote set-url origin https://x-access-token:${{ secrets.GH_PAGES_TOKEN }}@github.com/${{ github.repository }}
           # Extend shallow copy from actions/checkout to connect noetic-devel..master
           git fetch --depth=1 origin noetic-devel
           git fetch --depth=200 origin master

From 8a6c6ba003687566e75d0553633e412b130b7f3e Mon Sep 17 00:00:00 2001
From: Tim Redick <tim.redick@rwth-aachen.de>
Date: Tue, 16 Nov 2021 09:43:48 +0100
Subject: [PATCH 018/114] Add ns for depth image & pointcloud octomap updaters
 (#2916)

---
 .../depth_image_octomap_updater.h                  |  1 +
 .../src/depth_image_octomap_updater.cpp            | 14 ++++++++++----
 .../pointcloud_octomap_updater.h                   |  1 +
 .../src/pointcloud_octomap_updater.cpp             |  9 ++++++++-
 4 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h
index 6779854111..7683d00a4c 100644
--- a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h
+++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h
@@ -80,6 +80,7 @@ class DepthImageOctomapUpdater : public OccupancyMapUpdater
   ros::Time last_update_time_;
 
   std::string filtered_cloud_topic_;
+  std::string ns_;
   std::string sensor_type_;
   std::string image_topic_;
   std::size_t queue_size_;
diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp
index ef865194f2..c391989f90 100644
--- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp
+++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp
@@ -105,6 +105,8 @@ bool DepthImageOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params)
     readXmlParam(params, "skip_horizontal_pixels", &skip_horizontal_pixels_);
     if (params.hasMember("filtered_cloud_topic"))
       filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
+    if (params.hasMember("ns"))
+      ns_ = static_cast<const std::string&>(params["ns"]);
   }
   catch (XmlRpc::XmlRpcException& ex)
   {
@@ -135,14 +137,18 @@ bool DepthImageOctomapUpdater::initialize()
 void DepthImageOctomapUpdater::start()
 {
   image_transport::TransportHints hints("raw", ros::TransportHints(), nh_);
-  pub_model_depth_image_ = model_depth_transport_.advertiseCamera("model_depth", 1);
 
+  std::string prefix = "";
+  if (!ns_.empty())
+    prefix = ns_ + "/";
+
+  pub_model_depth_image_ = model_depth_transport_.advertiseCamera(prefix + "model_depth", 1);
   if (!filtered_cloud_topic_.empty())
-    pub_filtered_depth_image_ = filtered_depth_transport_.advertiseCamera(filtered_cloud_topic_, 1);
+    pub_filtered_depth_image_ = filtered_depth_transport_.advertiseCamera(prefix + filtered_cloud_topic_, 1);
   else
-    pub_filtered_depth_image_ = filtered_depth_transport_.advertiseCamera("filtered_depth", 1);
+    pub_filtered_depth_image_ = filtered_depth_transport_.advertiseCamera(prefix + "filtered_depth", 1);
 
-  pub_filtered_label_image_ = filtered_label_transport_.advertiseCamera("filtered_label", 1);
+  pub_filtered_label_image_ = filtered_label_transport_.advertiseCamera(prefix + "filtered_label", 1);
 
   sub_depth_image_ = input_depth_transport_.subscribeCamera(image_topic_, queue_size_,
                                                             &DepthImageOctomapUpdater::depthImageCallback, this, hints);
diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h
index 69cd73f047..44df18d637 100644
--- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h
+++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h
@@ -87,6 +87,7 @@ class PointCloudOctomapUpdater : public OccupancyMapUpdater
   unsigned int point_subsample_;
   double max_update_rate_;
   std::string filtered_cloud_topic_;
+  std::string ns_;
   ros::Publisher filtered_cloud_publisher_;
 
   message_filters::Subscriber<sensor_msgs::PointCloud2>* point_cloud_subscriber_;
diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp
index 859f13ec24..fb37bbf1a0 100644
--- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp
+++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp
@@ -82,6 +82,8 @@ bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params)
       readXmlParam(params, "max_update_rate", &max_update_rate_);
     if (params.hasMember("filtered_cloud_topic"))
       filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
+    if (params.hasMember("ns"))
+      ns_ = static_cast<const std::string&>(params["ns"]);
   }
   catch (XmlRpc::XmlRpcException& ex)
   {
@@ -98,8 +100,13 @@ bool PointCloudOctomapUpdater::initialize()
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, root_nh_);
   shape_mask_ = std::make_unique<point_containment_filter::ShapeMask>();
   shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, _1, _2));
+
+  std::string prefix = "";
+  if (!ns_.empty())
+    prefix = ns_ + "/";
   if (!filtered_cloud_topic_.empty())
-    filtered_cloud_publisher_ = private_nh_.advertise<sensor_msgs::PointCloud2>(filtered_cloud_topic_, 10, false);
+    filtered_cloud_publisher_ =
+        private_nh_.advertise<sensor_msgs::PointCloud2>(prefix + filtered_cloud_topic_, 10, false);
   return true;
 }
 

From 82ece8e6e239811cbaf4b6f4fc6b0dbffd0b1dff Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Tue, 16 Nov 2021 17:13:25 +0100
Subject: [PATCH 019/114] MSA: Notice file updates (#2964)

This commit fixes a MSA bug causing files in a loaded MoveIt config to be incorrectly classified as externally modified
after being written by the "Generate Package" button.
As this status is solely based on the file timestamp relative to the timestamp stored in the .setupassistant file,
we need to update this timestamp when we wrote the files.
---
 moveit_setup_assistant/src/tools/moveit_config_data.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
index d1e0e8a99f..4535622c36 100644
--- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp
+++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
@@ -182,7 +182,8 @@ bool MoveItConfigData::outputSetupAssistantFile(const std::string& file_path)
   emitter << YAML::Value << YAML::BeginMap;
   emitter << YAML::Key << "author_name" << YAML::Value << author_name_;
   emitter << YAML::Key << "author_email" << YAML::Value << author_email_;
-  emitter << YAML::Key << "generated_timestamp" << YAML::Value << std::time(nullptr);  // TODO: is this cross-platform?
+  auto cur_time = std::time(nullptr);
+  emitter << YAML::Key << "generated_timestamp" << YAML::Value << cur_time;  // TODO: is this cross-platform?
   emitter << YAML::EndMap;
 
   emitter << YAML::EndMap;
@@ -197,6 +198,10 @@ bool MoveItConfigData::outputSetupAssistantFile(const std::string& file_path)
   output_stream << emitter.c_str();
   output_stream.close();
 
+  /// Update the parsed setup_assistant timestamp
+  // NOTE: Needed for when people run the MSA generator multiple times in a row.
+  config_pkg_generated_timestamp_ = cur_time;
+
   return true;  // file created successfully
 }
 

From c4815761ffa350371a0c4ad19d8593758cdd2178 Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Tue, 16 Nov 2021 19:32:30 +0100
Subject: [PATCH 020/114] RDFLoader: clear buffer before reading content
 (#2963)

---
 moveit_ros/planning/rdf_loader/src/rdf_loader.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp
index cf9fdef1ad..d2a43cc9dd 100644
--- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp
+++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp
@@ -162,6 +162,7 @@ bool rdf_loader::RDFLoader::loadFileToString(std::string& buffer, const std::str
 bool rdf_loader::RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& path,
                                                   const std::vector<std::string>& xacro_args)
 {
+  buffer.clear();
   if (path.empty())
   {
     ROS_ERROR_NAMED("rdf_loader", "Path is empty");

From d0d76f171288846a00a7b91935e1571f31d154d3 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Tue, 16 Nov 2021 14:00:09 -0600
Subject: [PATCH 021/114] Add waypoint duration to the trajectory deep copy
 unit test (#2961)

* Add waypoint duration to the trajectory deep copy test

* Slightly more accurate comments
---
 .../test/test_robot_trajectory.cpp                 | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp
index 5826b6249f..ef8ca9dc1c 100644
--- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp
+++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp
@@ -115,6 +115,14 @@ class RobotTrajectoryTestFixture : public testing::Test
     std::vector<double> trajectory_first_state_after_update;
     trajectory_first_waypoint_after_update->copyJointGroupPositions(arm_jmg_name_, trajectory_first_state_after_update);
     EXPECT_EQ(trajectory_first_state[0], trajectory_first_state_after_update[0]);
+
+    // Modify the first waypoint duration
+    double trajectory_first_duration_before_update = trajectory->getWayPointDurationFromPrevious(0);
+    double new_duration = trajectory_first_duration_before_update + 0.1;
+    trajectory->setWayPointDurationFromPrevious(0, new_duration);
+
+    // Check that the trajectory's first duration was updated
+    EXPECT_EQ(trajectory->getWayPointDurationFromPrevious(0), new_duration);
   }
 
   void modifyFirstWaypointAndCheckTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory)
@@ -212,7 +220,7 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryShallowCopy)
   trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_,
                                                                       trajectory_copy_first_state_after_update);
 
-  // Check that we updated the value correctly in the trajectory
+  // Check that we updated the joint position correctly in the trajectory
   EXPECT_EQ(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]);
 }
 
@@ -238,8 +246,10 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDeepCopy)
   trajectory_copy_first_waypoint_after_update.copyJointGroupPositions(arm_jmg_name_,
                                                                       trajectory_copy_first_state_after_update);
 
-  // Check that we updated the value correctly in the trajectory
+  // Check that joint positions changed in the original trajectory but not the deep copy
   EXPECT_NE(trajectory_first_state_after_update[0], trajectory_copy_first_state_after_update[0]);
+  // Check that the first waypoint duration changed in the original trajectory but not the deep copy
+  EXPECT_NE(trajectory->getWayPointDurationFromPrevious(0), trajectory_copy->getWayPointDurationFromPrevious(0));
 }
 
 int main(int argc, char** argv)

From 928afabe6cd857e15ab9e5b1c35f621c3f586ae6 Mon Sep 17 00:00:00 2001
From: Colin Kohler <72100281+Colin-Kohler@users.noreply.github.com>
Date: Tue, 16 Nov 2021 15:07:54 -0500
Subject: [PATCH 022/114] MoveitCpp - added ability to set path constraints for
 PlanningComponent. (#2959)

---
 .../include/moveit/moveit_cpp/planning_component.h       | 4 ++++
 .../planning/moveit_cpp/src/planning_component.cpp       | 9 +++++++++
 2 files changed, 13 insertions(+)

diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h
index 702c275d78..14873f9011 100644
--- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h
+++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h
@@ -175,6 +175,9 @@ class PlanningComponent
   /** \brief Set the goal constraints generated from a named target state */
   bool setGoal(const std::string& named_target);
 
+  /** \brief Set the path constraints used for planning */
+  bool setPathConstraints(const moveit_msgs::Constraints& path_constraints);
+
   /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using
    * default parameters. */
   PlanSolution plan();
@@ -202,6 +205,7 @@ class PlanningComponent
   // The start state used in the planning motion request
   moveit::core::RobotStatePtr considered_start_state_;
   std::vector<moveit_msgs::Constraints> current_goal_constraints_;
+  moveit_msgs::Constraints current_path_constraints_;
   PlanRequestParameters plan_request_parameters_;
   moveit_msgs::WorkspaceParameters workspace_parameters_;
   bool workspace_parameters_set_ = false;
diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp
index dedd3b0dcc..515e186394 100644
--- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp
+++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp
@@ -99,6 +99,12 @@ const std::string& PlanningComponent::getPlanningGroupName() const
   return group_name_;
 }
 
+bool PlanningComponent::setPathConstraints(const moveit_msgs::Constraints& path_constraints)
+{
+  current_path_constraints_ = path_constraints;
+  return true;
+}
+
 PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters)
 {
   last_plan_solution_ = std::make_shared<PlanSolution>();
@@ -147,6 +153,9 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet
   }
   req.goal_constraints = current_goal_constraints_;
 
+  // Set path constraints
+  req.path_constraints = current_path_constraints_;
+
   // Run planning attempt
   ::planning_interface::MotionPlanResponse res;
   if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end())

From ed7b877bb9523a6e88ca629358fee79c4c666a0f Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Wed, 3 Nov 2021 21:51:57 +0100
Subject: [PATCH 023/114] Joints widget: avoid flickering of the nullspace
 slider

Show a (disabled) dummy slider if there is no nullspace.
This avoids flickering between zero and one slider, which is the most common case.
Also provide some tooltips to explain the usage.
---
 .../motion_planning_frame_joints_widget.cpp   |  3 ++
 ...otion_planning_rviz_plugin_frame_joints.ui | 34 +++++++++++++++++--
 2 files changed, 35 insertions(+), 2 deletions(-)

diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp
index 652c2e0e30..0ee9b0127c 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp
@@ -335,6 +335,9 @@ void MotionPlanningFrameJointsWidget::updateNullspaceSliders()
   if (i == 0)
     nullspace_.resize(0, 0);
 
+  // show/hide dummy slider
+  ui_->dummy_ns_slider_->setVisible(i == 0);
+
   // hide remaining sliders
   for (; i < ns_sliders_.size(); ++i)
     ns_sliders_[i]->hide();
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui
index 389365e9bd..ecde6d0514 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame_joints.ui
@@ -43,14 +43,44 @@
       </widget>
      </item>
      <item>
-      <widget class="QLabel" name="label">
+      <widget class="QLabel" name="nullspace_label">
+       <property name="toolTip">
+        <string>The sliders below allow for jogging the nullspace of the current configuration,
+i.e. trigger joint motions that don't affect the end-effector pose.
+
+Typically, redundant arms (with 7+ joints) offer such a nullspace.
+However, also singular configurations provide a nullspace.
+
+Each basis vector of the (linear) nullspace is represented by a separate slider.</string>
+       </property>
        <property name="text">
         <string>Nullspace exploration:</string>
        </property>
       </widget>
      </item>
      <item>
-      <layout class="QVBoxLayout" name="nullspace_layout_"/>
+      <layout class="QVBoxLayout" name="nullspace_layout_">
+       <item>
+        <widget class="QSlider" name="dummy_ns_slider_">
+         <property name="enabled">
+          <bool>false</bool>
+         </property>
+         <property name="toolTip">
+          <string>The slider will become active if the current robot configuration has a nullspace.
+That's typically the case for redundant robots, i.e. 7+ joints, or singular configurations.</string>
+         </property>
+         <property name="minimum">
+          <number>-1</number>
+         </property>
+         <property name="maximum">
+          <number>1</number>
+         </property>
+         <property name="orientation">
+          <enum>Qt::Horizontal</enum>
+         </property>
+        </widget>
+       </item>
+      </layout>
      </item>
     </layout>
    </item>

From 03ce4cf7dd6f679c1f8a65dc357c1aeab60d2642 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Thu, 18 Nov 2021 13:40:21 +0100
Subject: [PATCH 024/114] MPD: Avoid flickering of the progress bar

The progress bar shows the number of pending background jobs.
If there is only one job pending, the progress bar is shown and
immediately hidden as soon as the process is finished.
Thus, we shouldn't show the progress bar if there is only one job
and thus no actual progress to show.

Use the default size and color scheme.
---
 .../src/motion_planning_display.cpp           | 22 ++----
 .../ui/motion_planning_rviz_plugin_frame.ui   | 70 -------------------
 2 files changed, 7 insertions(+), 85 deletions(-)

diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
index b68d565d50..b09793418b 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
@@ -336,28 +336,20 @@ void MotionPlanningDisplay::updateBackgroundJobProgressBar()
 
   if (n == 0)
   {
-    p->setValue(p->maximum());
-    p->update();
     p->hide();
     p->setMaximum(0);
+    p->setValue(0);
   }
   else
   {
-    if (n == 1)
-    {
-      if (p->maximum() == 0)
-        p->setValue(0);
-      else
-        p->setValue(p->maximum() - 1);
-    }
-    else
+    if (p->maximum() < n)  // increase max
     {
-      if (p->maximum() < n)
-        p->setMaximum(n);
-      else
-        p->setValue(p->maximum() - n);
+      p->setMaximum(n);
+      if (n > 1)  // only show bar if there will be a progress to show
+        p->show();
     }
-    p->show();
+    else  // progress
+      p->setValue(p->maximum() - n);
     p->update();
   }
 }
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui
index 221318fd01..346228701e 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui
@@ -2042,82 +2042,12 @@ This is usually achieved by random seeding, which can flip the robot configurati
        <verstretch>0</verstretch>
       </sizepolicy>
      </property>
-     <property name="minimumSize">
-      <size>
-       <width>0</width>
-       <height>5</height>
-      </size>
-     </property>
      <property name="maximumSize">
       <size>
        <width>16777215</width>
        <height>5</height>
       </size>
      </property>
-     <property name="palette">
-      <palette>
-       <active>
-        <colorrole role="Highlight">
-         <brush brushstyle="SolidPattern">
-          <color alpha="255">
-           <red>255</red>
-           <green>255</green>
-           <blue>255</blue>
-          </color>
-         </brush>
-        </colorrole>
-        <colorrole role="HighlightedText">
-         <brush brushstyle="SolidPattern">
-          <color alpha="255">
-           <red>255</red>
-           <green>0</green>
-           <blue>0</blue>
-          </color>
-         </brush>
-        </colorrole>
-       </active>
-       <inactive>
-        <colorrole role="Highlight">
-         <brush brushstyle="SolidPattern">
-          <color alpha="255">
-           <red>255</red>
-           <green>255</green>
-           <blue>255</blue>
-          </color>
-         </brush>
-        </colorrole>
-        <colorrole role="HighlightedText">
-         <brush brushstyle="SolidPattern">
-          <color alpha="255">
-           <red>255</red>
-           <green>0</green>
-           <blue>0</blue>
-          </color>
-         </brush>
-        </colorrole>
-       </inactive>
-       <disabled>
-        <colorrole role="Highlight">
-         <brush brushstyle="SolidPattern">
-          <color alpha="255">
-           <red>240</red>
-           <green>240</green>
-           <blue>240</blue>
-          </color>
-         </brush>
-        </colorrole>
-        <colorrole role="HighlightedText">
-         <brush brushstyle="SolidPattern">
-          <color alpha="255">
-           <red>255</red>
-           <green>0</green>
-           <blue>0</blue>
-          </color>
-         </brush>
-        </colorrole>
-       </disabled>
-      </palette>
-     </property>
      <property name="whatsThis">
       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Progress of background jobs&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
      </property>

From ab42a1d7017b27eb6c353fb29331b2da08ab0039 Mon Sep 17 00:00:00 2001
From: Jochen Sprickerhof <git@jochen.sprickerhof.de>
Date: Thu, 18 Nov 2021 20:59:06 +0100
Subject: [PATCH 025/114] Switch to std::bind (#2967)

* boost::bind -> std::bind
  grep -rlI --exclude-dir=.git "boost::bind" | xargs sed -i 's/boost::bind/std::bind/g'
* Convert bind placeholders
  grep -rlI --exclude-dir=.git " _[0-9]" | xargs sed -i 's/ _\([0-9]\)/ std::placeholders::_\1/g'
* Update bind include header
  grep -rlI --exclude-dir=.git "boost/bind" | xargs sed -i 's#boost/bind.hpp#functional#'
---
 .../src/background_processing.cpp             |  2 +-
 .../src/collision_matrix.cpp                  |  4 +-
 .../collision_detection/src/world_diff.cpp    | 14 ++--
 .../collision_detection/test/test_world.cpp   | 10 +--
 .../src/collision_env_bullet.cpp              | 14 ++--
 .../src/collision_env_fcl.cpp                 | 12 ++-
 .../src/collision_env_distance_field.cpp      | 14 ++--
 .../src/default_constraint_samplers.cpp       |  6 +-
 .../test/test_constraint_samplers.cpp         |  6 +-
 .../src/kinematic_constraint.cpp              |  4 +-
 .../src/planning_request_adapter.cpp          | 15 ++--
 moveit_core/robot_state/src/robot_state.cpp   |  8 +-
 .../src/kinematics_constraint_aware.cpp       |  5 +-
 .../test/test_kinematics_plugin.cpp           |  6 +-
 .../src/ompl_planner_manager.cpp              |  2 +-
 .../src/move_group_sequence_action.cpp        | 10 +--
 .../src/trajectory_functions.cpp              |  4 +-
 .../integrationtest_command_list_manager.cpp  |  4 +-
 ...rationtest_sequence_service_capability.cpp |  4 +-
 .../test/unittest_trajectory_functions.cpp    |  4 +-
 .../src/xml_testdata_loader.cpp               | 40 +++++-----
 .../src/sbpl_meta_interface.cpp               |  8 +-
 .../src/moveit_fake_controllers.cpp           |  2 +-
 .../gripper_controller_handle.h               |  9 ++-
 ...low_joint_trajectory_controller_handle.cpp | 10 ++-
 .../benchmarks/src/benchmark_execution.cpp    | 13 +--
 .../src/pick_place_action_capability.cpp      | 40 +++++-----
 .../src/approach_and_translate_stage.cpp      | 11 +--
 .../pick_place/src/manipulation_pipeline.cpp  |  2 +-
 .../pick_place/src/pick_place.cpp             |  4 +-
 .../pick_place/src/pick_place_params.cpp      |  4 +-
 .../src/reachable_valid_pose_filter.cpp       |  7 +-
 .../cartesian_path_service_capability.cpp     |  5 +-
 .../execute_trajectory_action_capability.cpp  |  4 +-
 .../execute_trajectory_service_capability.cpp |  3 +-
 .../kinematics_service_capability.cpp         | 11 +--
 .../move_action_capability.cpp                | 20 ++---
 .../src/occupancy_map_monitor.cpp             | 15 ++--
 .../src/occupancy_map_server.cpp              |  2 +-
 .../src/depth_image_octomap_updater.cpp       |  3 +-
 .../src/lazy_free_space_updater.cpp           |  4 +-
 .../src/depth_self_filter_nodelet.cpp         |  2 +-
 .../mesh_filter/test/mesh_filter_test.cpp     |  3 +-
 .../src/pointcloud_octomap_updater.cpp        |  9 ++-
 .../src/kinematics_plugin_loader.cpp          |  5 +-
 .../plan_execution/src/plan_execution.cpp     | 13 +--
 .../plan_execution/src/plan_with_sensing.cpp  |  4 +-
 .../src/evaluate_collision_checking_speed.cpp |  2 +-
 .../src/current_state_monitor.cpp             |  2 +-
 .../src/planning_scene_monitor.cpp            | 48 +++++------
 .../src/trajectory_monitor.cpp                |  2 +-
 .../src/trajectory_execution_manager.cpp      |  6 +-
 .../src/move_group_interface.cpp              |  2 +-
 .../src/interaction_handler.cpp               | 14 ++--
 .../src/robot_interaction.cpp                 |  8 +-
 .../test/locked_robot_state_test.cpp          |  2 +-
 .../src/motion_planning_display.cpp           | 36 +++++----
 .../src/motion_planning_frame.cpp             | 28 +++----
 .../src/motion_planning_frame_context.cpp     | 17 ++--
 .../motion_planning_frame_manipulation.cpp    | 15 ++--
 .../src/motion_planning_frame_objects.cpp     | 22 ++---
 .../src/motion_planning_frame_planning.cpp    | 18 ++---
 .../src/motion_planning_frame_scenes.cpp      | 18 ++---
 .../src/planning_scene_display.cpp            | 13 +--
 .../warehouse/src/save_to_warehouse.cpp       | 10 ++-
 .../warehouse/src/warehouse_services.cpp      | 12 +--
 .../src/tools/compute_default_collisions.cpp  |  2 +-
 .../widgets/configuration_files_widget.cpp    | 80 +++++++++----------
 .../src/widgets/default_collisions_widget.cpp |  5 +-
 69 files changed, 411 insertions(+), 352 deletions(-)

diff --git a/moveit_core/background_processing/src/background_processing.cpp b/moveit_core/background_processing/src/background_processing.cpp
index 5614aa514a..00df01de05 100644
--- a/moveit_core/background_processing/src/background_processing.cpp
+++ b/moveit_core/background_processing/src/background_processing.cpp
@@ -46,7 +46,7 @@ BackgroundProcessing::BackgroundProcessing()
   // spin a thread that will process user events
   run_processing_thread_ = true;
   processing_ = false;
-  processing_thread_ = std::make_unique<boost::thread>(boost::bind(&BackgroundProcessing::processingThread, this));
+  processing_thread_ = std::make_unique<boost::thread>(std::bind(&BackgroundProcessing::processingThread, this));
 }
 
 BackgroundProcessing::~BackgroundProcessing()
diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp
index 05ea5b66ba..369774ef18 100644
--- a/moveit_core/collision_detection/src/collision_matrix.cpp
+++ b/moveit_core/collision_detection/src/collision_matrix.cpp
@@ -35,7 +35,7 @@
 /* Author: Ioan Sucan, E. Gil Jones */
 
 #include <moveit/collision_detection/collision_matrix.h>
-#include <boost/bind.hpp>
+#include <functional>
 #include <iomanip>
 
 namespace collision_detection
@@ -268,7 +268,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const
     else if (!found1 && found2)
       fn = fn2;
     else if (found1 && found2)
-      fn = boost::bind(&andDecideContact, fn1, fn2, _1);
+      fn = std::bind(&andDecideContact, fn1, fn2, std::placeholders::_1);
     else
       return false;
     return true;
diff --git a/moveit_core/collision_detection/src/world_diff.cpp b/moveit_core/collision_detection/src/world_diff.cpp
index dcf75b6d4d..c43b9d920a 100644
--- a/moveit_core/collision_detection/src/world_diff.cpp
+++ b/moveit_core/collision_detection/src/world_diff.cpp
@@ -35,7 +35,7 @@
 /* Author: Acorn Pooley, Ioan Sucan */
 
 #include <moveit/collision_detection/world_diff.h>
-#include <boost/bind.hpp>
+#include <functional>
 
 namespace collision_detection
 {
@@ -52,7 +52,8 @@ WorldDiff::WorldDiff()
 
 WorldDiff::WorldDiff(const WorldPtr& world) : world_(world)
 {
-  observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2));
+  observer_handle_ =
+      world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2));
 }
 
 WorldDiff::WorldDiff(WorldDiff& other)
@@ -63,7 +64,8 @@ WorldDiff::WorldDiff(WorldDiff& other)
     changes_ = other.changes_;
 
     WorldWeakPtr(world).swap(world_);
-    observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2));
+    observer_handle_ =
+        world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2));
   }
 }
 
@@ -87,7 +89,8 @@ void WorldDiff::reset(const WorldPtr& world)
     old_world->removeObserver(observer_handle_);
 
   WorldWeakPtr(world).swap(world_);
-  observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2));
+  observer_handle_ =
+      world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2));
 }
 
 void WorldDiff::setWorld(const WorldPtr& world)
@@ -101,7 +104,8 @@ void WorldDiff::setWorld(const WorldPtr& world)
 
   WorldWeakPtr(world).swap(world_);
 
-  observer_handle_ = world->addObserver(boost::bind(&WorldDiff::notify, this, _1, _2));
+  observer_handle_ =
+      world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2));
   world->notifyObserverAllObjects(observer_handle_, World::CREATE | World::ADD_SHAPE);
 }
 
diff --git a/moveit_core/collision_detection/test/test_world.cpp b/moveit_core/collision_detection/test/test_world.cpp
index 126143dafc..824ce083f1 100644
--- a/moveit_core/collision_detection/test/test_world.cpp
+++ b/moveit_core/collision_detection/test/test_world.cpp
@@ -37,7 +37,7 @@
 #include <gtest/gtest.h>
 #include <moveit/collision_detection/world.h>
 #include <geometric_shapes/shapes.h>
-#include <boost/bind.hpp>
+#include <functional>
 
 TEST(World, AddRemoveShape)
 {
@@ -236,7 +236,7 @@ TEST(World, TrackChanges)
 
   TestAction ta;
   collision_detection::World::ObserverHandle observer_ta;
-  observer_ta = world.addObserver(boost::bind(TrackChangesNotify, &ta, _1, _2));
+  observer_ta = world.addObserver(std::bind(TrackChangesNotify, &ta, std::placeholders::_1, std::placeholders::_2));
 
   // Create some shapes
   shapes::ShapePtr ball(new shapes::Sphere(1.0));
@@ -267,7 +267,7 @@ TEST(World, TrackChanges)
 
   TestAction ta2;
   collision_detection::World::ObserverHandle observer_ta2;
-  observer_ta2 = world.addObserver(boost::bind(TrackChangesNotify, &ta2, _1, _2));
+  observer_ta2 = world.addObserver(std::bind(TrackChangesNotify, &ta2, std::placeholders::_1, std::placeholders::_2));
 
   world.addToObject("obj2", cyl, Eigen::Isometry3d::Identity());
 
@@ -305,7 +305,7 @@ TEST(World, TrackChanges)
 
   TestAction ta3;
   collision_detection::World::ObserverHandle observer_ta3;
-  observer_ta3 = world.addObserver(boost::bind(TrackChangesNotify, &ta3, _1, _2));
+  observer_ta3 = world.addObserver(std::bind(TrackChangesNotify, &ta3, std::placeholders::_1, std::placeholders::_2));
 
   bool rm_good = world.removeShapeFromObject("obj2", cyl);
   EXPECT_TRUE(rm_good);
@@ -371,7 +371,7 @@ TEST(World, ObjectPoseAndSubframes)
 
   TestAction ta;
   collision_detection::World::ObserverHandle observer_ta;
-  observer_ta = world.addObserver(boost::bind(TrackChangesNotify, &ta, _1, _2));
+  observer_ta = world.addObserver(std::bind(TrackChangesNotify, &ta, std::placeholders::_1, std::placeholders::_2));
 
   // Create shapes
   shapes::ShapePtr ball(new shapes::Sphere(1.0));
diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp
index 3885f721dc..3e2ea9b1ab 100644
--- a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp
+++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp
@@ -38,7 +38,7 @@
 #include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>
 #include <moveit/collision_detection_bullet/bullet_integration/ros_bullet_utils.h>
 #include <moveit/collision_detection_bullet/bullet_integration/contact_checker_common.h>
-#include <boost/bind.hpp>
+#include <functional>
 #include <bullet/btBulletCollisionCommon.h>
 
 namespace collision_detection
@@ -54,7 +54,8 @@ CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& m
   : CollisionEnv(model, padding, scale)
 {
   // request notifications about changes to new world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
 
   for (const std::pair<const std::string, urdf::LinkSharedPtr>& link : robot_model_->getURDF()->links_)
   {
@@ -67,7 +68,8 @@ CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& m
   : CollisionEnv(model, world, padding, scale)
 {
   // request notifications about changes to new world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
 
   for (const std::pair<const std::string, urdf::LinkSharedPtr>& link : robot_model_->getURDF()->links_)
   {
@@ -81,7 +83,8 @@ CollisionEnvBullet::CollisionEnvBullet(const CollisionEnvBullet& other, const Wo
   : CollisionEnv(other, world)
 {
   // request notifications about changes to new world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
 
   for (const std::pair<const std::string, urdf::LinkSharedPtr>& link : other.robot_model_->getURDF()->links_)
   {
@@ -304,7 +307,8 @@ void CollisionEnvBullet::setWorld(const WorldPtr& world)
   CollisionEnv::setWorld(world);
 
   // request notifications about changes to new world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvBullet::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
 
   // get notifications any objects already in the new world
   getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
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 4895b27baf..b549f179fb 100644
--- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
+++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
@@ -105,7 +105,8 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model,
   manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
 
   // request notifications about changes to new world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
 }
 
 CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding,
@@ -140,7 +141,8 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model,
   manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
 
   // request notifications about changes to new world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
   getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
 }
 
@@ -162,7 +164,8 @@ CollisionEnvFCL::CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& w
   // manager_->update();
 
   // request notifications about changes to new world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
 }
 
 void CollisionEnvFCL::getAttachedBodyObjects(const moveit::core::AttachedBody* ab,
@@ -403,7 +406,8 @@ void CollisionEnvFCL::setWorld(const WorldPtr& world)
   CollisionEnv::setWorld(world);
 
   // request notifications about changes to new world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvFCL::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
 
   // get notifications any objects already in the new world
   getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
index dee29b010b..5e2c34bfbe 100644
--- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
+++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
@@ -42,7 +42,7 @@
 #include <moveit/collision_distance_field/collision_common_distance_field.h>
 #include <moveit/distance_field/propagation_distance_field.h>
 #include <moveit/collision_distance_field/collision_detector_allocator_distance_field.h>
-#include <boost/bind.hpp>
+#include <functional>
 #include <memory>
 #include <utility>
 
@@ -69,7 +69,8 @@ CollisionEnvDistanceField::CollisionEnvDistanceField(
   distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld();
 
   // request notifications about changes to world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
 }
 
 CollisionEnvDistanceField::CollisionEnvDistanceField(
@@ -85,7 +86,8 @@ CollisionEnvDistanceField::CollisionEnvDistanceField(
   distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld();
 
   // request notifications about changes to world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
 
   getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
 }
@@ -108,7 +110,8 @@ CollisionEnvDistanceField::CollisionEnvDistanceField(const CollisionEnvDistanceF
   planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
 
   // request notifications about changes to world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
   getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
 }
 
@@ -1705,7 +1708,8 @@ void CollisionEnvDistanceField::setWorld(const WorldPtr& world)
   CollisionEnv::setWorld(world);
 
   // request notifications about changes to new world
-  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvDistanceField::notifyObjectChange, this, _1, _2));
+  observer_handle_ = getWorld()->addObserver(
+      std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
 
   // get notifications any objects already in the new world
   getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
index 55ed6a618a..5d28e21206 100644
--- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
+++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
@@ -36,7 +36,7 @@
 
 #include <moveit/constraint_samplers/default_constraint_samplers.h>
 #include <cassert>
-#include <boost/bind.hpp>
+#include <functional>
 
 namespace constraint_samplers
 {
@@ -565,8 +565,8 @@ bool IKConstraintSampler::sampleHelper(moveit::core::RobotState& state, const mo
 
   kinematics::KinematicsBase::IKCallbackFn adapted_ik_validity_callback;
   if (group_state_validity_callback_)
-    adapted_ik_validity_callback =
-        boost::bind(&samplingIkCallbackFnAdapter, &state, jmg_, group_state_validity_callback_, _1, _2, _3);
+    adapted_ik_validity_callback = std::bind(&samplingIkCallbackFnAdapter, &state, jmg_, group_state_validity_callback_,
+                                             std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
 
   for (unsigned int a = 0; a < max_attempts; ++a)
   {
diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp
index 3cfd3a957f..f01f1e188a 100644
--- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp
+++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp
@@ -49,7 +49,7 @@
 #include <gtest/gtest.h>
 #include <urdf_parser/urdf_parser.h>
 #include <fstream>
-#include <boost/bind.hpp>
+#include <functional>
 
 #include "pr2_arm_kinematics_plugin.h"
 
@@ -82,8 +82,8 @@ class LoadPlanningModelsPr2 : public testing::Test
     pr2_kinematics_plugin_left_arm_->initialize(*robot_model_, "left_arm", "torso_lift_link", { "l_wrist_roll_link" },
                                                 .01);
 
-    func_right_arm_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, _1);
-    func_left_arm_ = boost::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, _1);
+    func_right_arm_ = std::bind(&LoadPlanningModelsPr2::getKinematicsSolverRightArm, this, std::placeholders::_1);
+    func_left_arm_ = std::bind(&LoadPlanningModelsPr2::getKinematicsSolverLeftArm, this, std::placeholders::_1);
 
     std::map<std::string, moveit::core::SolverAllocatorFn> allocators;
     allocators["right_arm"] = func_right_arm_;
diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
index 2588bd4541..7c3629c3a9 100644
--- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
+++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
@@ -42,7 +42,7 @@
 #include <geometric_shapes/check_isometry.h>
 #include <boost/math/constants/constants.hpp>
 #include <tf2_eigen/tf2_eigen.h>
-#include <boost/bind.hpp>
+#include <functional>
 #include <limits>
 #include <memory>
 
@@ -1112,7 +1112,7 @@ ConstraintEvaluationResult VisibilityConstraint::decide(const moveit::core::Robo
   collision_detection::CollisionRequest req;
   collision_detection::CollisionResult res;
   collision_detection::AllowedCollisionMatrix acm;
-  acm.setDefaultEntry("cone", boost::bind(&VisibilityConstraint::decideContact, this, _1));
+  acm.setDefaultEntry("cone", std::bind(&VisibilityConstraint::decideContact, this, std::placeholders::_1));
   req.contacts = true;
   req.verbose = verbose;
   req.max_contacts = 1;
diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
index aa04e8a8c9..35fac2f52d 100644
--- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
+++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
@@ -35,7 +35,7 @@
 /* Author: Ioan Sucan */
 
 #include <moveit/planning_request_adapter/planning_request_adapter.h>
-#include <boost/bind.hpp>
+#include <functional>
 #include <algorithm>
 
 // we could really use some c++11 lambda functions here :)
@@ -63,8 +63,9 @@ bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManag
                                           planning_interface::MotionPlanResponse& res,
                                           std::vector<std::size_t>& added_path_index) const
 {
-  return adaptAndPlan(boost::bind(&callPlannerInterfaceSolve, planner.get(), _1, _2, _3), planning_scene, req, res,
-                      added_path_index);
+  return adaptAndPlan(std::bind(&callPlannerInterfaceSolve, planner.get(), std::placeholders::_1, std::placeholders::_2,
+                                std::placeholders::_3),
+                      planning_scene, req, res, added_path_index);
 }
 
 bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
@@ -145,10 +146,12 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner
 
     // if there are adapters, construct a function pointer for each, in order,
     // so that in the end we have a nested sequence of function pointers that call the adapters in the correct order.
-    PlanningRequestAdapter::PlannerFn fn = boost::bind(&callAdapter1, adapters_.back().get(), planner, _1, _2, _3,
-                                                       boost::ref(added_path_index_each.back()));
+    PlanningRequestAdapter::PlannerFn fn =
+        std::bind(&callAdapter1, adapters_.back().get(), planner, std::placeholders::_1, std::placeholders::_2,
+                  std::placeholders::_3, boost::ref(added_path_index_each.back()));
     for (int i = adapters_.size() - 2; i >= 0; --i)
-      fn = boost::bind(&callAdapter2, adapters_[i].get(), fn, _1, _2, _3, boost::ref(added_path_index_each[i]));
+      fn = std::bind(&callAdapter2, adapters_[i].get(), fn, std::placeholders::_1, std::placeholders::_2,
+                     std::placeholders::_3, boost::ref(added_path_index_each[i]));
     bool result = fn(planning_scene, req, res);
     added_path_index.clear();
 
diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
index c60cd509cc..3d74b0a439 100644
--- a/moveit_core/robot_state/src/robot_state.cpp
+++ b/moveit_core/robot_state/src/robot_state.cpp
@@ -44,7 +44,7 @@
 #include <moveit/backtrace/backtrace.h>
 #include <moveit/profiler/profiler.h>
 #include <moveit/macros/console_colors.h>
-#include <boost/bind.hpp>
+#include <functional>
 #include <moveit/robot_model/aabb.h>
 
 namespace moveit
@@ -1780,7 +1780,8 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
   // set callback function
   kinematics::KinematicsBase::IKCallbackFn ik_callback_fn;
   if (constraint)
-    ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3);
+    ik_callback_fn = std::bind(&ikCallbackFnAdapter, this, jmg, constraint, std::placeholders::_1,
+                               std::placeholders::_2, std::placeholders::_3);
 
   // Bijection
   const std::vector<unsigned int>& bij = jmg->getKinematicsSolverJointBijection();
@@ -1923,7 +1924,8 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL::
   std::vector<geometry_msgs::Pose> ik_queries(poses_in.size());
   kinematics::KinematicsBase::IKCallbackFn ik_callback_fn;
   if (constraint)
-    ik_callback_fn = boost::bind(&ikCallbackFnAdapter, this, jmg, constraint, _1, _2, _3);
+    ik_callback_fn = std::bind(&ikCallbackFnAdapter, this, jmg, constraint, std::placeholders::_1,
+                               std::placeholders::_2, std::placeholders::_3);
 
   for (std::size_t i = 0; i < transformed_poses.size(); ++i)
   {
diff --git a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp
index b75ae595d2..b9fdc21666 100644
--- a/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp
+++ b/moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp
@@ -41,7 +41,7 @@
 #include <moveit/planning_scene/planning_scene.h>
 #include <Eigen/Geometry.h>
 #include <tf2_eigen/tf2_eigen.h>
-#include <boost/bind.hpp>
+#include <functional>
 
 namespace kinematics_constraint_aware
 {
@@ -161,7 +161,8 @@ bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPt
       transformPoses(planning_scene, kinematic_state, request.pose_stamped_vector_, kinematic_model_->getModelFrame());
 
   moveit::core::StateValidityCallbackFn constraint_callback_fn =
-      boost::bind(&KinematicsConstraintAware::validityCallbackFn, this, planning_scene, request, response, _1, _2);
+      std::bind(&KinematicsConstraintAware::validityCallbackFn, this, planning_scene, request, response,
+                std::placeholders::_1, std::placeholders::_2);
 
   bool result = false;
   if (has_sub_groups_)
diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp
index 7ca176cd0b..60fc35b37f 100644
--- a/moveit_kinematics/test/test_kinematics_plugin.cpp
+++ b/moveit_kinematics/test/test_kinematics_plugin.cpp
@@ -37,7 +37,7 @@
 
 #include <gtest/gtest.h>
 #include <memory>
-#include <boost/bind.hpp>
+#include <functional>
 #include <pluginlib/class_loader.h>
 #include <ros/ros.h>
 #include <tf2_eigen/tf2_eigen.h>
@@ -592,7 +592,9 @@ TEST_F(KinematicsTest, searchIKWithCallback)
     }
 
     kinematics_solver_->searchPositionIK(poses[0], fk_values, timeout_, solution,
-                                         boost::bind(&KinematicsTest::searchIKCallback, this, _1, _2, _3), error_code);
+                                         std::bind(&KinematicsTest::searchIKCallback, this, std::placeholders::_1,
+                                                   std::placeholders::_2, std::placeholders::_3),
+                                         error_code);
     if (error_code.val == error_code.SUCCESS)
       success++;
     else
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 199c873ab9..c201a066fe 100644
--- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp
+++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp
@@ -295,7 +295,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager
       pub_valid_states_ = nh_.advertise<moveit_msgs::DisplayRobotState>("ompl_planner_valid_states", 5);
       pub_valid_traj_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("ompl_planner_valid_trajectories", 5);
       display_random_valid_states_ = true;
-      //    pub_valid_states_thread_.reset(new boost::thread(boost::bind(&OMPLPlannerManager::displayRandomValidStates,
+      //    pub_valid_states_thread_.reset(new boost::thread(std::bind(&OMPLPlannerManager::displayRandomValidStates,
       //    this)));
     }
   }
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 be1caeb2c6..d7c0bac0f6 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
@@ -63,8 +63,8 @@ void MoveGroupSequenceAction::initialize()
   ROS_INFO_STREAM("initialize move group sequence action");
   move_action_server_ = std::make_unique<actionlib::SimpleActionServer<moveit_msgs::MoveGroupSequenceAction>>(
       root_node_handle_, "sequence_move_group",
-      boost::bind(&MoveGroupSequenceAction::executeSequenceCallback, this, _1), false);
-  move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupSequenceAction::preemptMoveCallback, this));
+      std::bind(&MoveGroupSequenceAction::executeSequenceCallback, this, std::placeholders::_1), false);
+  move_action_server_->registerPreemptCallback(std::bind(&MoveGroupSequenceAction::preemptMoveCallback, this));
   move_action_server_->start();
 
   command_list_manager_ = std::make_unique<pilz_industrial_motion_planner::CommandListManager>(
@@ -137,10 +137,10 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute(
   opt.replan_ = goal->planning_options.replan;
   opt.replan_attempts_ = goal->planning_options.replan_attempts;
   opt.replan_delay_ = goal->planning_options.replan_delay;
-  opt.before_execution_callback_ = boost::bind(&MoveGroupSequenceAction::startMoveExecutionCallback, this);
+  opt.before_execution_callback_ = std::bind(&MoveGroupSequenceAction::startMoveExecutionCallback, this);
 
-  opt.plan_callback_ =
-      boost::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, boost::cref(goal->request), _1);
+  opt.plan_callback_ = std::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, boost::cref(goal->request),
+                                 std::placeholders::_1);
 
   if (goal->planning_options.look_around && context_->plan_with_sensing_)
   {
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 93ba27e133..03694adf8f 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp
@@ -74,8 +74,8 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin
   rstate.setVariablePositions(seed);
 
   moveit::core::GroupStateValidityCallbackFn ik_constraint_function;
-  ik_constraint_function =
-      boost::bind(&pilz_industrial_motion_planner::isStateColliding, check_self_collision, scene, _1, _2, _3);
+  ik_constraint_function = std::bind(&pilz_industrial_motion_planner::isStateColliding, check_self_collision, scene,
+                                     std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
 
   // call ik
   if (rstate.setFromIK(robot_model->getJointModelGroup(group_name), pose, link_name, timeout, ik_constraint_function))
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp
index e7f1f01b99..fa596dcbc0 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_command_list_manager.cpp
@@ -624,13 +624,13 @@ TEST_F(IntegrationTestCommandListManager, TestGroupSpecificStartState)
   seq.erase(4, seq.size());
 
   Gripper& gripper{ seq.getCmd<Gripper>(0) };
-  gripper.getStartConfiguration().setCreateJointNameFunc(std::bind(&createGripperJointName, _1));
+  gripper.getStartConfiguration().setCreateJointNameFunc(std::bind(&createGripperJointName, std::placeholders::_1));
   // By deleting the model we guarantee that the start state only consists
   // of joints of the gripper group without the manipulator
   gripper.getStartConfiguration().clearModel();
 
   PtpJointCart& ptp{ seq.getCmd<PtpJointCart>(1) };
-  ptp.getStartConfiguration().setCreateJointNameFunc(std::bind(&createManipulatorJointName, _1));
+  ptp.getStartConfiguration().setCreateJointNameFunc(std::bind(&createManipulatorJointName, std::placeholders::_1));
   // By deleting the model we guarantee that the start state only consists
   // of joints of the manipulator group without the gripper
   ptp.getStartConfiguration().clearModel();
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp
index 2c04855230..5b5bdc7c73 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_sequence_service_capability.cpp
@@ -248,7 +248,9 @@ TEST_F(IntegrationTestSequenceService, TestSecondTrajInvalidStartState)
 
   // Set start state
   using std::placeholders::_1;
-  JointConfiguration config{ "MyGroupName", { -1., 2., -3., 4., -5., 0. }, std::bind(&createJointName, _1) };
+  JointConfiguration config{ "MyGroupName",
+                             { -1., 2., -3., 4., -5., 0. },
+                             std::bind(&createJointName, std::placeholders::_1) };
   req_list.items[1].req.start_state.joint_state = config.toSensorMsg();
 
   moveit_msgs::GetMotionSequence srv;
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp
index 4d4eb79ec3..30ce75d7a6 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp
@@ -456,7 +456,7 @@ TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForVali
 
   std::vector<double> ik_state;
   std::transform(ik_actual1.begin(), ik_actual1.end(), std::back_inserter(ik_state),
-                 boost::bind(&std::map<std::string, double>::value_type::second, _1));
+                 std::bind(&std::map<std::string, double>::value_type::second, std::placeholders::_1));
 
   rstate.setJointGroupPositions(jmg, ik_state);
   rstate.update();
@@ -477,7 +477,7 @@ TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForVali
 
   std::vector<double> ik_state2;
   std::transform(ik_actual2.begin(), ik_actual2.end(), std::back_inserter(ik_state2),
-                 boost::bind(&std::map<std::string, double>::value_type::second, _1));
+                 std::bind(&std::map<std::string, double>::value_type::second, std::placeholders::_1));
   rstate.setJointGroupPositions(jmg, ik_state2);
   rstate.update();
 
diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp
index 69054bc45d..38b5f4549b 100644
--- a/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner_testutils/src/xml_testdata_loader.cpp
@@ -141,27 +141,27 @@ XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename) : Testdat
   pt::read_xml(path_filename_, tree_, pt::xml_parser::no_comments);
 
   using std::placeholders::_1;
-  cmd_getter_funcs_["ptp"] =
-      AbstractCmdGetterUPtr(new CmdGetterAdapter<PtpJoint>(std::bind(&XmlTestdataLoader::getPtpJoint, this, _1)));
+  cmd_getter_funcs_["ptp"] = AbstractCmdGetterUPtr(
+      new CmdGetterAdapter<PtpJoint>(std::bind(&XmlTestdataLoader::getPtpJoint, this, std::placeholders::_1)));
   cmd_getter_funcs_["ptp_joint_cart"] = AbstractCmdGetterUPtr(
-      new CmdGetterAdapter<PtpJointCart>(std::bind(&XmlTestdataLoader::getPtpJointCart, this, _1)));
-  cmd_getter_funcs_["ptp_cart_cart"] =
-      AbstractCmdGetterUPtr(new CmdGetterAdapter<PtpCart>(std::bind(&XmlTestdataLoader::getPtpCart, this, _1)));
-
-  cmd_getter_funcs_["lin"] =
-      AbstractCmdGetterUPtr(new CmdGetterAdapter<LinJoint>(std::bind(&XmlTestdataLoader::getLinJoint, this, _1)));
-  cmd_getter_funcs_["lin_cart"] =
-      AbstractCmdGetterUPtr(new CmdGetterAdapter<LinCart>(std::bind(&XmlTestdataLoader::getLinCart, this, _1)));
-
-  cmd_getter_funcs_["circ_center_cart"] = AbstractCmdGetterUPtr(
-      new CmdGetterAdapter<CircCenterCart>(std::bind(&XmlTestdataLoader::getCircCartCenterCart, this, _1)));
-  cmd_getter_funcs_["circ_interim_cart"] = AbstractCmdGetterUPtr(
-      new CmdGetterAdapter<CircInterimCart>(std::bind(&XmlTestdataLoader::getCircCartInterimCart, this, _1)));
-  cmd_getter_funcs_["circ_joint_interim_cart"] = AbstractCmdGetterUPtr(
-      new CmdGetterAdapter<CircJointInterimCart>(std::bind(&XmlTestdataLoader::getCircJointInterimCart, this, _1)));
-
-  cmd_getter_funcs_["gripper"] =
-      AbstractCmdGetterUPtr(new CmdGetterAdapter<Gripper>(std::bind(&XmlTestdataLoader::getGripper, this, _1)));
+      new CmdGetterAdapter<PtpJointCart>(std::bind(&XmlTestdataLoader::getPtpJointCart, this, std::placeholders::_1)));
+  cmd_getter_funcs_["ptp_cart_cart"] = AbstractCmdGetterUPtr(
+      new CmdGetterAdapter<PtpCart>(std::bind(&XmlTestdataLoader::getPtpCart, this, std::placeholders::_1)));
+
+  cmd_getter_funcs_["lin"] = AbstractCmdGetterUPtr(
+      new CmdGetterAdapter<LinJoint>(std::bind(&XmlTestdataLoader::getLinJoint, this, std::placeholders::_1)));
+  cmd_getter_funcs_["lin_cart"] = AbstractCmdGetterUPtr(
+      new CmdGetterAdapter<LinCart>(std::bind(&XmlTestdataLoader::getLinCart, this, std::placeholders::_1)));
+
+  cmd_getter_funcs_["circ_center_cart"] = AbstractCmdGetterUPtr(new CmdGetterAdapter<CircCenterCart>(
+      std::bind(&XmlTestdataLoader::getCircCartCenterCart, this, std::placeholders::_1)));
+  cmd_getter_funcs_["circ_interim_cart"] = AbstractCmdGetterUPtr(new CmdGetterAdapter<CircInterimCart>(
+      std::bind(&XmlTestdataLoader::getCircCartInterimCart, this, std::placeholders::_1)));
+  cmd_getter_funcs_["circ_joint_interim_cart"] = AbstractCmdGetterUPtr(new CmdGetterAdapter<CircJointInterimCart>(
+      std::bind(&XmlTestdataLoader::getCircJointInterimCart, this, std::placeholders::_1)));
+
+  cmd_getter_funcs_["gripper"] = AbstractCmdGetterUPtr(
+      new CmdGetterAdapter<Gripper>(std::bind(&XmlTestdataLoader::getGripper, this, std::placeholders::_1)));
 }
 
 XmlTestdataLoader::XmlTestdataLoader(const std::string& path_filename,
diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp
index 65e6dfef33..36debbe2af 100644
--- a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp
+++ b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_meta_interface.cpp
@@ -57,10 +57,10 @@ bool SBPLMetaInterface::solve(const planning_scene::PlanningSceneConstPtr& plann
   PlanningParameters param_no_bfs;
   param_no_bfs.use_bfs_ = false;
   moveit_msgs::GetMotionPlan::Response res1, res2;
-  boost::thread thread1(boost::bind(&SBPLMetaInterface::runSolver, this, true, boost::cref(planning_scene),
-                                    boost::cref(req), boost::ref(res1), param_bfs));
-  boost::thread thread2(boost::bind(&SBPLMetaInterface::runSolver, this, false, boost::cref(planning_scene),
-                                    boost::cref(req), boost::ref(res2), param_no_bfs));
+  boost::thread thread1(std::bind(&SBPLMetaInterface::runSolver, this, true, boost::cref(planning_scene),
+                                  boost::cref(req), boost::ref(res1), param_bfs));
+  boost::thread thread2(std::bind(&SBPLMetaInterface::runSolver, this, false, boost::cref(planning_scene),
+                                  boost::cref(req), boost::ref(res2), param_no_bfs));
   boost::mutex::scoped_lock lock(planner_done_mutex_);
   planner_done_condition_.wait(lock);
 
diff --git a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp
index 156142157e..50c09184a7 100644
--- a/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp
+++ b/moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp
@@ -124,7 +124,7 @@ bool ThreadedController::sendTrajectory(const moveit_msgs::RobotTrajectory& t)
   cancelTrajectory();  // cancel any previous fake motion
   cancel_ = false;
   status_ = moveit_controller_manager::ExecutionStatus::PREEMPTED;
-  thread_ = boost::thread(boost::bind(&ThreadedController::execTrajectory, this, t));
+  thread_ = boost::thread(std::bind(&ThreadedController::execTrajectory, this, t));
   return true;
 }
 
diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
index aae275a2e2..212992de5b 100644
--- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
+++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
@@ -133,10 +133,11 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
         goal.command.max_effort = trajectory.joint_trajectory.points[tpoint].effort[idx];
     }
 
-    controller_action_client_->sendGoal(goal,
-                                        boost::bind(&GripperControllerHandle::controllerDoneCallback, this, _1, _2),
-                                        boost::bind(&GripperControllerHandle::controllerActiveCallback, this),
-                                        boost::bind(&GripperControllerHandle::controllerFeedbackCallback, this, _1));
+    controller_action_client_->sendGoal(
+        goal,
+        std::bind(&GripperControllerHandle::controllerDoneCallback, this, std::placeholders::_1, std::placeholders::_2),
+        std::bind(&GripperControllerHandle::controllerActiveCallback, this),
+        std::bind(&GripperControllerHandle::controllerFeedbackCallback, this, std::placeholders::_1));
 
     done_ = false;
     last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
diff --git a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp
index d911ed6357..93eb3750a6 100644
--- a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp
+++ b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp
@@ -62,10 +62,12 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::Ro
 
   control_msgs::FollowJointTrajectoryGoal goal = goal_template_;
   goal.trajectory = trajectory.joint_trajectory;
-  controller_action_client_->sendGoal(
-      goal, boost::bind(&FollowJointTrajectoryControllerHandle::controllerDoneCallback, this, _1, _2),
-      boost::bind(&FollowJointTrajectoryControllerHandle::controllerActiveCallback, this),
-      boost::bind(&FollowJointTrajectoryControllerHandle::controllerFeedbackCallback, this, _1));
+  controller_action_client_->sendGoal(goal,
+                                      std::bind(&FollowJointTrajectoryControllerHandle::controllerDoneCallback, this,
+                                                std::placeholders::_1, std::placeholders::_2),
+                                      std::bind(&FollowJointTrajectoryControllerHandle::controllerActiveCallback, this),
+                                      std::bind(&FollowJointTrajectoryControllerHandle::controllerFeedbackCallback,
+                                                this, std::placeholders::_1));
   done_ = false;
   last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
   return true;
diff --git a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp
index 373e5e6b8b..9c6d88fc4f 100644
--- a/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp
+++ b/moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp
@@ -1276,11 +1276,11 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR
     // Compute IK
     ROS_INFO_STREAM("Processing goal " << req.motion_plan_request.goal_constraints[0].name << " ...");
     ros::WallTime startTime = ros::WallTime::now();
-    success =
-        robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose,
-                              req.motion_plan_request.num_planning_attempts,
-                              req.motion_plan_request.allowed_planning_time,
-                              boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable));
+    success = robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose,
+                                    req.motion_plan_request.num_planning_attempts,
+                                    req.motion_plan_request.allowed_planning_time,
+                                    std::bind(&isIKSolutionCollisionFree, planning_scene_.get(), std::placeholders::_1,
+                                              std::placeholders::_2, std::placeholders::_3, &reachable));
     if (success)
     {
       ROS_INFO("  Success!");
@@ -1370,7 +1370,8 @@ void moveit_benchmarks::BenchmarkExecution::runGoalExistenceBenchmark(BenchmarkR
           robot_state.setFromIK(robot_state.getJointModelGroup(req.motion_plan_request.group_name), ik_pose,
                                 req.motion_plan_request.num_planning_attempts,
                                 req.motion_plan_request.allowed_planning_time,
-                                boost::bind(&isIKSolutionCollisionFree, planning_scene_.get(), _1, _2, _3, &reachable));
+                                std::bind(&isIKSolutionCollisionFree, planning_scene_.get(), std::placeholders::_1,
+                                          std::placeholders::_2, std::placeholders::_3, &reachable));
       double duration = (ros::WallTime::now() - startTime).toSec();
 
       if (success)
diff --git a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp
index 21268b5b99..91796b7215 100644
--- a/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp
+++ b/moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp
@@ -54,14 +54,16 @@ void move_group::MoveGroupPickPlaceAction::initialize()
 
   // start the pickup action server
   pickup_action_server_ = std::make_unique<actionlib::SimpleActionServer<moveit_msgs::PickupAction>>(
-      root_node_handle_, PICKUP_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, _1), false);
-  pickup_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this));
+      root_node_handle_, PICKUP_ACTION,
+      std::bind(&MoveGroupPickPlaceAction::executePickupCallback, this, std::placeholders::_1), false);
+  pickup_action_server_->registerPreemptCallback(std::bind(&MoveGroupPickPlaceAction::preemptPickupCallback, this));
   pickup_action_server_->start();
 
   // start the place action server
   place_action_server_ = std::make_unique<actionlib::SimpleActionServer<moveit_msgs::PlaceAction>>(
-      root_node_handle_, PLACE_ACTION, boost::bind(&MoveGroupPickPlaceAction::executePlaceCallback, this, _1), false);
-  place_action_server_->registerPreemptCallback(boost::bind(&MoveGroupPickPlaceAction::preemptPlaceCallback, this));
+      root_node_handle_, PLACE_ACTION,
+      std::bind(&MoveGroupPickPlaceAction::executePlaceCallback, this, std::placeholders::_1), false);
+  place_action_server_->registerPreemptCallback(std::bind(&MoveGroupPickPlaceAction::preemptPlaceCallback, this));
   place_action_server_->start();
 }
 
@@ -259,17 +261,18 @@ void move_group::MoveGroupPickPlaceAction::executePickupCallbackPlanAndExecute(
   opt.replan_ = goal->planning_options.replan;
   opt.replan_attempts_ = goal->planning_options.replan_attempts;
   opt.replan_delay_ = goal->planning_options.replan_delay;
-  opt.before_execution_callback_ = boost::bind(&MoveGroupPickPlaceAction::startPickupExecutionCallback, this);
+  opt.before_execution_callback_ = std::bind(&MoveGroupPickPlaceAction::startPickupExecutionCallback, this);
 
-  opt.plan_callback_ =
-      boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePickup, this, boost::cref(*goal), &action_res, _1);
+  opt.plan_callback_ = std::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePickup, this, boost::cref(*goal),
+                                 &action_res, std::placeholders::_1);
   if (goal->planning_options.look_around && context_->plan_with_sensing_)
   {
-    opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
-                                     _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
-                                     goal->planning_options.max_safe_execution_cost);
+    opt.plan_callback_ =
+        std::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
+                  std::placeholders::_1, opt.plan_callback_, goal->planning_options.look_around_attempts,
+                  goal->planning_options.max_safe_execution_cost);
     context_->plan_with_sensing_->setBeforeLookCallback(
-        boost::bind(&MoveGroupPickPlaceAction::startPickupLookCallback, this));
+        std::bind(&MoveGroupPickPlaceAction::startPickupLookCallback, this));
   }
 
   plan_execution::ExecutableMotionPlan plan;
@@ -290,16 +293,17 @@ void move_group::MoveGroupPickPlaceAction::executePlaceCallbackPlanAndExecute(co
   opt.replan_ = goal->planning_options.replan;
   opt.replan_attempts_ = goal->planning_options.replan_attempts;
   opt.replan_delay_ = goal->planning_options.replan_delay;
-  opt.before_execution_callback_ = boost::bind(&MoveGroupPickPlaceAction::startPlaceExecutionCallback, this);
-  opt.plan_callback_ =
-      boost::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePlace, this, boost::cref(*goal), &action_res, _1);
+  opt.before_execution_callback_ = std::bind(&MoveGroupPickPlaceAction::startPlaceExecutionCallback, this);
+  opt.plan_callback_ = std::bind(&MoveGroupPickPlaceAction::planUsingPickPlacePlace, this, boost::cref(*goal),
+                                 &action_res, std::placeholders::_1);
   if (goal->planning_options.look_around && context_->plan_with_sensing_)
   {
-    opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
-                                     _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
-                                     goal->planning_options.max_safe_execution_cost);
+    opt.plan_callback_ =
+        std::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
+                  std::placeholders::_1, opt.plan_callback_, goal->planning_options.look_around_attempts,
+                  goal->planning_options.max_safe_execution_cost);
     context_->plan_with_sensing_->setBeforeLookCallback(
-        boost::bind(&MoveGroupPickPlaceAction::startPlaceLookCallback, this));
+        std::bind(&MoveGroupPickPlaceAction::startPlaceLookCallback, this));
   }
 
   plan_execution::ExecutableMotionPlan plan;
diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp
index 4a667fd489..66b58f3119 100644
--- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp
+++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp
@@ -181,7 +181,8 @@ void addGripperTrajectory(const ManipulationPlanPtr& plan,
     plan_execution::ExecutableTrajectory et(ee_closed_traj, name);
 
     // Add a callback to attach the object to the EE after closing the gripper
-    et.effect_on_success_ = boost::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, _1);
+    et.effect_on_success_ =
+        std::bind(&executeAttachObject, plan->shared_data_, plan->approach_posture_, std::placeholders::_1);
     et.allowed_collision_matrix_ = collision_matrix;
     plan->trajectories_.push_back(et);
   }
@@ -225,8 +226,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const
 
   // state validity checking during the approach must ensure that the gripper posture is that for pre-grasping
   moveit::core::GroupStateValidityCallbackFn approach_valid_callback =
-      boost::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_,
-                  &plan->approach_posture_, _1, _2, _3);
+      std::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_,
+                &plan->approach_posture_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
   plan->goal_sampler_->setVerbose(verbose_);
   std::size_t attempted_possible_goal_states = 0;
   do  // continously sample possible goal states
@@ -276,8 +277,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const
           // state validity checking during the retreat after the grasp must ensure the gripper posture is that of the
           // actual grasp
           moveit::core::GroupStateValidityCallbackFn retreat_valid_callback =
-              boost::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_,
-                          &plan->retreat_posture_, _1, _2, _3);
+              std::bind(&isStateCollisionFree, planning_scene_after_approach.get(), collision_matrix_.get(), verbose_,
+                        &plan->retreat_posture_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
 
           // try to compute a straight line path that moves from the goal in a desired direction
           moveit::core::RobotStatePtr last_retreat_state(
diff --git a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp
index 29cab837f5..b8512d5ab6 100644
--- a/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp
+++ b/moveit_ros/manipulation/pick_place/src/manipulation_pipeline.cpp
@@ -114,7 +114,7 @@ void ManipulationPipeline::start()
     stage->resetStopSignal();
   for (std::size_t i = 0; i < processing_threads_.size(); ++i)
     if (!processing_threads_[i])
-      processing_threads_[i] = new boost::thread(boost::bind(&ManipulationPipeline::processingThread, this, i));
+      processing_threads_[i] = new boost::thread(std::bind(&ManipulationPipeline::processingThread, this, i));
 }
 
 void ManipulationPipeline::signalStop()
diff --git a/moveit_ros/manipulation/pick_place/src/pick_place.cpp b/moveit_ros/manipulation/pick_place/src/pick_place.cpp
index 6b0b2eca8e..21d4bb87e2 100644
--- a/moveit_ros/manipulation/pick_place/src/pick_place.cpp
+++ b/moveit_ros/manipulation/pick_place/src/pick_place.cpp
@@ -51,8 +51,8 @@ const double PickPlace::DEFAULT_GRASP_POSTURE_COMPLETION_DURATION = 7.0;  // sec
 PickPlacePlanBase::PickPlacePlanBase(const PickPlaceConstPtr& pick_place, const std::string& name)
   : pick_place_(pick_place), pipeline_(name, 4), last_plan_time_(0.0), done_(false)
 {
-  pipeline_.setSolutionCallback(boost::bind(&PickPlacePlanBase::foundSolution, this));
-  pipeline_.setEmptyQueueCallback(boost::bind(&PickPlacePlanBase::emptyQueue, this));
+  pipeline_.setSolutionCallback(std::bind(&PickPlacePlanBase::foundSolution, this));
+  pipeline_.setEmptyQueueCallback(std::bind(&PickPlacePlanBase::emptyQueue, this));
 }
 
 PickPlacePlanBase::~PickPlacePlanBase() = default;
diff --git a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp
index addc270d44..7102d23715 100644
--- a/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp
+++ b/moveit_ros/manipulation/pick_place/src/pick_place_params.cpp
@@ -49,8 +49,8 @@ class DynamicReconfigureImpl
 public:
   DynamicReconfigureImpl() : dynamic_reconfigure_server_(ros::NodeHandle("~/pick_place"))
   {
-    dynamic_reconfigure_server_.setCallback(
-        boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
+    dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this,
+                                                      std::placeholders::_1, std::placeholders::_2));
   }
 
   const PickPlaceParams& getParams() const
diff --git a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp
index 986c05602e..e30e58543c 100644
--- a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp
+++ b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp
@@ -37,7 +37,7 @@
 #include <moveit/pick_place/reachable_valid_pose_filter.h>
 #include <moveit/kinematic_constraints/utils.h>
 #include <tf2_eigen/tf2_eigen.h>
-#include <boost/bind.hpp>
+#include <functional>
 #include <ros/console.h>
 
 pick_place::ReachableAndValidPoseFilter::ReachableAndValidPoseFilter(
@@ -132,8 +132,9 @@ bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr
         constraints_sampler_manager_->selectSampler(planning_scene_, planning_group, plan->goal_constraints_);
     if (plan->goal_sampler_)
     {
-      plan->goal_sampler_->setGroupStateValidityCallback(boost::bind(
-          &isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, plan.get(), _1, _2, _3));
+      plan->goal_sampler_->setGroupStateValidityCallback(
+          std::bind(&isStateCollisionFree, planning_scene_.get(), collision_matrix_.get(), verbose_, plan.get(),
+                    std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
       plan->goal_sampler_->setVerbose(verbose_);
       if (plan->goal_sampler_->sample(*token_state, plan->shared_data_->max_goal_sampling_attempts_))
       {
diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp
index b281d8493d..ad8f373907 100644
--- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp
+++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp
@@ -136,10 +136,11 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath
             ls = std::make_unique<planning_scene_monitor::LockedPlanningSceneRO>(context_->planning_scene_monitor_);
             kset = std::make_unique<kinematic_constraints::KinematicConstraintSet>((*ls)->getRobotModel());
             kset->add(req.path_constraints, (*ls)->getTransforms());
-            constraint_fn = boost::bind(
+            constraint_fn = std::bind(
                 &isStateValid,
                 req.avoid_collisions ? static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get() : nullptr,
-                kset->empty() ? nullptr : kset.get(), _1, _2, _3);
+                kset->empty() ? nullptr : kset.get(), std::placeholders::_1, std::placeholders::_2,
+                std::placeholders::_3);
           }
           bool global_frame = !moveit::core::Transforms::sameFrame(link_name, req.header.frame_id);
           ROS_INFO_NAMED(getName(),
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 1003e8d0fe..cf7e900dbe 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
@@ -52,9 +52,9 @@ void MoveGroupExecuteTrajectoryAction::initialize()
   // start the move action server
   execute_action_server_ = std::make_unique<actionlib::SimpleActionServer<moveit_msgs::ExecuteTrajectoryAction>>(
       root_node_handle_, EXECUTE_ACTION_NAME,
-      boost::bind(&MoveGroupExecuteTrajectoryAction::executePathCallback, this, _1), false);
+      std::bind(&MoveGroupExecuteTrajectoryAction::executePathCallback, this, std::placeholders::_1), false);
   execute_action_server_->registerPreemptCallback(
-      boost::bind(&MoveGroupExecuteTrajectoryAction::preemptExecuteTrajectoryCallback, this));
+      std::bind(&MoveGroupExecuteTrajectoryAction::preemptExecuteTrajectoryCallback, this));
   execute_action_server_->start();
 }
 
diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp
index 9e61129856..6da4e85e9e 100644
--- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp
+++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp
@@ -60,7 +60,8 @@ void MoveGroupExecuteService::initialize()
   // Hence, we use our own asynchronous spinner listening to our own callback queue.
   ros::AdvertiseServiceOptions ops;
   ops.template init<moveit_msgs::ExecuteKnownTrajectory::Request, moveit_msgs::ExecuteKnownTrajectory::Response>(
-      EXECUTE_SERVICE_NAME, boost::bind(&MoveGroupExecuteService::executeTrajectoryService, this, _1, _2));
+      EXECUTE_SERVICE_NAME, std::bind(&MoveGroupExecuteService::executeTrajectoryService, this, std::placeholders::_1,
+                                      std::placeholders::_2));
   ops.callback_queue = &callback_queue_;
   execute_service_ = root_node_handle_.advertiseService(ops);
   spinner_.start();
diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp
index 78b03d2c31..279fc8aaa1 100644
--- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp
+++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp
@@ -158,11 +158,12 @@ bool MoveGroupKinematicsService::computeIKService(moveit_msgs::GetPositionIK::Re
     moveit::core::RobotState rs = ls->getCurrentState();
     kset.add(req.ik_request.constraints, ls->getTransforms());
     computeIK(req.ik_request, res.solution, res.error_code, rs,
-              boost::bind(&isIKSolutionValid,
-                          req.ik_request.avoid_collisions ?
-                              static_cast<const planning_scene::PlanningSceneConstPtr&>(ls).get() :
-                              nullptr,
-                          kset.empty() ? nullptr : &kset, _1, _2, _3));
+              std::bind(&isIKSolutionValid,
+                        req.ik_request.avoid_collisions ?
+                            static_cast<const planning_scene::PlanningSceneConstPtr&>(ls).get() :
+                            nullptr,
+                        kset.empty() ? nullptr : &kset, std::placeholders::_1, std::placeholders::_2,
+                        std::placeholders::_3));
   }
   else
   {
diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp
index a6793cf255..4c353cd40d 100644
--- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp
+++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp
@@ -55,8 +55,9 @@ void MoveGroupMoveAction::initialize()
 {
   // start the move action server
   move_action_server_ = std::make_unique<actionlib::SimpleActionServer<moveit_msgs::MoveGroupAction>>(
-      root_node_handle_, MOVE_ACTION, boost::bind(&MoveGroupMoveAction::executeMoveCallback, this, _1), false);
-  move_action_server_->registerPreemptCallback(boost::bind(&MoveGroupMoveAction::preemptMoveCallback, this));
+      root_node_handle_, MOVE_ACTION, std::bind(&MoveGroupMoveAction::executeMoveCallback, this, std::placeholders::_1),
+      false);
+  move_action_server_->registerPreemptCallback(std::bind(&MoveGroupMoveAction::preemptMoveCallback, this));
   move_action_server_->start();
 }
 
@@ -132,16 +133,17 @@ void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const moveit_msgs::M
   opt.replan_ = goal->planning_options.replan;
   opt.replan_attempts_ = goal->planning_options.replan_attempts;
   opt.replan_delay_ = goal->planning_options.replan_delay;
-  opt.before_execution_callback_ = boost::bind(&MoveGroupMoveAction::startMoveExecutionCallback, this);
+  opt.before_execution_callback_ = std::bind(&MoveGroupMoveAction::startMoveExecutionCallback, this);
 
-  opt.plan_callback_ =
-      boost::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this, boost::cref(motion_plan_request), _1);
+  opt.plan_callback_ = std::bind(&MoveGroupMoveAction::planUsingPlanningPipeline, this,
+                                 boost::cref(motion_plan_request), std::placeholders::_1);
   if (goal->planning_options.look_around && context_->plan_with_sensing_)
   {
-    opt.plan_callback_ = boost::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
-                                     _1, opt.plan_callback_, goal->planning_options.look_around_attempts,
-                                     goal->planning_options.max_safe_execution_cost);
-    context_->plan_with_sensing_->setBeforeLookCallback(boost::bind(&MoveGroupMoveAction::startMoveLookCallback, this));
+    opt.plan_callback_ =
+        std::bind(&plan_execution::PlanWithSensing::computePlan, context_->plan_with_sensing_.get(),
+                  std::placeholders::_1, opt.plan_callback_, goal->planning_options.look_around_attempts,
+                  goal->planning_options.max_safe_execution_cost);
+    context_->plan_with_sensing_->setBeforeLookCallback(std::bind(&MoveGroupMoveAction::startMoveLookCallback, this));
   }
 
   plan_execution::ExecutableMotionPlan plan;
diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp
index ed494f9f2d..390ab5db71 100644
--- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp
+++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp
@@ -200,14 +200,17 @@ void OccupancyMapMonitor::addUpdater(const OccupancyMapUpdaterPtr& updater)
       // when we had one updater only, we passed direcly the transform cache callback to that updater
       if (map_updaters_.size() == 2)
       {
-        map_updaters_[0]->setTransformCacheCallback(
-            boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0, _1, _2, _3));
-        map_updaters_[1]->setTransformCacheCallback(
-            boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 1, _1, _2, _3));
+        map_updaters_[0]->setTransformCacheCallback(std::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 0,
+                                                              std::placeholders::_1, std::placeholders::_2,
+                                                              std::placeholders::_3));
+        map_updaters_[1]->setTransformCacheCallback(std::bind(&OccupancyMapMonitor::getShapeTransformCache, this, 1,
+                                                              std::placeholders::_1, std::placeholders::_2,
+                                                              std::placeholders::_3));
       }
       else
-        map_updaters_.back()->setTransformCacheCallback(
-            boost::bind(&OccupancyMapMonitor::getShapeTransformCache, this, map_updaters_.size() - 1, _1, _2, _3));
+        map_updaters_.back()->setTransformCacheCallback(std::bind(&OccupancyMapMonitor::getShapeTransformCache, this,
+                                                                  map_updaters_.size() - 1, std::placeholders::_1,
+                                                                  std::placeholders::_2, std::placeholders::_3));
     }
     else
       updater->setTransformCacheCallback(transform_cache_callback_);
diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp
index b91b591d07..655d7916e3 100644
--- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp
+++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp
@@ -72,7 +72,7 @@ int main(int argc, char** argv)
   std::shared_ptr<tf2_ros::Buffer> buffer = std::make_shared<tf2_ros::Buffer>(ros::Duration(5.0));
   std::shared_ptr<tf2_ros::TransformListener> listener = std::make_shared<tf2_ros::TransformListener>(*buffer, nh);
   occupancy_map_monitor::OccupancyMapMonitor server(buffer);
-  server.setUpdateCallback(boost::bind(&publishOctomap, &octree_binary_pub, &server));
+  server.setUpdateCallback(std::bind(&publishOctomap, &octree_binary_pub, &server));
   server.startMonitor();
 
   ros::spin();
diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp
index c391989f90..3b5b907987 100644
--- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp
+++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp
@@ -129,7 +129,8 @@ bool DepthImageOctomapUpdater::initialize()
   mesh_filter_->setShadowThreshold(shadow_threshold_);
   mesh_filter_->setPaddingOffset(padding_offset_);
   mesh_filter_->setPaddingScale(padding_scale_);
-  mesh_filter_->setTransformCallback(boost::bind(&DepthImageOctomapUpdater::getShapeTransform, this, _1, _2));
+  mesh_filter_->setTransformCallback(
+      std::bind(&DepthImageOctomapUpdater::getShapeTransform, this, std::placeholders::_1, std::placeholders::_2));
 
   return true;
 }
diff --git a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp
index 7040e5fdeb..34e7e56923 100644
--- a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp
+++ b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp
@@ -48,8 +48,8 @@ LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const collision_detection::OccMapTree
   , max_sensor_delta_(1e-3)  // 1mm
   , process_occupied_cells_set_(nullptr)
   , process_model_cells_set_(nullptr)
-  , update_thread_(boost::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this))
-  , process_thread_(boost::bind(&LazyFreeSpaceUpdater::processThread, this))
+  , update_thread_(std::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this))
+  , process_thread_(std::bind(&LazyFreeSpaceUpdater::processThread, this))
 {
 }
 
diff --git a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp
index acad8248c5..5bbe882391 100644
--- a/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp
+++ b/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp
@@ -91,7 +91,7 @@ void mesh_filter::DepthSelfFiltering::onInit()
   model_label_ptr_ = std::make_shared<cv_bridge::CvImage>();
 
   mesh_filter_ = std::make_shared<MeshFilter<StereoCameraModel>>(
-      bind(&TransformProvider::getTransform, &transform_provider_, _1, _2),
+      bind(&TransformProvider::getTransform, &transform_provider_, std::placeholders::_1, std::placeholders::_2),
       mesh_filter::StereoCameraModel::REGISTERED_PSDK_PARAMS);
   mesh_filter_->parameters().setDepthRange(near_clipping_plane_distance_, far_clipping_plane_distance_);
   mesh_filter_->setShadowThreshold(shadow_threshold_);
diff --git a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp
index 7ba637ba4f..c6ba027510 100644
--- a/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp
+++ b/moveit_ros/perception/mesh_filter/test/mesh_filter_test.cpp
@@ -118,7 +118,8 @@ MeshFilterTest<Type>::MeshFilterTest(unsigned width, unsigned height, double nea
   , shadow_(shadow)
   , epsilon_(epsilon)
   , sensor_parameters_(width, height, near_, far_, width >> 1, height >> 1, width >> 1, height >> 1, 0.1, 0.1)
-  , filter_(std::bind(&MeshFilterTest<Type>::transformCallback, this, _1, _2), sensor_parameters_)
+  , filter_(std::bind(&MeshFilterTest<Type>::transformCallback, this, std::placeholders::_1, std::placeholders::_2),
+            sensor_parameters_)
   , sensor_data_(width_ * height_)
   , distance_(0.0)
 {
diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp
index fb37bbf1a0..0295ace054 100644
--- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp
+++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp
@@ -99,7 +99,8 @@ bool PointCloudOctomapUpdater::initialize()
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>();
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, root_nh_);
   shape_mask_ = std::make_unique<point_containment_filter::ShapeMask>();
-  shape_mask_->setTransformCallback(boost::bind(&PointCloudOctomapUpdater::getShapeTransform, this, _1, _2));
+  shape_mask_->setTransformCallback(
+      std::bind(&PointCloudOctomapUpdater::getShapeTransform, this, std::placeholders::_1, std::placeholders::_2));
 
   std::string prefix = "";
   if (!ns_.empty())
@@ -120,13 +121,15 @@ void PointCloudOctomapUpdater::start()
   {
     point_cloud_filter_ = new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*point_cloud_subscriber_, *tf_buffer_,
                                                                                monitor_->getMapFrame(), 5, root_nh_);
-    point_cloud_filter_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1));
+    point_cloud_filter_->registerCallback(
+        std::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, std::placeholders::_1));
     ROS_INFO_NAMED(LOGNAME, "Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(),
                    point_cloud_filter_->getTargetFramesString().c_str());
   }
   else
   {
-    point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, _1));
+    point_cloud_subscriber_->registerCallback(
+        std::bind(&PointCloudOctomapUpdater::cloudMsgCallback, this, std::placeholders::_1));
     ROS_INFO_NAMED(LOGNAME, "Listening to '%s'", point_cloud_topic_.c_str());
   }
 }
diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp
index eb01773539..2ec58d243a 100644
--- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp
+++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp
@@ -258,7 +258,7 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction()
   moveit::tools::Profiler::ScopedBlock prof_block("KinematicsPluginLoader::getLoaderFunction");
 
   if (loader_)
-    return boost::bind(&KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), _1);
+    return std::bind(&KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), std::placeholders::_1);
 
   rdf_loader::RDFLoader rml(robot_description_);
   robot_description_ = rml.getRobotDescription();
@@ -447,6 +447,7 @@ moveit::core::SolverAllocatorFn KinematicsPluginLoader::getLoaderFunction(const
                                                      iksolver_to_tip_links);
   }
 
-  return boost::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(), _1);
+  return std::bind(&KinematicsPluginLoader::KinematicsLoaderImpl::allocKinematicsSolverWithCache, loader_.get(),
+                   std::placeholders::_1);
 }
 }  // namespace kinematics_plugin_loader
diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp
index 577c5a02ba..aff65e3a98 100644
--- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp
+++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp
@@ -54,8 +54,8 @@ class PlanExecution::DynamicReconfigureImpl
   DynamicReconfigureImpl(PlanExecution* owner)
     : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/plan_execution"))
   {
-    dynamic_reconfigure_server_.setCallback(
-        boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
+    dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this,
+                                                      std::placeholders::_1, std::placeholders::_2));
   }
 
 private:
@@ -86,7 +86,8 @@ plan_execution::PlanExecution::PlanExecution(
   new_scene_update_ = false;
 
   // we want to be notified when new information is available
-  planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanExecution::planningSceneUpdatedCallback, this, _1));
+  planning_scene_monitor_->addUpdateCallback(
+      std::bind(&PlanExecution::planningSceneUpdatedCallback, this, std::placeholders::_1));
 
   // start the dynamic-reconfigure server
   reconfigure_impl_ = new DynamicReconfigureImpl(this);
@@ -404,9 +405,9 @@ moveit_msgs::MoveItErrorCodes plan_execution::PlanExecution::executeAndMonitor(E
     trajectory_monitor_->startTrajectoryMonitor();
 
   // start a trajectory execution thread
-  trajectory_execution_manager_->execute(boost::bind(&PlanExecution::doneWithTrajectoryExecution, this, _1),
-                                         boost::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan,
-                                                     _1));
+  trajectory_execution_manager_->execute(
+      std::bind(&PlanExecution::doneWithTrajectoryExecution, this, std::placeholders::_1),
+      std::bind(&PlanExecution::successfulTrajectorySegmentExecution, this, &plan, std::placeholders::_1));
   // wait for path to be done, while checking that the path does not become invalid
   ros::Rate r(100);
   path_became_invalid_ = false;
diff --git a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp
index 9eb12255de..3b1cca9e7d 100644
--- a/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp
+++ b/moveit_ros/planning/plan_execution/src/plan_with_sensing.cpp
@@ -52,8 +52,8 @@ class PlanWithSensing::DynamicReconfigureImpl
   DynamicReconfigureImpl(PlanWithSensing* owner)
     : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/sense_for_plan"))
   {
-    dynamic_reconfigure_server_.setCallback(
-        boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
+    dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this,
+                                                      std::placeholders::_1, std::placeholders::_2));
   }
 
 private:
diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp
index b8530d349e..fae2970a20 100644
--- a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp
+++ b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp
@@ -118,7 +118,7 @@ int main(int argc, char** argv)
 
     for (unsigned int i = 0; i < states.size(); ++i)
       threads.push_back(new boost::thread(
-          boost::bind(&runCollisionDetection, i, trials, psm.getPlanningScene().get(), states[i].get())));
+          std::bind(&runCollisionDetection, i, trials, psm.getPlanningScene().get(), states[i].get())));
 
     for (unsigned int i = 0; i < states.size(); ++i)
     {
diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp
index 415906fef2..125dc1b983 100644
--- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp
+++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp
@@ -148,7 +148,7 @@ void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topi
     if (tf_buffer_ && !robot_model_->getMultiDOFJointModels().empty())
     {
       tf_connection_ = std::make_shared<TFConnection>(
-          tf_buffer_->_addTransformsChangedListener(boost::bind(&CurrentStateMonitor::tfCallback, this)));
+          tf_buffer_->_addTransformsChangedListener(std::bind(&CurrentStateMonitor::tfCallback, this)));
     }
     state_monitor_started_ = true;
     monitor_start_time_ = ros::Time::now();
diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
index f1edc7be71..773da3da8a 100644
--- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
+++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
@@ -62,8 +62,8 @@ class PlanningSceneMonitor::DynamicReconfigureImpl
   DynamicReconfigureImpl(PlanningSceneMonitor* owner)
     : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle(decideNamespace(owner->getName())))
   {
-    dynamic_reconfigure_server_.setCallback(
-        boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
+    dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this,
+                                                      std::placeholders::_1, std::placeholders::_2));
   }
 
 private:
@@ -223,10 +223,10 @@ void PlanningSceneMonitor::initialize(const planning_scene::PlanningScenePtr& sc
     {
       // The scene_ is loaded on the collision loader only if it was correctly instantiated
       collision_loader_.setupScene(nh_, scene_);
-      scene_->setAttachedBodyUpdateCallback(
-          boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
-      scene_->setCollisionObjectUpdateCallback(
-          boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
+      scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback,
+                                                      this, std::placeholders::_1, std::placeholders::_2));
+      scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this,
+                                                         std::placeholders::_1, std::placeholders::_2));
     }
   }
   else
@@ -271,10 +271,10 @@ void PlanningSceneMonitor::monitorDiffs(bool flag)
         parent_scene_ = scene_;
         scene_ = parent_scene_->diff();
         scene_const_ = scene_;
-        scene_->setAttachedBodyUpdateCallback(
-            boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
-        scene_->setCollisionObjectUpdateCallback(
-            boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
+        scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback,
+                                                        this, std::placeholders::_1, std::placeholders::_2));
+        scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback,
+                                                           this, std::placeholders::_1, std::placeholders::_2));
       }
     }
     else
@@ -327,7 +327,7 @@ void PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_t
     ROS_INFO_NAMED(LOGNAME, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
     monitorDiffs(true);
     publish_planning_scene_ =
-        std::make_unique<boost::thread>(boost::bind(&PlanningSceneMonitor::scenePublishingThread, this));
+        std::make_unique<boost::thread>(std::bind(&PlanningSceneMonitor::scenePublishingThread, this));
   }
 }
 
@@ -385,10 +385,10 @@ void PlanningSceneMonitor::scenePublishingThread()
           scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn());
           scene_->pushDiffs(parent_scene_);
           scene_->clearDiffs();
-          scene_->setAttachedBodyUpdateCallback(
-              boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
-          scene_->setCollisionObjectUpdateCallback(
-              boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
+          scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback,
+                                                          this, std::placeholders::_1, std::placeholders::_2));
+          scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback,
+                                                             this, std::placeholders::_1, std::placeholders::_2));
           if (octomap_monitor_)
           {
             excludeAttachedBodiesFromOctree();  // in case updates have happened to the attached bodies, put them in
@@ -593,10 +593,10 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningSc
       parent_scene_ = scene_;
       scene_ = parent_scene_->diff();
       scene_const_ = scene_;
-      scene_->setAttachedBodyUpdateCallback(
-          boost::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback, this, _1, _2));
-      scene_->setCollisionObjectUpdateCallback(
-          boost::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this, _1, _2));
+      scene_->setAttachedBodyUpdateCallback(std::bind(&PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback,
+                                                      this, std::placeholders::_1, std::placeholders::_2));
+      scene_->setCollisionObjectUpdateCallback(std::bind(&PlanningSceneMonitor::currentWorldObjectUpdateCallback, this,
+                                                         std::placeholders::_1, std::placeholders::_2));
     }
     if (octomap_monitor_)
     {
@@ -1097,9 +1097,10 @@ void PlanningSceneMonitor::startWorldGeometryMonitor(const std::string& collisio
       excludeAttachedBodiesFromOctree();
       excludeWorldObjectsFromOctree();
 
-      octomap_monitor_->setTransformCacheCallback(
-          boost::bind(&PlanningSceneMonitor::getShapeTransformCache, this, _1, _2, _3));
-      octomap_monitor_->setUpdateCallback(boost::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
+      octomap_monitor_->setTransformCacheCallback(std::bind(&PlanningSceneMonitor::getShapeTransformCache, this,
+                                                            std::placeholders::_1, std::placeholders::_2,
+                                                            std::placeholders::_3));
+      octomap_monitor_->setUpdateCallback(std::bind(&PlanningSceneMonitor::octomapUpdateCallback, this));
     }
     octomap_monitor_->startMonitor();
   }
@@ -1129,7 +1130,8 @@ void PlanningSceneMonitor::startStateMonitor(const std::string& joint_states_top
   {
     if (!current_state_monitor_)
       current_state_monitor_ = std::make_shared<CurrentStateMonitor>(getRobotModel(), tf_buffer_, root_nh_);
-    current_state_monitor_->addUpdateCallback(boost::bind(&PlanningSceneMonitor::onStateUpdate, this, _1));
+    current_state_monitor_->addUpdateCallback(
+        std::bind(&PlanningSceneMonitor::onStateUpdate, this, std::placeholders::_1));
     current_state_monitor_->startStateMonitor(joint_states_topic);
 
     {
diff --git a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp
index a6cea18d91..9449d1a573 100644
--- a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp
+++ b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp
@@ -77,7 +77,7 @@ void planning_scene_monitor::TrajectoryMonitor::startTrajectoryMonitor()
 {
   if (sampling_frequency_ > std::numeric_limits<double>::epsilon() && !record_states_thread_)
   {
-    record_states_thread_ = std::make_unique<boost::thread>(boost::bind(&TrajectoryMonitor::recordStates, this));
+    record_states_thread_ = std::make_unique<boost::thread>(std::bind(&TrajectoryMonitor::recordStates, this));
     ROS_DEBUG_NAMED(LOGNAME, "Started trajectory monitor");
   }
 }
diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
index e5ef47bb5a..abe969aa11 100644
--- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
+++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
@@ -60,8 +60,8 @@ class TrajectoryExecutionManager::DynamicReconfigureImpl
   DynamicReconfigureImpl(TrajectoryExecutionManager* owner)
     : owner_(owner), dynamic_reconfigure_server_(ros::NodeHandle("~/trajectory_execution"))
   {
-    dynamic_reconfigure_server_.setCallback(
-        boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this, _1, _2));
+    dynamic_reconfigure_server_.setCallback(std::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback, this,
+                                                      std::placeholders::_1, std::placeholders::_2));
   }
 
 private:
@@ -360,7 +360,7 @@ bool TrajectoryExecutionManager::pushAndExecute(const moveit_msgs::RobotTrajecto
       continuous_execution_queue_.push_back(context);
       if (!continuous_execution_thread_)
         continuous_execution_thread_ =
-            std::make_unique<boost::thread>(boost::bind(&TrajectoryExecutionManager::continuousExecutionThread, this));
+            std::make_unique<boost::thread>(std::bind(&TrajectoryExecutionManager::continuousExecutionThread, this));
     }
     last_execution_status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
     continuous_execution_condition_.notify_all();
diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
index c1ceab384b..81db35e044 100644
--- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
+++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
@@ -1247,7 +1247,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     if (constraints_init_thread_)
       constraints_init_thread_->join();
     constraints_init_thread_ = std::make_unique<boost::thread>(
-        boost::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port));
+        std::bind(&MoveGroupInterfaceImpl::initializeConstraintsStorageThread, this, host, port));
   }
 
   void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz)
diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp
index e26ccdc182..18868cb7c6 100644
--- a/moveit_ros/robot_interaction/src/interaction_handler.cpp
+++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp
@@ -207,7 +207,7 @@ void InteractionHandler::handleGeneric(const GenericInteraction& g,
     StateChangeCallbackFn callback;
     // modify the RobotState in-place with the state_lock_ held.
     LockedRobotState::modifyState(
-        boost::bind(&InteractionHandler::updateStateGeneric, this, _1, &g, &feedback, &callback));
+        std::bind(&InteractionHandler::updateStateGeneric, this, std::placeholders::_1, &g, &feedback, &callback));
 
     // This calls update_callback_ to notify client that state changed.
     if (callback)
@@ -238,8 +238,8 @@ void InteractionHandler::handleEndEffector(const EndEffectorInteraction& eef,
 
   // modify the RobotState in-place with state_lock_ held.
   // This locks state_lock_ before calling updateState()
-  LockedRobotState::modifyState(
-      boost::bind(&InteractionHandler::updateStateEndEffector, this, _1, &eef, &tpose.pose, &callback));
+  LockedRobotState::modifyState(std::bind(&InteractionHandler::updateStateEndEffector, this, std::placeholders::_1,
+                                          &eef, &tpose.pose, &callback));
 
   // This calls update_callback_ to notify client that state changed.
   if (callback)
@@ -270,7 +270,7 @@ void InteractionHandler::handleJoint(const JointInteraction& vj,
   // modify the RobotState in-place with state_lock_ held.
   // This locks state_lock_ before calling updateState()
   LockedRobotState::modifyState(
-      boost::bind(&InteractionHandler::updateStateJoint, this, _1, &vj, &tpose.pose, &callback));
+      std::bind(&InteractionHandler::updateStateJoint, this, std::placeholders::_1, &vj, &tpose.pose, &callback));
 
   // This calls update_callback_ to notify client that state changed.
   if (callback)
@@ -285,7 +285,7 @@ void InteractionHandler::updateStateGeneric(moveit::core::RobotState* state, con
   bool ok = g->process_feedback(*state, *feedback);
   bool error_state_changed = setErrorState(g->marker_name_suffix, !ok);
   if (update_callback_)
-    *callback = boost::bind(update_callback_, _1, error_state_changed);
+    *callback = std::bind(update_callback_, std::placeholders::_1, error_state_changed);
 }
 
 // MUST hold state_lock_ when calling this!
@@ -299,7 +299,7 @@ void InteractionHandler::updateStateEndEffector(moveit::core::RobotState* state,
   bool ok = kinematic_options.setStateFromIK(*state, eef->parent_group, eef->parent_link, *pose);
   bool error_state_changed = setErrorState(eef->parent_group, !ok);
   if (update_callback_)
-    *callback = boost::bind(update_callback_, _1, error_state_changed);
+    *callback = std::bind(update_callback_, std::placeholders::_1, error_state_changed);
 }
 
 // MUST hold state_lock_ when calling this!
@@ -316,7 +316,7 @@ void InteractionHandler::updateStateJoint(moveit::core::RobotState* state, const
   state->update();
 
   if (update_callback_)
-    *callback = boost::bind(update_callback_, _1, false);
+    *callback = std::bind(update_callback_, std::placeholders::_1, false);
 }
 
 bool InteractionHandler::inError(const EndEffectorInteraction& eef) const
diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp
index 2e555b5aec..d4128f0dc2 100644
--- a/moveit_ros/robot_interaction/src/robot_interaction.cpp
+++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp
@@ -70,7 +70,7 @@ RobotInteraction::RobotInteraction(const moveit::core::RobotModelConstPtr& robot
 
   // spin a thread that will process feedback events
   run_processing_thread_ = true;
-  processing_thread_ = std::make_unique<boost::thread>(boost::bind(&RobotInteraction::processingThread, this));
+  processing_thread_ = std::make_unique<boost::thread>(std::bind(&RobotInteraction::processingThread, this));
 }
 
 RobotInteraction::~RobotInteraction()
@@ -539,7 +539,8 @@ void RobotInteraction::addInteractiveMarkers(const InteractionHandlerPtr& handle
   for (const visualization_msgs::InteractiveMarker& im : ims)
   {
     int_marker_server_->insert(im);
-    int_marker_server_->setCallback(im.name, boost::bind(&RobotInteraction::processInteractiveMarkerFeedback, this, _1));
+    int_marker_server_->setCallback(im.name, std::bind(&RobotInteraction::processInteractiveMarkerFeedback, this,
+                                                       std::placeholders::_1));
 
     // Add menu handler to all markers that this interaction handler creates.
     if (std::shared_ptr<interactive_markers::MenuHandler> mh = handler->getMenuHandler())
@@ -569,7 +570,8 @@ void RobotInteraction::toggleMoveInteractiveMarkerTopic(bool enable)
         std::string topic_name = int_marker_move_topics_[i];
         std::string marker_name = int_marker_names_[i];
         int_marker_move_subscribers_.push_back(nh.subscribe<geometry_msgs::PoseStamped>(
-            topic_name, 1, boost::bind(&RobotInteraction::moveInteractiveMarker, this, marker_name, _1)));
+            topic_name, 1,
+            std::bind(&RobotInteraction::moveInteractiveMarker, this, marker_name, std::placeholders::_1)));
       }
     }
   }
diff --git a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp
index 8ff126b8c4..3e5b155768 100644
--- a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp
+++ b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp
@@ -433,7 +433,7 @@ void MyInfo::modifyThreadFunc(robot_interaction::LockedRobotState* locked_state,
     {
       val += 0.0001;
 
-      locked_state->modifyState(boost::bind(&MyInfo::modifyFunc, this, _1, val));
+      locked_state->modifyState(std::bind(&MyInfo::modifyFunc, this, std::placeholders::_1, val));
     }
 
     cnt_lock_.lock();
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
index b68d565d50..aa343aead9 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
@@ -174,7 +174,8 @@ MotionPlanningDisplay::MotionPlanningDisplay()
   trajectory_visual_ = std::make_shared<TrajectoryVisualization>(path_category_, this);
 
   // Start background jobs
-  background_process_.setJobUpdateEvent(boost::bind(&MotionPlanningDisplay::backgroundJobUpdate, this, _1, _2));
+  background_process_.setJobUpdateEvent(
+      std::bind(&MotionPlanningDisplay::backgroundJobUpdate, this, std::placeholders::_1, std::placeholders::_2));
 }
 
 // ******************************************************************************************
@@ -285,7 +286,7 @@ void MotionPlanningDisplay::toggleSelectPlanningGroupSubscription(bool enable)
 void MotionPlanningDisplay::selectPlanningGroupCallback(const std_msgs::StringConstPtr& msg)
 {
   // synchronize ROS callback with main loop
-  addMainLoopJob(boost::bind(&MotionPlanningDisplay::changePlanningGroup, this, msg->data));
+  addMainLoopJob(std::bind(&MotionPlanningDisplay::changePlanningGroup, this, msg->data));
 }
 
 void MotionPlanningDisplay::reset()
@@ -324,7 +325,7 @@ void MotionPlanningDisplay::setName(const QString& name)
 void MotionPlanningDisplay::backgroundJobUpdate(moveit::tools::BackgroundProcessing::JobEvent /*unused*/,
                                                 const std::string& /*unused*/)
 {
-  addMainLoopJob(boost::bind(&MotionPlanningDisplay::updateBackgroundJobProgressBar, this));
+  addMainLoopJob(std::bind(&MotionPlanningDisplay::updateBackgroundJobProgressBar, this));
 }
 
 void MotionPlanningDisplay::updateBackgroundJobProgressBar()
@@ -722,7 +723,7 @@ void MotionPlanningDisplay::changedQueryStartState()
   setStatusTextColor(query_start_color_property_->getColor());
   addStatusText("Changed start state");
   drawQueryStartState();
-  addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true),
+  addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true),
                    "publishInteractiveMarkers");
 }
 
@@ -733,7 +734,7 @@ void MotionPlanningDisplay::changedQueryGoalState()
   setStatusTextColor(query_goal_color_property_->getColor());
   addStatusText("Changed goal state");
   drawQueryGoalState();
-  addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true),
+  addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, true),
                    "publishInteractiveMarkers");
 }
 
@@ -806,7 +807,7 @@ void MotionPlanningDisplay::resetInteractiveMarkers()
 {
   query_start_state_->clearError();
   query_goal_state_->clearError();
-  addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false),
+  addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false),
                    "publishInteractiveMarkers");
 }
 
@@ -912,7 +913,7 @@ void MotionPlanningDisplay::scheduleDrawQueryStartState(robot_interaction::Inter
 {
   if (!planning_scene_monitor_)
     return;
-  addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed),
+  addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed),
                    "publishInteractiveMarkers");
   updateQueryStartState();
 }
@@ -922,7 +923,7 @@ void MotionPlanningDisplay::scheduleDrawQueryGoalState(robot_interaction::Intera
 {
   if (!planning_scene_monitor_)
     return;
-  addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed),
+  addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, !error_state_changed),
                    "publishInteractiveMarkers");
   updateQueryGoalState();
 }
@@ -931,7 +932,7 @@ void MotionPlanningDisplay::updateQueryStartState()
 {
   queryStartStateChanged();
   recomputeQueryStartStateMetrics();
-  addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedQueryStartState, this));
+  addMainLoopJob(std::bind(&MotionPlanningDisplay::changedQueryStartState, this));
   context_->queueRender();
 }
 
@@ -939,7 +940,7 @@ void MotionPlanningDisplay::updateQueryGoalState()
 {
   queryGoalStateChanged();
   recomputeQueryGoalStateMetrics();
-  addMainLoopJob(boost::bind(&MotionPlanningDisplay::changedQueryGoalState, this));
+  addMainLoopJob(std::bind(&MotionPlanningDisplay::changedQueryGoalState, this));
   context_->queueRender();
 }
 
@@ -1049,7 +1050,7 @@ void MotionPlanningDisplay::changedPlanningGroup()
 
   if (frame_)
     frame_->changePlanningGroup();
-  addBackgroundJob(boost::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false),
+  addBackgroundJob(std::bind(&MotionPlanningDisplay::publishInteractiveMarkers, this, false),
                    "publishInteractiveMarkers");
 }
 
@@ -1119,7 +1120,7 @@ void MotionPlanningDisplay::populateMenuHandler(std::shared_ptr<interactive_mark
     if ((state_name == "same as start" && is_start) || (state_name == "same as goal" && !is_start))
       continue;
     mh->insert(menu_states, state_name,
-               boost::bind(&MotionPlanningDisplay::setQueryStateHelper, this, is_start, state_name));
+               std::bind(&MotionPlanningDisplay::setQueryStateHelper, this, is_start, state_name));
   }
 
   //  // Group commands, which end up being the same for both interaction handlers
@@ -1127,7 +1128,7 @@ void MotionPlanningDisplay::populateMenuHandler(std::shared_ptr<interactive_mark
   //  immh::EntryHandle menu_groups = mh->insert("Planning Group", immh::FeedbackCallback());
   //  for (int i = 0; i < group_names.size(); ++i)
   //    mh->insert(menu_groups, group_names[i],
-  //               boost::bind(&MotionPlanningDisplay::changePlanningGroup, this, group_names[i]));
+  //               std::bind(&MotionPlanningDisplay::changePlanningGroup, this, group_names[i]));
 }
 
 void MotionPlanningDisplay::onRobotModelLoaded()
@@ -1138,7 +1139,8 @@ void MotionPlanningDisplay::onRobotModelLoaded()
   robot_interaction_ =
       std::make_shared<robot_interaction::RobotInteraction>(getRobotModel(), "rviz_moveit_motion_planning_display");
   robot_interaction::KinematicOptions o;
-  o.state_validity_callback_ = boost::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, _1, _2, _3);
+  o.state_validity_callback_ = std::bind(&MotionPlanningDisplay::isIKSolutionCollisionFree, this, std::placeholders::_1,
+                                         std::placeholders::_2, std::placeholders::_3);
   robot_interaction_->getKinematicOptionsMap()->setOptions(
       robot_interaction::KinematicOptionsMap::ALL, o, robot_interaction::KinematicOptions::STATE_VALIDITY_CALLBACK);
 
@@ -1153,8 +1155,10 @@ void MotionPlanningDisplay::onRobotModelLoaded()
       robot_interaction_, "start", *previous_state_, planning_scene_monitor_->getTFClient());
   query_goal_state_ = std::make_shared<robot_interaction::InteractionHandler>(
       robot_interaction_, "goal", *previous_state_, planning_scene_monitor_->getTFClient());
-  query_start_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this, _1, _2));
-  query_goal_state_->setUpdateCallback(boost::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this, _1, _2));
+  query_start_state_->setUpdateCallback(std::bind(&MotionPlanningDisplay::scheduleDrawQueryStartState, this,
+                                                  std::placeholders::_1, std::placeholders::_2));
+  query_goal_state_->setUpdateCallback(std::bind(&MotionPlanningDisplay::scheduleDrawQueryGoalState, this,
+                                                 std::placeholders::_1, std::placeholders::_2));
 
   // Interactive marker menus
   populateMenuHandler(menu_handler_start_);
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp
index 8c627d9345..5775ea432f 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp
@@ -34,6 +34,8 @@
 
 /* Author: Ioan Sucan */
 
+#include <functional>
+
 #include <moveit/common_planning_interface_objects/common_objects.h>
 #include <moveit/motion_planning_rviz_plugin/motion_planning_frame.h>
 #include <moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h>
@@ -211,7 +213,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz::
       semantic_world_.reset();
     if (semantic_world_)
     {
-      semantic_world_->addTableCallback(boost::bind(&MotionPlanningFrame::updateTables, this));
+      semantic_world_->addTableCallback(std::bind(&MotionPlanningFrame::updateTables, this));
     }
   }
   catch (std::exception& ex)
@@ -334,14 +336,13 @@ void MotionPlanningFrame::changePlanningGroupHelper()
   if (!planning_display_->getPlanningSceneMonitor())
     return;
 
-  planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::fillStateSelectionOptions, this));
-  planning_display_->addMainLoopJob(
-      boost::bind(&MotionPlanningFrame::populateConstraintsList, this, std::vector<std::string>()));
+  planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::fillStateSelectionOptions, this));
+  planning_display_->addMainLoopJob([this]() { populateConstraintsList(std::vector<std::string>()); });
 
   const moveit::core::RobotModelConstPtr& robot_model = planning_display_->getRobotModel();
   std::string group = planning_display_->getCurrentPlanningGroup();
   planning_display_->addMainLoopJob(
-      boost::bind(&MotionPlanningParamWidget::setGroupName, ui_->planner_param_treeview, group));
+      std::bind(&MotionPlanningParamWidget::setGroupName, ui_->planner_param_treeview, group));
   planning_display_->addMainLoopJob(
       [=]() { ui_->planning_group_combo_box->setCurrentText(QString::fromStdString(group)); });
 
@@ -373,7 +374,7 @@ void MotionPlanningFrame::changePlanningGroupHelper()
       ROS_ERROR("%s", ex.what());
     }
     planning_display_->addMainLoopJob(
-        boost::bind(&MotionPlanningParamWidget::setMoveGroup, ui_->planner_param_treeview, move_group_));
+        std::bind(&MotionPlanningParamWidget::setMoveGroup, ui_->planner_param_treeview, move_group_));
     if (move_group_)
     {
       move_group_->allowLooking(ui_->allow_looking->isChecked());
@@ -382,9 +383,8 @@ void MotionPlanningFrame::changePlanningGroupHelper()
       planning_display_->addMainLoopJob([=]() { ui_->use_cartesian_path->setEnabled(has_unique_endeffector); });
       std::vector<moveit_msgs::PlannerInterfaceDescription> desc;
       if (move_group_->getInterfaceDescriptions(desc))
-        planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlannersList, this, desc));
-      planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::populateConstraintsList, this),
-                                          "populateConstraintsList");
+        planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlannersList, this, desc));
+      planning_display_->addBackgroundJob([this]() { populateConstraintsList(); }, "populateConstraintsList");
 
       if (first_time_)
       {
@@ -399,7 +399,7 @@ void MotionPlanningFrame::changePlanningGroupHelper()
         planning_display_->useApproximateIK(ui_->approximate_ik->isChecked());
         if (ui_->allow_external_program->isChecked())
           planning_display_->addMainLoopJob(
-              boost::bind(&MotionPlanningFrame::allowExternalProgramCommunication, this, true));
+              std::bind(&MotionPlanningFrame::allowExternalProgramCommunication, this, true));
       }
     }
   }
@@ -407,7 +407,7 @@ void MotionPlanningFrame::changePlanningGroupHelper()
 
 void MotionPlanningFrame::changePlanningGroup()
 {
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::changePlanningGroupHelper, this),
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::changePlanningGroupHelper, this),
                                       "Frame::changePlanningGroup");
   joints_tab_->changePlanningGroup(planning_display_->getCurrentPlanningGroup(),
                                    planning_display_->getQueryStartStateHandler(),
@@ -417,7 +417,7 @@ void MotionPlanningFrame::changePlanningGroup()
 void MotionPlanningFrame::sceneUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type)
 {
   if (update_type & planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY)
-    planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
+    planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
 }
 
 void MotionPlanningFrame::addSceneObject()
@@ -494,11 +494,11 @@ void MotionPlanningFrame::addSceneObject()
   }
   setLocalSceneEdited();
 
-  planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
+  planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
 
   // Automatically select the inserted object so that its IM is displayed
   planning_display_->addMainLoopJob(
-      boost::bind(&MotionPlanningFrame::setItemSelectionInList, this, shape_name, true, ui_->collision_objects_list));
+      std::bind(&MotionPlanningFrame::setItemSelectionInList, this, shape_name, true, ui_->collision_objects_list));
 
   planning_display_->queueRenderSceneGeometry();
 }
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp
index 358c8452da..f429794f02 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_context.cpp
@@ -53,7 +53,7 @@ namespace moveit_rviz_plugin
 {
 void MotionPlanningFrame::databaseConnectButtonClicked()
 {
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClicked, this),
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClicked, this),
                                       "connect to database");
 }
 
@@ -108,7 +108,7 @@ void MotionPlanningFrame::resetDbButtonClicked()
     return;
 
   planning_display_->addBackgroundJob(
-      boost::bind(&MotionPlanningFrame::computeResetDbButtonClicked, this, response.toStdString()), "reset database");
+      std::bind(&MotionPlanningFrame::computeResetDbButtonClicked, this, response.toStdString()), "reset database");
 }
 
 void MotionPlanningFrame::computeDatabaseConnectButtonClicked()
@@ -119,12 +119,12 @@ void MotionPlanningFrame::computeDatabaseConnectButtonClicked()
     robot_state_storage_.reset();
     constraints_storage_.reset();
     planning_display_->addMainLoopJob(
-        boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 1));
+        std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 1));
   }
   else
   {
     planning_display_->addMainLoopJob(
-        boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 2));
+        std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 2));
     try
     {
       warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase();
@@ -138,19 +138,19 @@ void MotionPlanningFrame::computeDatabaseConnectButtonClicked()
       else
       {
         planning_display_->addMainLoopJob(
-            boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3));
+            std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3));
         return;
       }
     }
     catch (std::exception& ex)
     {
       planning_display_->addMainLoopJob(
-          boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3));
+          std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 3));
       ROS_ERROR("%s", ex.what());
       return;
     }
     planning_display_->addMainLoopJob(
-        boost::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 4));
+        std::bind(&MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper, this, 4));
   }
 }
 
@@ -204,8 +204,7 @@ void MotionPlanningFrame::computeDatabaseConnectButtonClickedHelper(int mode)
     if (move_group_)
     {
       move_group_->setConstraintsDatabase(ui_->database_host->text().toStdString(), ui_->database_port->value());
-      planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::populateConstraintsList, this),
-                                          "populateConstraintsList");
+      planning_display_->addBackgroundJob([this]() { populateConstraintsList(); }, "populateConstraintsList");
     }
   }
 }
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp
index 76e4c0d6da..80ec560597 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_manipulation.cpp
@@ -59,11 +59,10 @@ void MotionPlanningFrame::detectObjectsButtonClicked()
     }
     if (semantic_world_)
     {
-      semantic_world_->addTableCallback(boost::bind(&MotionPlanningFrame::updateTables, this));
+      semantic_world_->addTableCallback(std::bind(&MotionPlanningFrame::updateTables, this));
     }
   }
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::triggerObjectDetection, this),
-                                      "detect objects");
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::triggerObjectDetection, this), "detect objects");
 }
 
 void MotionPlanningFrame::processDetectedObjects()
@@ -167,7 +166,7 @@ void MotionPlanningFrame::triggerObjectDetection()
 void MotionPlanningFrame::listenDetectedObjects(const object_recognition_msgs::RecognizedObjectArrayPtr& /*msg*/)
 {
   ros::Duration(1.0).sleep();
-  planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::processDetectedObjects, this));
+  planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::processDetectedObjects, this));
 }
 
 void MotionPlanningFrame::updateDetectedObjectsList(const std::vector<std::string>& object_ids)
@@ -197,13 +196,13 @@ void MotionPlanningFrame::updateDetectedObjectsList(const std::vector<std::strin
 void MotionPlanningFrame::updateTables()
 {
   ROS_DEBUG("Update table callback");
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::publishTables, this), "publish tables");
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::publishTables, this), "publish tables");
 }
 
 void MotionPlanningFrame::publishTables()
 {
   semantic_world_->addTablesToCollisionWorld();
-  planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::updateSupportSurfacesList, this));
+  planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::updateSupportSurfacesList, this));
 }
 
 void MotionPlanningFrame::selectedSupportSurfaceChanged()
@@ -296,7 +295,7 @@ void MotionPlanningFrame::pickObjectButtonClicked()
   }
   ROS_INFO("Trying to pick up object %s from support surface %s", pick_object_name_[group_name].c_str(),
            support_surface_name_.c_str());
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::pickObject, this), "pick");
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::pickObject, this), "pick");
 }
 
 void MotionPlanningFrame::placeObjectButtonClicked()
@@ -342,7 +341,7 @@ void MotionPlanningFrame::placeObjectButtonClicked()
                                                      upright_orientation, 0.1);
   planning_display_->visualizePlaceLocations(place_poses_);
   place_object_name_ = attached_bodies[0]->getName();
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::placeObject, this), "place");
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::placeObject, this), "place");
 }
 
 void MotionPlanningFrame::pickObject()
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp
index a196132170..d3a0e706dc 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp
@@ -146,7 +146,7 @@ void MotionPlanningFrame::clearScene()
     ps->getPlanningSceneMsg(msg);
     planning_scene_publisher_.publish(msg);
     setLocalSceneEdited(false);
-    planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
+    planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
     planning_display_->queueRenderSceneGeometry();
   }
 }
@@ -228,7 +228,7 @@ void MotionPlanningFrame::removeSceneObject()
         ps->getCurrentStateNonConst().clearAttachedBody(sel[i]->text().toStdString());
     scene_marker_.reset();
     setLocalSceneEdited();
-    planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
+    planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
     planning_display_->queueRenderSceneGeometry();
   }
 }
@@ -498,7 +498,7 @@ void MotionPlanningFrame::copySelectedCollisionObject()
     ROS_DEBUG("Copied collision object to '%s'", name.c_str());
   }
   setLocalSceneEdited();
-  planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
+  planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
 }
 
 void MotionPlanningFrame::computeSaveSceneButtonClicked()
@@ -517,7 +517,7 @@ void MotionPlanningFrame::computeSaveSceneButtonClicked()
       ROS_ERROR("%s", ex.what());
     }
 
-    planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
+    planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
   }
 }
 
@@ -538,7 +538,7 @@ void MotionPlanningFrame::computeSaveQueryButtonClicked(const std::string& scene
       ROS_ERROR("%s", ex.what());
     }
 
-    planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
+    planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
   }
 }
 
@@ -575,7 +575,7 @@ void MotionPlanningFrame::computeDeleteSceneButtonClicked()
           ROS_ERROR("%s", ex.what());
         }
       }
-      planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
+      planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
     }
   }
 }
@@ -601,7 +601,7 @@ void MotionPlanningFrame::computeDeleteQueryButtonClicked()
           ROS_ERROR("%s", ex.what());
         }
         planning_display_->addMainLoopJob(
-            boost::bind(&MotionPlanningFrame::computeDeleteQueryButtonClickedHelper, this, s));
+            std::bind(&MotionPlanningFrame::computeDeleteQueryButtonClickedHelper, this, s));
       }
     }
   }
@@ -847,7 +847,7 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item)
       if (scene_marker_)
       {
         scene_marker_.reset();
-        planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::createSceneInteractiveMarker, this));
+        planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::createSceneInteractiveMarker, this));
       }
     }
   }
@@ -998,7 +998,7 @@ void MotionPlanningFrame::exportGeometryAsTextButtonClicked()
       QFileDialog::getSaveFileName(this, tr("Export Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)"));
   if (!path.isEmpty())
     planning_display_->addBackgroundJob(
-        boost::bind(&MotionPlanningFrame::computeExportGeometryAsText, this, path.toStdString()), "export as text");
+        std::bind(&MotionPlanningFrame::computeExportGeometryAsText, this, path.toStdString()), "export as text");
 }
 
 void MotionPlanningFrame::computeExportGeometryAsText(const std::string& path)
@@ -1028,7 +1028,7 @@ void MotionPlanningFrame::computeImportGeometryFromText(const std::string& path)
     if (ps->loadGeometryFromStream(fin))
     {
       ROS_INFO("Loaded scene geometry from '%s'", path.c_str());
-      planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
+      planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populateCollisionObjectsList, this));
       planning_display_->queueRenderSceneGeometry();
       setLocalSceneEdited();
     }
@@ -1047,6 +1047,6 @@ void MotionPlanningFrame::importGeometryFromTextButtonClicked()
       QFileDialog::getOpenFileName(this, tr("Import Scene Geometry"), tr(""), tr("Scene Geometry (*.scene)"));
   if (!path.isEmpty())
     planning_display_->addBackgroundJob(
-        boost::bind(&MotionPlanningFrame::computeImportGeometryFromText, this, path.toStdString()), "import from text");
+        std::bind(&MotionPlanningFrame::computeImportGeometryFromText, this, path.toStdString()), "import from text");
 }
 }  // namespace moveit_rviz_plugin
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
index f5d116cbf2..c8767bef3a 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
@@ -53,15 +53,14 @@ namespace moveit_rviz_plugin
 void MotionPlanningFrame::planButtonClicked()
 {
   publishSceneIfNeeded();
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanButtonClicked, this),
-                                      "compute plan");
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computePlanButtonClicked, this), "compute plan");
 }
 
 void MotionPlanningFrame::executeButtonClicked()
 {
   ui_->execute_button->setEnabled(false);
   // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution
-  planning_display_->spawnBackgroundJob(boost::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this));
+  planning_display_->spawnBackgroundJob(std::bind(&MotionPlanningFrame::computeExecuteButtonClicked, this));
 }
 
 void MotionPlanningFrame::planAndExecuteButtonClicked()
@@ -70,13 +69,13 @@ void MotionPlanningFrame::planAndExecuteButtonClicked()
   ui_->plan_and_execute_button->setEnabled(false);
   ui_->execute_button->setEnabled(false);
   // execution is done in a separate thread, to not block other background jobs by blocking for synchronous execution
-  planning_display_->spawnBackgroundJob(boost::bind(&MotionPlanningFrame::computePlanAndExecuteButtonClicked, this));
+  planning_display_->spawnBackgroundJob(std::bind(&MotionPlanningFrame::computePlanAndExecuteButtonClicked, this));
 }
 
 void MotionPlanningFrame::stopButtonClicked()
 {
   ui_->stop_button->setEnabled(false);  // avoid clicking again
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeStopButtonClicked, this), "stop");
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeStopButtonClicked, this), "stop");
 }
 
 void MotionPlanningFrame::allowReplanningToggled(bool checked)
@@ -269,8 +268,8 @@ void MotionPlanningFrame::onNewPlanningSceneState()
 void MotionPlanningFrame::startStateTextChanged(const QString& start_state)
 {
   // use background job: fetching the current state might take up to a second
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::startStateTextChangedExec, this,
-                                                  start_state.toStdString()),
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::startStateTextChangedExec, this,
+                                                start_state.toStdString()),
                                       "update start state");
 }
 
@@ -285,7 +284,7 @@ void MotionPlanningFrame::goalStateTextChanged(const QString& goal_state)
 {
   // use background job: fetching the current state might take up to a second
   planning_display_->addBackgroundJob(
-      boost::bind(&MotionPlanningFrame::goalStateTextChangedExec, this, goal_state.toStdString()), "update goal state");
+      std::bind(&MotionPlanningFrame::goalStateTextChangedExec, this, goal_state.toStdString()), "update goal state");
 }
 
 void MotionPlanningFrame::goalStateTextChangedExec(const std::string& goal_state)
@@ -454,8 +453,7 @@ void MotionPlanningFrame::populatePlannerDescription(const moveit_msgs::PlannerI
 void MotionPlanningFrame::populateConstraintsList()
 {
   if (move_group_)
-    planning_display_->addMainLoopJob(
-        boost::bind(&MotionPlanningFrame::populateConstraintsList, this, move_group_->getKnownConstraints()));
+    planning_display_->addMainLoopJob([this]() { populateConstraintsList(move_group_->getKnownConstraints()); });
 }
 
 void MotionPlanningFrame::populateConstraintsList(const std::vector<std::string>& constr)
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp
index 430e9cfbd3..b1c2e0849e 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp
@@ -108,7 +108,7 @@ void MotionPlanningFrame::saveSceneButtonClicked()
       }
     }
 
-    planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeSaveSceneButtonClicked, this),
+    planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeSaveSceneButtonClicked, this),
                                         "save scene");
   }
 }
@@ -132,7 +132,7 @@ void MotionPlanningFrame::saveQueryButtonClicked()
       {
         std::string scene = s->text(0).toStdString();
         planning_display_->addBackgroundJob(
-            boost::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, ""), "save query");
+            std::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, ""), "save query");
       }
       else
       {
@@ -177,7 +177,7 @@ void MotionPlanningFrame::saveQueryButtonClicked()
           }
         }
         planning_display_->addBackgroundJob(
-            boost::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, query_name), "save query");
+            std::bind(&MotionPlanningFrame::computeSaveQueryButtonClicked, this, scene, query_name), "save query");
       }
     }
   }
@@ -185,25 +185,25 @@ void MotionPlanningFrame::saveQueryButtonClicked()
 
 void MotionPlanningFrame::deleteSceneButtonClicked()
 {
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeDeleteSceneButtonClicked, this),
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeDeleteSceneButtonClicked, this),
                                       "delete scene");
 }
 
 void MotionPlanningFrame::deleteQueryButtonClicked()
 {
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeDeleteQueryButtonClicked, this),
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeDeleteQueryButtonClicked, this),
                                       "delete query");
 }
 
 void MotionPlanningFrame::loadSceneButtonClicked()
 {
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeLoadSceneButtonClicked, this),
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeLoadSceneButtonClicked, this),
                                       "load scene");
 }
 
 void MotionPlanningFrame::loadQueryButtonClicked()
 {
-  planning_display_->addBackgroundJob(boost::bind(&MotionPlanningFrame::computeLoadQueryButtonClicked, this),
+  planning_display_->addBackgroundJob(std::bind(&MotionPlanningFrame::computeLoadQueryButtonClicked, this),
                                       "load query");
 }
 
@@ -221,7 +221,7 @@ void MotionPlanningFrame::warehouseItemNameChanged(QTreeWidgetItem* item, int co
 
     if (planning_scene_storage->hasPlanningScene(new_name))
     {
-      planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
+      planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
       QMessageBox::warning(this, "Scene not renamed",
                            QString("The scene name '").append(item->text(column)).append("' already exists"));
       return;
@@ -239,7 +239,7 @@ void MotionPlanningFrame::warehouseItemNameChanged(QTreeWidgetItem* item, int co
     std::string new_name = item->text(column).toStdString();
     if (planning_scene_storage->hasPlanningQuery(scene, new_name))
     {
-      planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
+      planning_display_->addMainLoopJob(std::bind(&MotionPlanningFrame::populatePlanningSceneTreeView, this));
       QMessageBox::warning(this, "Query not renamed",
                            QString("The query name '")
                                .append(item->text(column))
diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
index bea40fafa0..cb9603c505 100644
--- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
+++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
@@ -210,7 +210,7 @@ void PlanningSceneDisplay::reset()
   Display::reset();
 
   if (isEnabled())
-    addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel");
+    addBackgroundJob(std::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel");
 
   if (planning_scene_robot_)
   {
@@ -386,7 +386,7 @@ void PlanningSceneDisplay::changedPlanningSceneTopic()
       service_name = ros::names::append(getMoveGroupNS(), service_name);
     auto bg_func = [=]() {
       if (planning_scene_monitor_->requestPlanningSceneState(service_name))
-        addMainLoopJob(boost::bind(&PlanningSceneDisplay::onNewPlanningSceneState, this));
+        addMainLoopJob(std::bind(&PlanningSceneDisplay::onNewPlanningSceneState, this));
       else
         setStatus(rviz::StatusProperty::Warn, "PlanningScene", "Requesting initial scene failed");
     };
@@ -529,7 +529,7 @@ void PlanningSceneDisplay::loadRobotModel()
   // we need to make sure the clearing of the robot model is in the main thread,
   // so that rendering operations do not have data removed from underneath,
   // so the next function is executed in the main loop
-  addMainLoopJob(boost::bind(&PlanningSceneDisplay::clearRobotModel, this));
+  addMainLoopJob(std::bind(&PlanningSceneDisplay::clearRobotModel, this));
 
   waitForAllMainLoopJobs();
 
@@ -537,8 +537,9 @@ void PlanningSceneDisplay::loadRobotModel()
   if (psm->getPlanningScene())
   {
     planning_scene_monitor_.swap(psm);
-    planning_scene_monitor_->addUpdateCallback(boost::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, _1));
-    addMainLoopJob(boost::bind(&PlanningSceneDisplay::onRobotModelLoaded, this));
+    planning_scene_monitor_->addUpdateCallback(
+        std::bind(&PlanningSceneDisplay::sceneMonitorReceivedUpdate, this, std::placeholders::_1));
+    addMainLoopJob(std::bind(&PlanningSceneDisplay::onRobotModelLoaded, this));
     waitForAllMainLoopJobs();
   }
   else
@@ -598,7 +599,7 @@ void PlanningSceneDisplay::onEnable()
 {
   Display::onEnable();
 
-  addBackgroundJob(boost::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel");
+  addBackgroundJob(std::bind(&PlanningSceneDisplay::loadRobotModel, this), "loadRobotModel");
 
   if (planning_scene_robot_)
   {
diff --git a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp
index 4b1afa32a3..b24612c183 100644
--- a/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp
+++ b/moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp
@@ -186,14 +186,16 @@ int main(int argc, char** argv)
       ROS_INFO(" * %s", name.c_str());
   }
 
-  psm.addUpdateCallback(boost::bind(&onSceneUpdate, &psm, &pss));
+  psm.addUpdateCallback(std::bind(&onSceneUpdate, &psm, &pss));
 
   boost::function<void(const moveit_msgs::MotionPlanRequestConstPtr&)> callback1 =
-      boost::bind(&onMotionPlanRequest, _1, &psm, &pss);
+      std::bind(&onMotionPlanRequest, std::placeholders::_1, &psm, &pss);
   ros::Subscriber mplan_req_sub = nh.subscribe("motion_plan_request", 100, callback1);
-  boost::function<void(const moveit_msgs::ConstraintsConstPtr&)> callback2 = boost::bind(&onConstraints, _1, &cs);
+  boost::function<void(const moveit_msgs::ConstraintsConstPtr&)> callback2 =
+      std::bind(&onConstraints, std::placeholders::_1, &cs);
   ros::Subscriber constr_sub = nh.subscribe("constraints", 100, callback2);
-  boost::function<void(const moveit_msgs::RobotStateConstPtr&)> callback3 = boost::bind(&onRobotState, _1, &rs);
+  boost::function<void(const moveit_msgs::RobotStateConstPtr&)> callback3 =
+      std::bind(&onRobotState, std::placeholders::_1, &rs);
   ros::Subscriber state_sub = nh.subscribe("robot_state", 100, callback3);
 
   std::vector<std::string> topics;
diff --git a/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp b/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp
index cc1614f668..2bf03d529c 100644
--- a/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp
+++ b/moveit_ros/warehouse/warehouse/src/warehouse_services.cpp
@@ -181,27 +181,27 @@ int main(int argc, char** argv)
 
   boost::function<bool(moveit_msgs::SaveRobotStateToWarehouse::Request & request,
                        moveit_msgs::SaveRobotStateToWarehouse::Response & response)>
-      save_cb = boost::bind(&storeState, _1, _2, &rs);
+      save_cb = std::bind(&storeState, std::placeholders::_1, std::placeholders::_2, &rs);
 
   boost::function<bool(moveit_msgs::ListRobotStatesInWarehouse::Request & request,
                        moveit_msgs::ListRobotStatesInWarehouse::Response & response)>
-      list_cb = boost::bind(&listStates, _1, _2, &rs);
+      list_cb = std::bind(&listStates, std::placeholders::_1, std::placeholders::_2, &rs);
 
   boost::function<bool(moveit_msgs::GetRobotStateFromWarehouse::Request & request,
                        moveit_msgs::GetRobotStateFromWarehouse::Response & response)>
-      get_cb = boost::bind(&getState, _1, _2, &rs);
+      get_cb = std::bind(&getState, std::placeholders::_1, std::placeholders::_2, &rs);
 
   boost::function<bool(moveit_msgs::CheckIfRobotStateExistsInWarehouse::Request & request,
                        moveit_msgs::CheckIfRobotStateExistsInWarehouse::Response & response)>
-      has_cb = boost::bind(&hasState, _1, _2, &rs);
+      has_cb = std::bind(&hasState, std::placeholders::_1, std::placeholders::_2, &rs);
 
   boost::function<bool(moveit_msgs::RenameRobotStateInWarehouse::Request & request,
                        moveit_msgs::RenameRobotStateInWarehouse::Response & response)>
-      rename_cb = boost::bind(&renameState, _1, _2, &rs);
+      rename_cb = std::bind(&renameState, std::placeholders::_1, std::placeholders::_2, &rs);
 
   boost::function<bool(moveit_msgs::DeleteRobotStateFromWarehouse::Request & request,
                        moveit_msgs::DeleteRobotStateFromWarehouse::Response & response)>
-      delete_cb = boost::bind(&deleteState, _1, _2, &rs);
+      delete_cb = std::bind(&deleteState, std::placeholders::_1, std::placeholders::_2, &rs);
 
   ros::ServiceServer save_state_server = node.advertiseService("save_robot_state", save_cb);
   ros::ServiceServer list_states_server = node.advertiseService("list_robot_states", list_cb);
diff --git a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp
index 58421c7b40..ab57be15d4 100644
--- a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp
+++ b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp
@@ -564,7 +564,7 @@ unsigned int disableNeverInCollision(const unsigned int num_trials, planning_sce
   for (int i = 0; i < num_threads; ++i)
   {
     ThreadComputation tc(scene, req, i, num_trials / num_threads, &links_seen_colliding, &lock, progress);
-    bgroup.create_thread(boost::bind(&disableNeverInCollisionThread, tc));
+    bgroup.create_thread(std::bind(&disableNeverInCollisionThread, tc));
   }
 
   try
diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
index 55458bbf89..2685d6d1d8 100644
--- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
@@ -226,7 +226,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = file.file_name_;
   template_path = config_data_->appendPaths(config_data_->template_package_path_, "package.xml.template");
   file.description_ = "Defines a ROS package";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = MoveItConfigData::AUTHOR_INFO;
   gen_files_.push_back(file);
 
@@ -235,7 +235,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = file.file_name_;
   template_path = config_data_->appendPaths(config_data_->template_package_path_, file.file_name_);
   file.description_ = "CMake build system configuration file";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -249,7 +249,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = file.file_name_;
   file.description_ = "Folder containing all MoveIt configuration files for your robot. This folder is required and "
                       "cannot be disabled.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::createFolder, this, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::createFolder, this, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -263,7 +263,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
                       "representation of semantic information about robots. This format is intended to represent "
                       "information about the robot that is not in the URDF file, but it is useful for a variety of "
                       "applications. The intention is to include information that has a semantic aspect to it.";
-  file.gen_func_ = boost::bind(&srdf::SRDFWriter::writeSRDF, config_data_->srdf_, _1);
+  file.gen_func_ = std::bind(&srdf::SRDFWriter::writeSRDF, config_data_->srdf_, std::placeholders::_1);
   file.write_on_changes = MoveItConfigData::SRDF;
   gen_files_.push_back(file);
   // special step required so the generated .setup_assistant yaml has this value
@@ -280,7 +280,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
                       "planner_configs tag. While defining a planner configuration, the only mandatory parameter is "
                       "'type', which is the name of the motion planner to be used. Any other planner-specific "
                       "parameters can be defined but are optional.";
-  file.gen_func_ = boost::bind(&MoveItConfigData::outputOMPLPlanningYAML, config_data_, _1);
+  file.gen_func_ = std::bind(&MoveItConfigData::outputOMPLPlanningYAML, config_data_, std::placeholders::_1);
   file.write_on_changes = MoveItConfigData::GROUPS;
   gen_files_.push_back(file);
 
@@ -288,7 +288,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.file_name_ = "chomp_planning.yaml";
   file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_);
   file.description_ = "Specifies which chomp planning plugin parameters to be used for the CHOMP planner";
-  file.gen_func_ = boost::bind(&MoveItConfigData::outputCHOMPPlanningYAML, config_data_, _1);
+  file.gen_func_ = std::bind(&MoveItConfigData::outputCHOMPPlanningYAML, config_data_, std::placeholders::_1);
   file.write_on_changes = MoveItConfigData::GROUPS;  // need to double check if this is actually correct!
   gen_files_.push_back(file);
 
@@ -297,7 +297,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_);
   file.description_ = "Specifies which kinematic solver plugin to use for each planning group in the SRDF, as well as "
                       "the kinematic solver search resolution.";
-  file.gen_func_ = boost::bind(&MoveItConfigData::outputKinematicsYAML, config_data_, _1);
+  file.gen_func_ = std::bind(&MoveItConfigData::outputKinematicsYAML, config_data_, std::placeholders::_1);
   file.write_on_changes = MoveItConfigData::GROUPS | MoveItConfigData::GROUP_KINEMATICS;
   gen_files_.push_back(file);
 
@@ -309,7 +309,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
                       "and acceleration than those contained in your URDF. This information is used by our trajectory "
                       "filtering system to assign reasonable velocities and timing for the trajectory before it is "
                       "passed to the robots controllers.";
-  file.gen_func_ = boost::bind(&MoveItConfigData::outputJointLimitsYAML, config_data_, _1);
+  file.gen_func_ = std::bind(&MoveItConfigData::outputJointLimitsYAML, config_data_, std::placeholders::_1);
   file.write_on_changes = 0;  // Can they be changed?
   gen_files_.push_back(file);
 
@@ -320,7 +320,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.description_ = "Cartesian velocity for planning in the workspace."
                       "The velocity is used by pilz industrial motion planner as maximum velocity for cartesian "
                       "planning requests scaled by the velocity scaling factor of an individual planning request.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;  // Can they be changed?
   gen_files_.push_back(file);
 
@@ -329,7 +329,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_);
   file.description_ = "Creates dummy configurations for controllers that correspond to defined groups. This is mostly "
                       "useful for testing.";
-  file.gen_func_ = boost::bind(&MoveItConfigData::outputFakeControllersYAML, config_data_, _1);
+  file.gen_func_ = std::bind(&MoveItConfigData::outputFakeControllersYAML, config_data_, std::placeholders::_1);
   file.write_on_changes = MoveItConfigData::GROUPS;
   gen_files_.push_back(file);
 
@@ -337,7 +337,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.file_name_ = "simple_moveit_controllers.yaml";
   file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_);
   file.description_ = "Creates controller configuration for SimpleMoveItControllerManager";
-  file.gen_func_ = boost::bind(&MoveItConfigData::outputSimpleControllersYAML, config_data_, _1);
+  file.gen_func_ = std::bind(&MoveItConfigData::outputSimpleControllersYAML, config_data_, std::placeholders::_1);
   file.write_on_changes = MoveItConfigData::GROUPS;
   gen_files_.push_back(file);
 
@@ -346,14 +346,14 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_);
   template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_);
   file.description_ = "Configuration of Gazebo controllers";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   gen_files_.push_back(file);
 
   // ros_controllers.yaml --------------------------------------------------------------------------------------
   file.file_name_ = "ros_controllers.yaml";
   file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_);
   file.description_ = "Creates controller configurations for ros_control.";
-  file.gen_func_ = boost::bind(&MoveItConfigData::outputROSControllersYAML, config_data_, _1);
+  file.gen_func_ = std::bind(&MoveItConfigData::outputROSControllersYAML, config_data_, std::placeholders::_1);
   file.write_on_changes = MoveItConfigData::GROUPS;
   gen_files_.push_back(file);
 
@@ -361,7 +361,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.file_name_ = "sensors_3d.yaml";
   file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_);
   file.description_ = "Creates configurations 3d sensors.";
-  file.gen_func_ = boost::bind(&MoveItConfigData::output3DSensorPluginYAML, config_data_, _1);
+  file.gen_func_ = std::bind(&MoveItConfigData::output3DSensorPluginYAML, config_data_, std::placeholders::_1);
   file.write_on_changes = MoveItConfigData::SENSORS_CONFIG;
   gen_files_.push_back(file);
 
@@ -376,7 +376,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = file.file_name_;
   file.description_ = "Folder containing all MoveIt launch files for your robot. "
                       "This folder is required and cannot be disabled.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::createFolder, this, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::createFolder, this, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -387,7 +387,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.description_ = "Launches the move_group node that provides the MoveGroup action and other parameters <a "
                       "href='http://moveit.ros.org/doxygen/"
                       "classmoveit_1_1planning__interface_1_1MoveGroup.html#details'>MoveGroup action</a>";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -397,7 +397,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Loads settings for the ROS parameter server, required for running MoveIt. This includes the "
                       "SRDF, joints_limits.yaml file, ompl_planning.yaml file, optionally the URDF, etc";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -407,7 +407,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Visualize in Rviz the robot's planning groups running with interactive markers that allow goal "
                       "states to be set.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -419,7 +419,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.description_ = "Intended to be included in other launch files that require the OMPL planning plugin. Defines "
                       "the proper plugin name on the parameter server and a default selection of planning request "
                       "adapters.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -431,7 +431,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.description_ = "Intended to be included in other launch files that require the Pilz industrial motion planner "
                       "planning plugin. Defines the proper plugin name on the parameter server and a default selection "
                       "of planning request adapters.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -443,7 +443,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.description_ = "Intended to be included in other launch files that require the CHOMP planning plugin. Defines "
                       "the proper plugin name on the parameter server and a default selection of planning request "
                       "adapters.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -452,7 +452,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Helper launch file that can choose between different planning pipelines to be loaded.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -461,7 +461,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Helper launch file that specifies default settings for MongoDB.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -470,7 +470,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Launch file for starting MongoDB.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -479,7 +479,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Launch file for starting the warehouse with a default MongoDB.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -488,7 +488,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Launch file for benchmarking OMPL planners";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -497,7 +497,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Helper launch file that can choose between different sensor managers to be loaded.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -506,7 +506,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, "moveit_sensor_manager.launch.xml");
   file.description_ = "Placeholder for settings specific to the MoveIt sensor manager implemented for you robot.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -516,7 +516,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Loads settings for the ROS parameter server required for executing trajectories using the "
                       "trajectory_execution_manager::TrajectoryExecutionManager.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -524,7 +524,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Loads the fake controller plugin.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -532,7 +532,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Loads the default controller plugin.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -540,7 +540,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Loads the ros_control controller plugin.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -549,7 +549,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Run a demo of MoveIt.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -558,7 +558,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Run a demo of MoveIt with Gazebo and Rviz";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -568,7 +568,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   template_path = config_data_->appendPaths(template_launch_path, "gazebo.launch");
   file.description_ = "Gazebo launch file which also launches ros_controllers and sends robot urdf to param server, "
                       "then using gazebo_ros pkg the robot is spawned to Gazebo";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   gen_files_.push_back(file);
 
   // joystick_control.launch ------------------------------------------------------------------
@@ -576,7 +576,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
   file.description_ = "Control the Rviz Motion Planning Plugin with a joystick";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -588,7 +588,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
                                                                    // up with the SA's real launch file
   file.description_ = "Launch file for easily re-starting the MoveIt Setup Assistant to edit this robot's generated "
                       "configuration package.";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -597,7 +597,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
   template_path = config_data_->appendPaths(template_launch_path, "ros_controllers.launch");
   file.description_ = "ros_controllers launch file";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = MoveItConfigData::GROUPS;
   gen_files_.push_back(file);
 
@@ -607,7 +607,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   template_path = config_data_->appendPaths(template_launch_path, "moveit.rviz");
   file.description_ = "Configuration file for Rviz with the Motion Planning Plugin already setup. Used by passing "
                       "roslaunch moveit_rviz.launch config:=true";
-  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.gen_func_ = std::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, std::placeholders::_1);
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
@@ -619,7 +619,7 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.file_name_ = SETUP_ASSISTANT_FILE;
   file.rel_path_ = file.file_name_;
   file.description_ = "MoveIt Setup Assistant's hidden settings file. You should not need to edit this file.";
-  file.gen_func_ = boost::bind(&MoveItConfigData::outputSetupAssistantFile, config_data_, _1);
+  file.gen_func_ = std::bind(&MoveItConfigData::outputSetupAssistantFile, config_data_, std::placeholders::_1);
   file.write_on_changes = -1;  // write on any changes
   gen_files_.push_back(file);
 
diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp
index 76be30b8db..cb548414e4 100644
--- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp
@@ -235,7 +235,8 @@ void DefaultCollisionsWidget::startGeneratingCollisionTable()
   btn_revert_->setEnabled(true);  // allow to interrupt and revert
 
   // create a MonitorThread running generateCollisionTable() in a worker thread and monitoring the progress
-  worker_ = new MonitorThread(boost::bind(&DefaultCollisionsWidget::generateCollisionTable, this, _1), progress_bar_);
+  worker_ = new MonitorThread(std::bind(&DefaultCollisionsWidget::generateCollisionTable, this, std::placeholders::_1),
+                              progress_bar_);
   connect(worker_, SIGNAL(finished()), this, SLOT(finishGeneratingCollisionTable()));
   worker_->start();  // start after having finished() signal connected
 }
@@ -820,7 +821,7 @@ moveit_setup_assistant::MonitorThread::MonitorThread(const boost::function<void(
   : progress_(0), canceled_(false)
 {
   // start worker thread
-  worker_ = boost::thread(boost::bind(f, &progress_));
+  worker_ = boost::thread(std::bind(f, &progress_));
   // connect progress bar for updates
   if (progress_bar)
     connect(this, SIGNAL(progress(int)), progress_bar, SLOT(setValue(int)));

From fd36674cc327962aaf27925ddf1ba9c6a8667d35 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Michael=20G=C3=B6rner?= <me@v4hn.de>
Date: Sun, 28 Nov 2021 20:18:39 +0100
Subject: [PATCH 026/114] Add marker for subgroups even if no endeffector is
 defined for them (#2977)

For single groups, the old logic fell back to add a marker
for the last link if IK is supported for it and no endeffector is defined.

That (quite reasonable) fallback did not yet work for subgroups though.
---
 .../src/robot_interaction.cpp                 | 59 +++++++++----------
 1 file changed, 28 insertions(+), 31 deletions(-)

diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp
index d4128f0dc2..7d3787ac5a 100644
--- a/moveit_ros/robot_interaction/src/robot_interaction.cpp
+++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp
@@ -280,56 +280,53 @@ void RobotInteraction::decideActiveEndEffectors(const std::string& group, Intera
   const std::pair<moveit::core::JointModelGroup::KinematicsSolver, moveit::core::JointModelGroup::KinematicsSolverMap>&
       smap = jmg->getGroupKinematics();
 
-  // if we have an IK solver for the selected group, we check if there are any end effectors attached to this group
-  if (smap.first)
-  {
+  auto add_active_end_effectors_for_single_group = [&](const moveit::core::JointModelGroup* single_group) {
+    bool found_eef{ false };
     for (const srdf::Model::EndEffector& eef : eefs)
-      if ((jmg->hasLinkModel(eef.parent_link_) || jmg->getName() == eef.parent_group_) &&
-          jmg->canSetStateFromIK(eef.parent_link_))
+      if ((single_group->hasLinkModel(eef.parent_link_) || single_group->getName() == eef.parent_group_) &&
+          single_group->canSetStateFromIK(eef.parent_link_))
       {
         // We found an end-effector whose parent is the group.
         EndEffectorInteraction ee;
-        ee.parent_group = group;
+        ee.parent_group = single_group->getName();
         ee.parent_link = eef.parent_link_;
         ee.eef_group = eef.component_group_;
         ee.interaction = style;
         active_eef_.push_back(ee);
+        found_eef = true;
       }
 
-    // No end effectors found.  Use last link in group as the "end effector".
-    if (active_eef_.empty() && !jmg->getLinkModelNames().empty())
+    // No end effectors found. Use last link in group as the "end effector".
+    if (!found_eef && !single_group->getLinkModelNames().empty())
     {
-      EndEffectorInteraction ee;
-      ee.parent_group = group;
-      ee.parent_link = jmg->getLinkModelNames().back();
-      ee.eef_group = group;
-      ee.interaction = style;
-      active_eef_.push_back(ee);
+      std::string last_link{ single_group->getLinkModelNames().back() };
+
+      if (single_group->canSetStateFromIK(last_link))
+      {
+        EndEffectorInteraction ee;
+        ee.parent_group = single_group->getName();
+        ee.parent_link = last_link;
+        ee.eef_group = single_group->getName();
+        ee.interaction = style;
+        active_eef_.push_back(ee);
+      }
     }
+  };
+
+  // if we have an IK solver for the selected group, we check if there are any end effectors attached to this group
+  if (smap.first)
+  {
+    add_active_end_effectors_for_single_group(jmg);
   }
+  // if the group contains subgroups with IK, add markers for them individually
   else if (!smap.second.empty())
   {
     for (const std::pair<const moveit::core::JointModelGroup* const, moveit::core::JointModelGroup::KinematicsSolver>&
              it : smap.second)
-    {
-      for (const srdf::Model::EndEffector& eef : eefs)
-      {
-        if ((it.first->hasLinkModel(eef.parent_link_) || jmg->getName() == eef.parent_group_) &&
-            it.first->canSetStateFromIK(eef.parent_link_))
-        {
-          // We found an end-effector whose parent is a subgroup of the group.  (May be more than one)
-          EndEffectorInteraction ee;
-          ee.parent_group = it.first->getName();
-          ee.parent_link = eef.parent_link_;
-          ee.eef_group = eef.component_group_;
-          ee.interaction = style;
-          active_eef_.push_back(ee);
-          break;
-        }
-      }
-    }
+      add_active_end_effectors_for_single_group(it.first);
   }
 
+  // lastly determine automatic marker sizes
   for (EndEffectorInteraction& eef : active_eef_)
   {
     // if we have a separate group for the eef, we compute the scale based on it;

From fb0a5b5239c4c1364b12b76629eaa3b9af2d8739 Mon Sep 17 00:00:00 2001
From: Simon Schmeisser <simon.schmeisser@optonic.com>
Date: Tue, 7 Dec 2021 15:17:28 +0100
Subject: [PATCH 027/114] Use termination condition for simplification step
 (#2981)

... to allow canceling the simplification step
---
 .../ompl_interface/src/model_based_planning_context.cpp     | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

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 691ab5a9e8..7bb0d51b54 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
@@ -403,8 +403,12 @@ void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_m
 
 void ompl_interface::ModelBasedPlanningContext::simplifySolution(double timeout)
 {
-  ompl_simple_setup_->simplifySolution(timeout);
+  ompl::time::point start = ompl::time::now();
+  ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout, start);
+  registerTerminationCondition(ptc);
+  ompl_simple_setup_->simplifySolution(ptc);
   last_simplify_time_ = ompl_simple_setup_->getLastSimplificationTime();
+  unregisterTerminationCondition();
 }
 
 void ompl_interface::ModelBasedPlanningContext::interpolateSolution()

From 9d78be4705fad661a9cd2ef90b0f9d933d60b325 Mon Sep 17 00:00:00 2001
From: Wolfgang Merkt <wxmerkt@users.noreply.github.com>
Date: Fri, 10 Dec 2021 14:15:29 +0000
Subject: [PATCH 028/114] Add backwards compatibility for old scene
 serialization format (#2986)

* [moveit_core] test_planning_scene: Add failing unit test for old scene format

The serialization format for the .scene files changed in
https://github.com/ros-planning/moveit/pull/2037. This commits a
testcase using the old scene format. It will fail and a subsequent
commit to introduce backwards compatibility to the scene-file parsing
will make it pass.

* [moveit_core] PlanningScene: Add backwards compatibility for old scene version format

This commit adds a mechanism for detecting the version of the scene file
format to enable the loadGeometryFromStream method to read old version
scene files without having to migrate them. To detect the version of the
scene format, we use the content of the line following the start of an
object: In the old version of the format, this specified the number of
shapes in the object (a single int). In the new version of the format,
it is the translational part of the pose of the object (i.e. three
double values separated by spaces). To detect the format, we check for
the number of spaces after trimming the string.

* Simplify code: Avoid reading full stream

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
---
 .../planning_scene/src/planning_scene.cpp     | 48 +++++++++++++------
 .../test/test_planning_scene.cpp              | 30 +++++++++++-
 2 files changed, 63 insertions(+), 15 deletions(-)

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
index 43639be742..d59d952fa8 100644
--- a/moveit_core/planning_scene/src/planning_scene.cpp
+++ b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -1007,7 +1007,23 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet
     ROS_ERROR_NAMED(LOGNAME, "Bad input stream when loading scene geometry");
     return false;
   }
+  // Read scene name
   std::getline(in, name_);
+
+  // Identify scene format version for backwards compatibility of parser
+  auto pos = in.tellg();  // remember current stream position
+  std::string line;
+  do
+  {
+    std::getline(in, line);
+  } while (in.good() && !in.eof() && (line.empty() || line[0] != '*'));  // read * marker
+  std::getline(in, line);                                                // next line determines format
+  boost::algorithm::trim(line);
+  // new format: line specifies position of object, with spaces as delimiter -> spaces indicate new format
+  // old format: line specifies number of shapes
+  bool uses_new_scene_format = line.find(' ') != std::string::npos;
+  in.seekg(pos);
+
   Eigen::Isometry3d pose;  // Transient
   do
   {
@@ -1029,8 +1045,9 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet
       }
       boost::algorithm::trim(object_id);
 
-      // Read in object pose
-      if (!readPoseFromText(in, pose))
+      // Read in object pose (added in the new scene format)
+      pose.setIdentity();
+      if (uses_new_scene_format && !readPoseFromText(in, pose))
       {
         ROS_ERROR_NAMED(LOGNAME, "Failed to read object pose from scene file");
         return false;
@@ -1075,22 +1092,25 @@ bool PlanningScene::loadGeometryFromStream(std::istream& in, const Eigen::Isomet
         }
       }
 
-      // Read in subframes
-      moveit::core::FixedTransformsMap subframes;
-      unsigned int subframe_count;
-      in >> subframe_count;
-      for (std::size_t i = 0; i < subframe_count && in.good() && !in.eof(); ++i)
+      // Read in subframes (added in the new scene format)
+      if (uses_new_scene_format)
       {
-        std::string subframe_name;
-        in >> subframe_name;
-        if (!readPoseFromText(in, pose))
+        moveit::core::FixedTransformsMap subframes;
+        unsigned int subframe_count;
+        in >> subframe_count;
+        for (std::size_t i = 0; i < subframe_count && in.good() && !in.eof(); ++i)
         {
-          ROS_ERROR_NAMED(LOGNAME, "Failed to read subframe pose from scene file");
-          return false;
+          std::string subframe_name;
+          in >> subframe_name;
+          if (!readPoseFromText(in, pose))
+          {
+            ROS_ERROR_NAMED(LOGNAME, "Failed to read subframe pose from scene file");
+            return false;
+          }
+          subframes[subframe_name] = pose;
         }
-        subframes[subframe_name] = pose;
+        world_->setSubframesOfObject(object_id, subframes);
       }
-      world_->setSubframesOfObject(object_id, subframes);
     }
     else if (marker == ".")
     {
diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp
index 2afaa788fb..86df646a23 100644
--- a/moveit_core/planning_scene/test/test_planning_scene.cpp
+++ b/moveit_core/planning_scene/test/test_planning_scene.cpp
@@ -164,7 +164,7 @@ TEST(PlanningScene, isStateValid)
   }
 }
 
-TEST(PlanningScene, loadGoodSceneGeometry)
+TEST(PlanningScene, loadGoodSceneGeometryNewFormat)
 {
   moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2");
   auto ps = std::make_shared<planning_scene::PlanningScene>(robot_model->getURDF(), robot_model->getSRDF());
@@ -199,6 +199,32 @@ TEST(PlanningScene, loadGoodSceneGeometry)
   EXPECT_FALSE(ps->getWorld()->hasObject("baz"));  // Sanity check.
 }
 
+TEST(PlanningScene, loadGoodSceneGeometryOldFormat)
+{
+  moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2");
+  auto ps = std::make_shared<planning_scene::PlanningScene>(robot_model->getURDF(), robot_model->getSRDF());
+
+  std::istringstream good_scene_geometry;
+  good_scene_geometry.str("foobar_scene\n"
+                          "* foo\n"
+                          "2\n"
+                          "box\n"
+                          ".77 0.39 0.05\n"
+                          "0 0 0.025\n"
+                          "0 0 0 1\n"
+                          "0.82 0.75 0.60 1\n"
+                          "box\n"
+                          ".77 0.39 0.05\n"
+                          "0 0 1.445\n"
+                          "0 0 0 1\n"
+                          "0.82 0.75 0.60 1\n"
+                          ".\n");
+  EXPECT_TRUE(ps->loadGeometryFromStream(good_scene_geometry));
+  EXPECT_EQ(ps->getName(), "foobar_scene");
+  EXPECT_TRUE(ps->getWorld()->hasObject("foo"));
+  EXPECT_FALSE(ps->getWorld()->hasObject("baz"));  // Sanity check.
+}
+
 TEST(PlanningScene, loadBadSceneGeometry)
 {
   moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2");
@@ -211,6 +237,8 @@ TEST(PlanningScene, loadBadSceneGeometry)
   std::istringstream malformed_scene_geometry;
   malformed_scene_geometry.str("malformed_scene_geometry\n"
                                "* foo\n"
+                               "0 0 0\n"
+                               "0 0 0 1\n"
                                "1\n"
                                "box\n"
                                "2.58 1.36\n" /* Only two tokens; should be 3 */

From 9bfb2c73cf70acef7ba9d127698831d3fe14bd04 Mon Sep 17 00:00:00 2001
From: Simon Schmeisser <simon.schmeisser@optonic.com>
Date: Fri, 10 Dec 2021 15:20:49 +0100
Subject: [PATCH 029/114] Allow restricting collision pairs to a group (#2987)

---
 .../moveit/planning_scene/planning_scene.h    | 29 +++++++++++--------
 .../planning_scene/src/planning_scene.cpp     |  4 ++-
 2 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
index 7187b47893..fb8c5336ec 100644
--- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
+++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
@@ -560,37 +560,42 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
     getCollidingPairs(contacts, getCurrentState(), getAllowedCollisionMatrix());
   }
 
-  /** \brief Get the names of the links that are involved in collisions for the state \e robot_state */
+  /** \brief Get the names of the links that are involved in collisions for the state \e robot_state.
+   *  Can be restricted to links part of or updated by \e group_name */
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
-                         const moveit::core::RobotState& robot_state) const
+                         const moveit::core::RobotState& robot_state, const std::string& group_name = "") const
   {
-    getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix());
+    getCollidingPairs(contacts, robot_state, getAllowedCollisionMatrix(), group_name);
   }
 
   /** \brief Get the names of the links that are involved in collisions for the state \e robot_state.
-      Update the link transforms for \e robot_state if needed. */
+      Update the link transforms for \e robot_state if needed.
+      Can be restricted to links part of or updated by \e group_name */
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
-                         moveit::core::RobotState& robot_state) const
+                         moveit::core::RobotState& robot_state, const std::string& group_name = "") const
   {
     robot_state.updateCollisionBodyTransforms();
-    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix());
+    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), getAllowedCollisionMatrix(),
+                      group_name);
   }
 
   /** \brief  Get the names of the links that are involved in collisions for the state \e robot_state given the
-      allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed. */
+      allowed collision matrix (\e acm). Update the link transforms for \e robot_state if needed.
+      Can be restricted to links part of or updated by \e group_name*/
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
-                         moveit::core::RobotState& robot_state,
-                         const collision_detection::AllowedCollisionMatrix& acm) const
+                         moveit::core::RobotState& robot_state, const collision_detection::AllowedCollisionMatrix& acm,
+                         const std::string& group_name = "") const
   {
     robot_state.updateCollisionBodyTransforms();
-    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), acm);
+    getCollidingPairs(contacts, static_cast<const moveit::core::RobotState&>(robot_state), acm, group_name);
   }
 
   /** \brief  Get the names of the links that are involved in collisions for the state \e robot_state given the
-      allowed collision matrix (\e acm) */
+      allowed collision matrix (\e acm). Can be restricted to links part of or updated by \e group_name */
   void getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
                          const moveit::core::RobotState& robot_state,
-                         const collision_detection::AllowedCollisionMatrix& acm) const;
+                         const collision_detection::AllowedCollisionMatrix& acm,
+                         const std::string& group_name = "") const;
 
   /**@}*/
 
diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
index d59d952fa8..96db06e223 100644
--- a/moveit_core/planning_scene/src/planning_scene.cpp
+++ b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -539,12 +539,14 @@ void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::Cont
 
 void PlanningScene::getCollidingPairs(collision_detection::CollisionResult::ContactMap& contacts,
                                       const moveit::core::RobotState& robot_state,
-                                      const collision_detection::AllowedCollisionMatrix& acm) const
+                                      const collision_detection::AllowedCollisionMatrix& acm,
+                                      const std::string& group_name) const
 {
   collision_detection::CollisionRequest req;
   req.contacts = true;
   req.max_contacts = getRobotModel()->getLinkModelsWithCollisionGeometry().size() + 1;
   req.max_contacts_per_pair = 1;
+  req.group_name = group_name;
   collision_detection::CollisionResult res;
   checkCollision(req, res, robot_state, acm);
   res.contacts.swap(contacts);

From 983f494f083c4773efc409b4190ff973a8c72c36 Mon Sep 17 00:00:00 2001
From: Tyler Weaver <tyler@picknik.ai>
Date: Mon, 13 Dec 2021 01:09:12 -0700
Subject: [PATCH 030/114] Increase ccache size (#2990)

Increase ccache size to 10G. This is particularly needed for ccov's debug build, which uses 8G according to the stats.

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
---
 .github/workflows/ci.yaml | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index 23274bd83e..5620d6d59b 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -36,6 +36,10 @@ jobs:
       # Pull any updates to the upstream workspace (after restoring it from cache)
       AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src
       AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src
+      # Clear the ccache stats before and log the stats after the build
+      AFTER_SETUP_CCACHE: ccache --zero-stats --max-size=10.0G
+      AFTER_BUILD_TARGET_WORKSPACE: ccache --show-stats
+      AFTER_BUILD_DOWNSTREAM_WORKSPACE: ccache --show-stats
       # Compile CCOV with Debug. Enable -Werror (except CLANG_TIDY=pedantic, which makes the clang-tidy step fail on warnings)
       TARGET_CMAKE_ARGS: >
         -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}}

From 70b0368eeab50fee4e7cd5dcffedd02b724d713f Mon Sep 17 00:00:00 2001
From: Wolf Vollprecht <w.vollprecht@gmail.com>
Date: Mon, 13 Dec 2021 09:45:17 +0100
Subject: [PATCH 031/114] RoboStack CI: Pin tinyxml2 version (#2993)

---
 .github/robostack_env.yaml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.github/robostack_env.yaml b/.github/robostack_env.yaml
index e74f96701e..4e7c7259b7 100644
--- a/.github/robostack_env.yaml
+++ b/.github/robostack_env.yaml
@@ -16,3 +16,5 @@ dependencies:
   - orocos-kdl 1.5.0
   - python-orocos-kdl 1.5.0
   - openssl 1.1.1*
+  # we need the static library of this build
+  - tinyxml2 9.0.0 *_1

From 953f58d650cf070ce48c75eb029612fe4cce26e6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Michael=20G=C3=B6rner?= <me@v4hn.de>
Date: Tue, 14 Dec 2021 11:12:58 +0100
Subject: [PATCH 032/114] quietly use backward_cpp/ros if available (#2988)

This is simply convenient and you always need it when you did not explicitly add it.

Follow @tylerjw's initiative to add it to MoveIt2:
https://github.com/ros-planning/moveit2/pull/794
---
 moveit_core/cmake/moveit.cmake | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/moveit_core/cmake/moveit.cmake b/moveit_core/cmake/moveit.cmake
index 831c218aef..8baa8f96aa 100644
--- a/moveit_core/cmake/moveit.cmake
+++ b/moveit_core/cmake/moveit.cmake
@@ -5,6 +5,8 @@ macro(moveit_build_options)
   set(CMAKE_CXX_STANDARD_REQUIRED ON)
   set(CMAKE_CXX_EXTENSIONS OFF)
 
+  find_package(backward_ros QUIET)
+
   if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
     message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance")
     set(CMAKE_BUILD_TYPE Release)

From a4e023b50c0813fa54be271a97d928e0f1433ab4 Mon Sep 17 00:00:00 2001
From: v4hn <me@v4hn.de>
Date: Tue, 14 Dec 2021 12:06:43 +0100
Subject: [PATCH 033/114] add comment to mention loading of old scene files

Followup to 9d78be4705fad661a9cd2ef90b0f9d933d60b325
---
 MIGRATION.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/MIGRATION.md b/MIGRATION.md
index aaee781532..432fafe365 100644
--- a/MIGRATION.md
+++ b/MIGRATION.md
@@ -6,7 +6,7 @@ API changes in MoveIt releases
 - `CollisionObject` messages are now defined with a `Pose`, and shapes and subframes are defined relative to the object's pose. This makes it easier to place objects with subframes and multiple shapes in the scene. This causes several changes:
     - `getFrameTransform()` now returns this pose instead of the first shape's pose.
     - The Rviz plugin's manipulation tab now uses the object's pose instead of the shape pose to evaluate if object's are in the region of interest.
-    - Planning scene geometry text files (`.scene`) have changed format. Add a line `0 0 0 0 0 0 1` under each line with an asterisk to upgrade old files if required.
+    - Planning scene geometry text files (`.scene`) have changed format. Loading old files is still supported. You can add a line `0 0 0 0 0 0 1` under each line with an asterisk to upgrade old files.
 - add API for passing RNG to setToRandomPositionsNearBy
 - Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`.
 - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link.

From 125d5e775f0b23334a2f9e1d222ebd38e64c4f33 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@users.noreply.github.com>
Date: Wed, 15 Dec 2021 09:28:32 +0100
Subject: [PATCH 034/114] Provide MOVEIT_VERSION_CHECK macro (#2997)

- Rename MOVEIT_VERSION -> MOVEIT_VERSION_STR
- MOVEIT_VERSION becomes a numeric identifier
- Use like: #if MOVEIT_VERSION >= MOVEIT_VERSION_CHECK(1, 0, 0)
---
 moveit_core/version/version.cpp                 | 2 +-
 moveit_core/version/version.h.in                | 8 +++++++-
 moveit_ros/benchmarks/src/BenchmarkExecutor.cpp | 2 +-
 3 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/moveit_core/version/version.cpp b/moveit_core/version/version.cpp
index 75ce900180..c71fca5b81 100644
--- a/moveit_core/version/version.cpp
+++ b/moveit_core/version/version.cpp
@@ -39,6 +39,6 @@
 
 int main(int /*argc*/, char** /*argv*/)
 {
-  printf("%s\n", MOVEIT_VERSION);
+  printf("%s\n", MOVEIT_VERSION_STR);
   return 0;
 }
diff --git a/moveit_core/version/version.h.in b/moveit_core/version/version.h.in
index 573a2e72b5..8e596d8f9f 100644
--- a/moveit_core/version/version.h.in
+++ b/moveit_core/version/version.h.in
@@ -35,11 +35,17 @@
 #ifndef MOVEIT_VERSION_
 #define MOVEIT_VERSION_
 
-#define MOVEIT_VERSION "@MOVEIT_VERSION@"
+#define MOVEIT_VERSION_STR "@MOVEIT_VERSION@"
 
 #define MOVEIT_VERSION_MAJOR @MOVEIT_VERSION_MAJOR@
 #define MOVEIT_VERSION_MINOR @MOVEIT_VERSION_MINOR@
 #define MOVEIT_VERSION_PATCH @MOVEIT_VERSION_PATCH@
 #define MOVEIT_VERSION_EXTRA "@MOVEIT_VERSION_EXTRA@"
 
+/// MOVEIT_VERSION is (major << 16) + (minor << 8) + patch.
+#define MOVEIT_VERSION MOVEIT_VERSION_CHECK(MOVEIT_VERSION_MAJOR, MOVEIT_VERSION_MINOR, MOVEIT_VERSION_PATCH)
+
+/// Use like: #if MOVEIT_VERSION >= MOVEIT_VERSION_CHECK(1, 0, 0)
+#define MOVEIT_VERSION_CHECK(major, minor, patch) ((major << 16) | (minor << 8) | (patch))
+
 #endif
diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp
index 35e7e98a18..ddaf611c3c 100644
--- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp
+++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp
@@ -1077,7 +1077,7 @@ void BenchmarkExecutor::writeOutput(const BenchmarkRequest& brequest, const std:
     return;
   }
 
-  out << "MoveIt version " << MOVEIT_VERSION << std::endl;
+  out << "MoveIt version " << MOVEIT_VERSION_STR << std::endl;
   out << "Experiment " << brequest.name << std::endl;
   out << "Running on " << hostname << std::endl;
   out << "Starting at " << start_time << std::endl;

From 38b84dedb8d8da5678df744f8a12637fc4255956 Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Wed, 15 Dec 2021 10:23:06 +0100
Subject: [PATCH 035/114] feat(simple_controller_manager): add `max_effort`
 parameter to GripperCommand action (#2984)

This commit adds the `max_effort` parameter to the GripperCommand
declaration in the `controller_list` (see issue #2956). This value is
only used when effort is set in the requested gripper trajectory.

Co-authored-by: Jafar Abdi <cafer.abdi@gmail.com>
---
 .../gripper_controller_handle.h                  | 16 ++++++++++++++--
 .../src/moveit_simple_controller_manager.cpp     |  5 ++++-
 2 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
index 212992de5b..ecdce0fdeb 100644
--- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
+++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h
@@ -51,10 +51,11 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
 {
 public:
   /* Topics will map to name/ns/goal, name/ns/result, etc */
-  GripperControllerHandle(const std::string& name, const std::string& ns)
+  GripperControllerHandle(const std::string& name, const std::string& ns, const double max_effort = 0.0)
     : ActionBasedControllerHandle<control_msgs::GripperCommandAction>(name, ns)
     , allow_failure_(false)
     , parallel_jaw_gripper_(false)
+    , max_effort_(max_effort)
   {
   }
 
@@ -110,7 +111,6 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
     // goal to be sent
     control_msgs::GripperCommandGoal goal;
     goal.command.position = 0.0;
-    goal.command.max_effort = 0.0;
 
     // send last point
     int tpoint = trajectory.joint_trajectory.points.size() - 1;
@@ -130,7 +130,13 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
       goal.command.position += trajectory.joint_trajectory.points[tpoint].positions[idx];
 
       if (trajectory.joint_trajectory.points[tpoint].effort.size() > idx)
+      {
         goal.command.max_effort = trajectory.joint_trajectory.points[tpoint].effort[idx];
+      }
+      else
+      {
+        goal.command.max_effort = max_effort_;
+      }
     }
 
     controller_action_client_->sendGoal(
@@ -202,6 +208,12 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
    */
   bool parallel_jaw_gripper_;
 
+  /*
+   * The ``max_effort`` used in the GripperCommand message when no ``max_effort`` was
+   * specified in the requested trajectory. Defaults to ``0.0``.
+   */
+  double max_effort_;
+
   /*
    * A GripperCommand message has only a single float64 for the
    * "command", thus only a single joint angle can be sent -- however,
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 37172863b2..e4e3275394 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
@@ -94,7 +94,10 @@ class MoveItSimpleControllerManager : public moveit_controller_manager::MoveItCo
         ActionBasedControllerHandleBasePtr new_handle;
         if (type == "GripperCommand")
         {
-          new_handle = std::make_shared<GripperControllerHandle>(name, action_ns);
+          const double max_effort =
+              controller_list[i].hasMember("max_effort") ? double(controller_list[i]["max_effort"]) : 0.0;
+
+          new_handle = std::make_shared<GripperControllerHandle>(name, action_ns, max_effort);
           if (static_cast<GripperControllerHandle*>(new_handle.get())->isConnected())
           {
             if (controller_list[i].hasMember("parallel"))

From 3471bc125695ebbbb96a4c862d6459d3edf1b229 Mon Sep 17 00:00:00 2001
From: JafarAbdi <cafer.abdi@gmail.com>
Date: Wed, 15 Dec 2021 17:04:05 +0300
Subject: [PATCH 036/114] Remove all remaining usage of robot_model

---
 .../src/motion_planning_display.cpp                         | 3 +++
 .../src/motion_planning_frame_joints_widget.cpp             | 2 ++
 .../rviz_plugin_render_tools/trajectory_visualization.h     | 1 +
 .../src/trajectory_visualization.cpp                        | 6 ++++++
 4 files changed, 12 insertions(+)

diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
index 9a9d65fecc..45359555fe 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
@@ -1145,11 +1145,14 @@ void MotionPlanningDisplay::clearRobotModel()
   // Invalidate all references to the RobotModel ...
   if (frame_)
     frame_->clearRobotModel();
+  if (trajectory_visual_)
+    trajectory_visual_->clearRobotModel();
   previous_state_.reset();
   query_start_state_.reset();
   query_goal_state_.reset();
   kinematics_metrics_.reset();
   robot_interaction_.reset();
+  dynamics_solver_.clear();
   // ... before calling the parent's method, which finally destroys the creating RobotModelLoader.
   PlanningSceneDisplay::clearRobotModel();
 }
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp
index fdd7f0fe9f..7c9bf7e904 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp
@@ -201,6 +201,8 @@ MotionPlanningFrameJointsWidget::~MotionPlanningFrameJointsWidget()
 void MotionPlanningFrameJointsWidget::clearRobotModel()
 {
   ui_->joints_view_->setModel(nullptr);
+  start_state_handler_.reset();
+  goal_state_handler_.reset();
   start_state_model_.reset();
   goal_state_model_.reset();
 }
diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h
index 6dad72927e..2ec0174d39 100644
--- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h
+++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h
@@ -89,6 +89,7 @@ class TrajectoryVisualization : public QObject
   virtual void reset();
 
   void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context, const ros::NodeHandle& update_nh);
+  void clearRobotModel();
   void onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model);
   void onEnable();
   void onDisable();
diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp
index 854c872312..976ba5a6cf 100644
--- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp
+++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp
@@ -605,4 +605,10 @@ void TrajectoryVisualization::trajectorySliderPanelVisibilityChange(bool enable)
     trajectory_slider_panel_->onDisable();
 }
 
+void TrajectoryVisualization::clearRobotModel()
+{
+  robot_model_.reset();
+  robot_state_.reset();
+}
+
 }  // namespace moveit_rviz_plugin

From 5ddb027492f16f714fb22d80e012372f1e262d9e Mon Sep 17 00:00:00 2001
From: Tomislav Bazina <tbazina@gmail.com>
Date: Wed, 15 Dec 2021 17:15:47 +0100
Subject: [PATCH 037/114] round_collada_numbers.py: python 2/3 compatibility
 (#2983)

Python3 requires the files to be opened in binary mode read a bytes object instead of a string, which is needed in turn by etree.parse().

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
---
 .../scripts/round_collada_numbers.py          | 19 +++----------------
 1 file changed, 3 insertions(+), 16 deletions(-)

diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py
index d72ab98cd8..a3d1bef186 100755
--- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py
+++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/round_collada_numbers.py
@@ -44,7 +44,6 @@
 from lxml import etree
 import shlex
 import sys
-import io
 
 
 def doRound(values, decimal_places):
@@ -82,19 +81,8 @@ def doRound(values, decimal_places):
     print("\nCollada Number Rounder")
     print("Rounding numbers to", decimal_places, "decimal places\n")
 
-    # Read string from file
-    f = open(input_file, "r")
-    xml = f.read()
-
-    # Parse XML
-    # doc = etree.fromstring(xml)
-    # print(doc.tag)
-    # doc = etree.parse(io.BytesIO(xml))
-    # element=doc.xpath('//ns:asset',namespaces={'ns','http://www.collada.org/2008/03/COLLADASchema'})
-    # print(element)
-
     namespace = "http://www.collada.org/2008/03/COLLADASchema"
-    dom = etree.parse(io.BytesIO(xml))
+    dom = etree.parse(input_file)
 
     # find elements of particular name
     elements = dom.xpath("//ns:translate", namespaces={"ns": namespace})
@@ -122,6 +110,5 @@ def doRound(values, decimal_places):
         elements[i].text = doRound(elements[i].text, decimal_places)
 
     # save changes
-    f = open(output_file, "w")
-    f.write(etree.tostring(dom))
-    f.close()
+    with open(output_file, "wb") as f:
+        dom.write(f, encoding="utf-8")

From c32bbd2f0481fed32bd7fbba7f0fc8df7b6f46c6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Michael=20G=C3=B6rner?= <me@v4hn.de>
Date: Tue, 21 Dec 2021 10:51:39 +0100
Subject: [PATCH 038/114] Do not assert on printTransform with non-isometry
 (#3005)

instead print a tag and the matrix
building a Quaternion from non-isometries is undefined behavior in Eigen, thus the split.
---
 moveit_core/robot_state/src/robot_state.cpp | 19 ++++++++++++++-----
 1 file changed, 14 insertions(+), 5 deletions(-)

diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
index 3d74b0a439..c870d945e8 100644
--- a/moveit_core/robot_state/src/robot_state.cpp
+++ b/moveit_core/robot_state/src/robot_state.cpp
@@ -2194,11 +2194,20 @@ void RobotState::printStateInfo(std::ostream& out) const
 
 void RobotState::printTransform(const Eigen::Isometry3d& transform, std::ostream& out) const
 {
-  ASSERT_ISOMETRY(transform)  // unsanitized input, could contain a non-isometry
-  Eigen::Quaterniond q(transform.linear());
-  out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", "
-      << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w()
-      << "]" << std::endl;
+  if (checkIsometry(transform, CHECK_ISOMETRY_PRECISION, false))
+  {
+    Eigen::Quaterniond q(transform.linear());
+    out << "T.xyz = [" << transform.translation().x() << ", " << transform.translation().y() << ", "
+        << transform.translation().z() << "], Q.xyzw = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w()
+        << "]";
+  }
+  else
+  {
+    out << "[NON-ISOMETRY] "
+        << transform.matrix().format(
+               Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "; ", "", "", "[", "]"));
+  }
+  out << std::endl;
 }
 
 void RobotState::printTransforms(std::ostream& out) const

From 6a46aa0f0cdee6bda87fca8c0b93fd1d1d0adc6d Mon Sep 17 00:00:00 2001
From: v4hn <me@v4hn.de>
Date: Wed, 15 Dec 2021 11:28:53 +0100
Subject: [PATCH 039/114] add pilz planner to moveit_planners dependency

It should have been there for quite some time now...
---
 moveit_planners/moveit_planners/package.xml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml
index 4d366b59e1..84d8c332db 100644
--- a/moveit_planners/moveit_planners/package.xml
+++ b/moveit_planners/moveit_planners/package.xml
@@ -19,6 +19,7 @@
   <exec_depend>chomp_motion_planner</exec_depend>
   <exec_depend>moveit_planners_chomp</exec_depend>
   <exec_depend>moveit_planners_ompl</exec_depend>
+  <exec_depend>pilz_industrial_motion_planner</exec_depend>
 
   <export>
     <metapackage/>

From f6dffc43a505e6127cf465daceec3e193f368402 Mon Sep 17 00:00:00 2001
From: v4hn <me@v4hn.de>
Date: Wed, 15 Dec 2021 11:28:24 +0100
Subject: [PATCH 040/114] pilz: report joint name with invalid limits in start
 state

- remove unused utility method
  it does not provide enough feedback, is almost trivial and does redundant checks in the single case it's called from.
---
 .../joint_limits_container.h                  |  9 ---------
 .../src/joint_limits_container.cpp            | 19 -------------------
 .../src/trajectory_generator.cpp              | 18 +++++++++++++++---
 .../test/unittest_joint_limits_container.cpp  | 19 -------------------
 4 files changed, 15 insertions(+), 50 deletions(-)

diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h
index 7a91ffe545..5543dea2d4 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/joint_limits_container.h
@@ -140,15 +140,6 @@ class JointLimitsContainer
    */
   bool verifyPositionLimit(const std::string& joint_name, const double& joint_position) const;
 
-  /**
-   * @brief verify position limits of multiple joints
-   * @param joint_names
-   * @param joint_positions
-   * @return
-   */
-  bool verifyPositionLimits(const std::vector<std::string>& joint_names,
-                            const std::vector<double>& joint_positions) const;
-
 private:
   /**
    * @brief update the most strict limit with given joint limit
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 7bdb1d983f..d5a6a142e8 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
@@ -117,25 +117,6 @@ bool JointLimitsContainer::verifyPositionLimit(const std::string& joint_name, co
             (joint_position < getLimit(joint_name).min_position || joint_position > getLimit(joint_name).max_position)));
 }
 
-bool JointLimitsContainer::verifyPositionLimits(const std::vector<std::string>& joint_names,
-                                                const std::vector<double>& joint_positions) const
-{
-  if (joint_names.size() != joint_positions.size())
-  {
-    throw std::out_of_range("joint_names vector has a different size than joint_positions vector.");
-  }
-
-  for (std::size_t i = 0; i < joint_names.size(); ++i)
-  {
-    if (!verifyPositionLimit(joint_names.at(i), joint_positions.at(i)))
-    {
-      return false;
-    }
-  }
-
-  return true;
-}
-
 void JointLimitsContainer::updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit)
 {
   // check position limits
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 aebdfaf90d..4626eab04c 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
@@ -36,6 +36,8 @@
 
 #include <cassert>
 
+#include <boost/range/combine.hpp>
+
 #include <kdl/velocityprofile_trap.hpp>
 #include <moveit/robot_state/conversions.h>
 
@@ -93,10 +95,20 @@ void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_s
     throw SizeMismatchInStartState("Joint state name and position do not match in start state");
   }
 
-  if (!planner_limits_.getJointLimitContainer().verifyPositionLimits(start_state.joint_state.name,
-                                                                     start_state.joint_state.position))
+  // verify joint position limits
+  const JointLimitsContainer& limits{ planner_limits_.getJointLimitContainer() };
+  std::string error_msg;
+  for (auto joint : boost::combine(start_state.joint_state.name, start_state.joint_state.position))
+  {
+    if (!limits.verifyPositionLimit(joint.get<0>(), joint.get<1>()))
+    {
+      error_msg.append(error_msg.empty() ? "start state joints outside their position limits: " : ", ");
+      error_msg.append(joint.get<0>());
+    }
+  }
+  if (!error_msg.empty())
   {
-    throw JointsOfStartStateOutOfRange("Joint state out of range in start state");
+    throw JointsOfStartStateOutOfRange(error_msg);
   }
 
   // does not allow start velocity
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp
index 2f7f7b80a4..b46e73d628 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_joint_limits_container.cpp
@@ -203,25 +203,6 @@ TEST_F(JointLimitsContainerTest, FirstPositionEmpty)
   EXPECT_EQ(-1, limits.min_position);
 }
 
-/**
- * @brief Check position limits
- */
-TEST_F(JointLimitsContainerTest, CheckVerifyPositionLimits)
-{
-  // positive check: inside limits
-  std::vector<std::string> joint_names{ "joint1", "joint2" };
-  std::vector<double> joint_positions{ 0.5, 0.5 };
-  EXPECT_TRUE(container_.verifyPositionLimits(joint_names, joint_positions));
-
-  // outside limit2
-  joint_positions[1] = 7;
-  EXPECT_FALSE(container_.verifyPositionLimits(joint_names, joint_positions));
-
-  // invalid size
-  std::vector<double> joint_positions1{ 0. };
-  EXPECT_THROW(container_.verifyPositionLimits(joint_names, joint_positions1), std::out_of_range);
-}
-
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);

From 38395dce8f1b6ee7f7d3308d010cc01634d2e3d0 Mon Sep 17 00:00:00 2001
From: v4hn <me@v4hn.de>
Date: Wed, 15 Dec 2021 12:39:59 +0100
Subject: [PATCH 041/114] pilz: restrict start state check to active group

---
 .../trajectory_generator.h                    |  7 +++-
 .../src/trajectory_generator.cpp              | 34 ++++++++++++++++---
 2 files changed, 36 insertions(+), 5 deletions(-)

diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h
index 4cb7d261ea..bf6b1598ed 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h
@@ -223,7 +223,7 @@ class TrajectoryGenerator
    *     - The start state velocity is below
    * TrajectoryGenerator::VELOCITY_TOLERANCE
    */
-  void checkStartState(const moveit_msgs::RobotState& start_state) const;
+  void checkStartState(const moveit_msgs::RobotState& start_state, const std::string& group) const;
 
   void checkGoalConstraints(const moveit_msgs::MotionPlanRequest::_goal_constraints_type& goal_constraints,
                             const std::vector<std::string>& expected_joint_names, const std::string& group_name) const;
@@ -235,6 +235,11 @@ class TrajectoryGenerator
   void checkCartesianGoalConstraint(const moveit_msgs::Constraints& constraint, const std::string& group_name) const;
 
 private:
+  /**
+   * @return joint state message including only active joints in group
+   */
+  sensor_msgs::JointState filterGroupValues(const sensor_msgs::JointState& robot_state, const std::string& group) const;
+
   /**
    * @return True if scaling factor is valid, otherwise false.
    */
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 4626eab04c..5bc83360a6 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
@@ -45,6 +45,30 @@
 
 namespace pilz_industrial_motion_planner
 {
+sensor_msgs::JointState TrajectoryGenerator::filterGroupValues(const sensor_msgs::JointState& robot_state,
+                                                               const std::string& group) const
+{
+  const std::vector<std::string>& group_joints{ robot_model_->getJointModelGroup(group)->getActiveJointModelNames() };
+  sensor_msgs::JointState group_state;
+  group_state.name.reserve(group_joints.size());
+  group_state.position.reserve(group_joints.size());
+  group_state.velocity.reserve(group_joints.size());
+
+  for (size_t i = 0; i < robot_state.name.size(); ++i)
+  {
+    if (std::find(group_joints.begin(), group_joints.end(), robot_state.name.at(i)) != group_joints.end())
+    {
+      group_state.name.push_back(robot_state.name.at(i));
+      group_state.position.push_back(robot_state.position.at(i));
+      if (i < robot_state.velocity.size())
+      {
+        group_state.velocity.push_back(robot_state.velocity.at(i));
+      }
+    }
+  }
+  return group_state;
+}
+
 void TrajectoryGenerator::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& /*req*/) const
 {
   // Empty implementation, in case the derived class does not want
@@ -83,7 +107,7 @@ void TrajectoryGenerator::checkForValidGroupName(const std::string& group_name)
   }
 }
 
-void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_state) const
+void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_state, const std::string& group) const
 {
   if (start_state.joint_state.name.empty())
   {
@@ -95,10 +119,12 @@ void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_s
     throw SizeMismatchInStartState("Joint state name and position do not match in start state");
   }
 
+  sensor_msgs::JointState group_start_state{ filterGroupValues(start_state.joint_state, group) };
+
   // verify joint position limits
   const JointLimitsContainer& limits{ planner_limits_.getJointLimitContainer() };
   std::string error_msg;
-  for (auto joint : boost::combine(start_state.joint_state.name, start_state.joint_state.position))
+  for (auto joint : boost::combine(group_start_state.name, group_start_state.position))
   {
     if (!limits.verifyPositionLimit(joint.get<0>(), joint.get<1>()))
     {
@@ -112,7 +138,7 @@ void TrajectoryGenerator::checkStartState(const moveit_msgs::RobotState& start_s
   }
 
   // does not allow start velocity
-  if (!std::all_of(start_state.joint_state.velocity.begin(), start_state.joint_state.velocity.end(),
+  if (!std::all_of(group_start_state.velocity.begin(), group_start_state.velocity.end(),
                    [this](double v) { return std::fabs(v) < this->VELOCITY_TOLERANCE; }))
   {
     throw NonZeroVelocityInStartState("Trajectory Generator does not allow non-zero start velocity");
@@ -222,7 +248,7 @@ void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRe
   checkVelocityScaling(req.max_velocity_scaling_factor);
   checkAccelerationScaling(req.max_acceleration_scaling_factor);
   checkForValidGroupName(req.group_name);
-  checkStartState(req.start_state);
+  checkStartState(req.start_state, req.group_name);
   checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name);
 }
 

From 7bb9d12bbd0dcbcca4dc442adb532fe71721d72d Mon Sep 17 00:00:00 2001
From: Henning Kayser <henningkayser@picknik.ai>
Date: Thu, 11 Nov 2021 17:08:33 +0000
Subject: [PATCH 042/114] Add API stress tests for TOTG

---
 .../trajectory_processing/CMakeLists.txt      |   2 +-
 ...est_time_optimal_trajectory_generation.cpp | 121 ++++++++++++++++++
 2 files changed, 122 insertions(+), 1 deletion(-)

diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt
index e595724de0..3ff7fe4a16 100644
--- a/moveit_core/trajectory_processing/CMakeLists.txt
+++ b/moveit_core/trajectory_processing/CMakeLists.txt
@@ -22,5 +22,5 @@ if(CATKIN_ENABLE_TESTING)
   catkin_add_gtest(test_time_parameterization test/test_time_parameterization.cpp)
   target_link_libraries(test_time_parameterization moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME})
   catkin_add_gtest(test_time_optimal_trajectory_generation test/test_time_optimal_trajectory_generation.cpp)
-  target_link_libraries(test_time_optimal_trajectory_generation ${catkin_LIBRARIES} ${console_bridge_LIBRARIES} ${MOVEIT_LIB_NAME})
+  target_link_libraries(test_time_optimal_trajectory_generation moveit_test_utils ${catkin_LIBRARIES} ${console_bridge_LIBRARIES} ${MOVEIT_LIB_NAME})
 endif()
diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
index 7413afac44..a504f7d45f 100644
--- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
+++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
@@ -38,8 +38,10 @@
 
 #include <gtest/gtest.h>
 #include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
+#include <moveit/utils/robot_model_test_utils.h>
 
 using trajectory_processing::Path;
+using trajectory_processing::TimeOptimalTrajectoryGeneration;
 using trajectory_processing::Trajectory;
 
 TEST(time_optimal_trajectory_generation, test1)
@@ -227,6 +229,125 @@ TEST(time_optimal_trajectory_generation, testLargeAccel)
   }
 }
 
+TEST(time_optimal_trajectory_generation, testPluginAPI)
+{
+  constexpr auto robot_name{ "panda" };
+  constexpr auto group_name{ "panda_arm" };
+
+  auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
+  ASSERT_TRUE((bool)robot_model) << "Failed to load robot model" << robot_name;
+  auto group = robot_model->getJointModelGroup(group_name);
+  ASSERT_TRUE((bool)group) << "Failed to load joint model group " << group_name;
+  moveit::core::RobotState waypoint_state(robot_model);
+  waypoint_state.setToDefaultValues();
+
+  robot_trajectory::RobotTrajectory trajectory(robot_model, group);
+  waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 });
+  trajectory.addSuffixWayPoint(waypoint_state, 0.1);
+  waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 });
+  trajectory.addSuffixWayPoint(waypoint_state, 0.1);
+  waypoint_state.setJointGroupPositions(group, std::vector<double>{ 0.0, -3.5, 1.4, -1.2, -1.0, -0.2, 0.0 });
+  trajectory.addSuffixWayPoint(waypoint_state, 0.1);
+  waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 });
+  trajectory.addSuffixWayPoint(waypoint_state, 0.1);
+  waypoint_state.setJointGroupPositions(group, std::vector<double>{ 0.0, -3.5, 1.4, -1.2, -1.0, -0.2, 0.0 });
+  trajectory.addSuffixWayPoint(waypoint_state, 0.1);
+  waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 });
+  trajectory.addSuffixWayPoint(waypoint_state, 0.1);
+
+  // Number TOTG iterations
+  constexpr size_t totg_iterations = 10;
+
+  // Test computing the dynamics repeatedly with the same totg instance
+  moveit_msgs::RobotTrajectory first_trajectory_msg_start, first_trajectory_msg_end;
+  {
+    robot_trajectory::RobotTrajectory test_trajectory(trajectory, true /* deep copy */);
+
+    // Test if the trajectory was copied correctly
+    ASSERT_EQ(test_trajectory.getDuration(), trajectory.getDuration());
+    moveit::core::JointBoundsVector test_bounds = test_trajectory.getRobotModel()->getActiveJointModelsBounds();
+    moveit::core::JointBoundsVector original_bounds = trajectory.getRobotModel()->getActiveJointModelsBounds();
+    ASSERT_EQ(test_bounds.size(), original_bounds.size());
+    for (size_t bound_idx = 0; bound_idx < test_bounds.at(0)->size(); ++bound_idx)
+    {
+      ASSERT_EQ(test_bounds.at(0)->at(bound_idx).max_velocity_, original_bounds.at(0)->at(bound_idx).max_velocity_);
+      ASSERT_EQ(test_bounds.at(0)->at(bound_idx).min_velocity_, original_bounds.at(0)->at(bound_idx).min_velocity_);
+      ASSERT_EQ(test_bounds.at(0)->at(bound_idx).velocity_bounded_,
+                original_bounds.at(0)->at(bound_idx).velocity_bounded_);
+
+      ASSERT_EQ(test_bounds.at(0)->at(bound_idx).max_acceleration_,
+                original_bounds.at(0)->at(bound_idx).max_acceleration_);
+      ASSERT_EQ(test_bounds.at(0)->at(bound_idx).min_acceleration_,
+                original_bounds.at(0)->at(bound_idx).min_acceleration_);
+      ASSERT_EQ(test_bounds.at(0)->at(bound_idx).acceleration_bounded_,
+                original_bounds.at(0)->at(bound_idx).acceleration_bounded_);
+    }
+    ASSERT_EQ(test_trajectory.getWayPointDurationFromPrevious(1), trajectory.getWayPointDurationFromPrevious(1));
+
+    TimeOptimalTrajectoryGeneration totg;
+    ASSERT_TRUE(totg.computeTimeStamps(test_trajectory)) << "Failed to compute time stamps";
+
+    test_trajectory.getRobotTrajectoryMsg(first_trajectory_msg_start);
+
+    // Iteratively recompute timestamps with same totg instance
+    for (size_t i = 0; i < totg_iterations; ++i)
+    {
+      bool totg_success = totg.computeTimeStamps(test_trajectory);
+      EXPECT_TRUE(totg_success) << "Failed to compute time stamps with a same TOTG instance in iteration " << i;
+    }
+
+    test_trajectory.getRobotTrajectoryMsg(first_trajectory_msg_end);
+  }
+
+  // Test computing the dynamics repeatedly with one TOTG instance per call
+  moveit_msgs::RobotTrajectory second_trajectory_msg_start, second_trajectory_msg_end;
+  {
+    robot_trajectory::RobotTrajectory test_trajectory(trajectory, true /* deep copy */);
+
+    {
+      TimeOptimalTrajectoryGeneration totg;
+      ASSERT_TRUE(totg.computeTimeStamps(test_trajectory)) << "Failed to compute time stamps";
+    }
+
+    test_trajectory.getRobotTrajectoryMsg(second_trajectory_msg_start);
+
+    // Iteratively recompute timestamps with new totg instances
+    for (size_t i = 0; i < totg_iterations; ++i)
+    {
+      TimeOptimalTrajectoryGeneration totg;
+      bool totg_success = totg.computeTimeStamps(test_trajectory);
+      EXPECT_TRUE(totg_success) << "Failed to compute time stamps with a new TOTG instance in iteration " << i;
+    }
+
+    test_trajectory.getRobotTrajectoryMsg(second_trajectory_msg_end);
+  }
+
+  // Make sure trajectories produce equal waypoints independent of TOTG instances
+  ASSERT_EQ(first_trajectory_msg_start, second_trajectory_msg_start);
+  ASSERT_EQ(first_trajectory_msg_end, second_trajectory_msg_end);
+
+  // Iterate on the original trajectory again
+  moveit_msgs::RobotTrajectory third_trajectory_msg_end;
+
+  {
+    TimeOptimalTrajectoryGeneration totg;
+    ASSERT_TRUE(totg.computeTimeStamps(trajectory)) << "Failed to compute time stamps";
+  }
+
+  for (size_t i = 0; i < totg_iterations; ++i)
+  {
+    TimeOptimalTrajectoryGeneration totg;
+    bool totg_success = totg.computeTimeStamps(trajectory);
+    ASSERT_TRUE(totg_success) << "Failed to compute timestamps on a new TOTG instance in iteration " << i;
+  }
+
+  // Compare with previous work
+  trajectory.getRobotTrajectoryMsg(third_trajectory_msg_end);
+
+  // Make sure trajectories produce equal waypoints independent of TOTG instances
+  ASSERT_EQ(first_trajectory_msg_end, third_trajectory_msg_end);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);

From 2827369b901425dbd2b0f99a6ec7621d9b204652 Mon Sep 17 00:00:00 2001
From: Jochen Sprickerhof <git@jochen.sprickerhof.de>
Date: Wed, 17 Nov 2021 09:47:48 +0100
Subject: [PATCH 043/114] TOTG: catch division by 0

This bug is already in the original implementation:

https://github.com/tobiaskunz/trajectories/blob/master/Path.cpp

In case the dot product between the two vectors is close to +/-1,
angle becomes +/-PI and cos/tan of 0.5 * PI in the lines below will
produce a division by 0.

This happens easily if a optimal trajectory is processed by TOTG, i.e.,
if a trajectory is processed by TOTG twice.
---
 .../src/time_optimal_trajectory_generation.cpp             | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
index 299d080905..15aff331cd 100644
--- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
+++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -106,8 +106,10 @@ class CircularPathSegment : public PathSegment
 
     const Eigen::VectorXd start_direction = (intersection - start).normalized();
     const Eigen::VectorXd end_direction = (end - intersection).normalized();
+    const double start_dot_end = start_direction.dot(end_direction);
 
-    if ((start_direction - end_direction).norm() < 0.000001)
+    // catch division by 0 in computations below
+    if (start_dot_end > 0.999999 || start_dot_end < -0.999999)
     {
       length_ = 0.0;
       radius = 1.0;
@@ -117,8 +119,7 @@ class CircularPathSegment : public PathSegment
       return;
     }
 
-    // directions must be different at this point so angle is always non-zero
-    const double angle = acos(std::max(-1.0, start_direction.dot(end_direction)));
+    const double angle = acos(start_dot_end);
     const double start_distance = (start - intersection).norm();
     const double end_distance = (end - intersection).norm();
 

From a25515b73d682df03ed3eccd839110c296aa79fc Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Thu, 23 Dec 2021 11:56:30 -0500
Subject: [PATCH 044/114] Fix MoveGroupInterface uninitialized RobotState
 (#3008)

---
 .../move_group_interface/src/move_group_interface.cpp          | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
index 81db35e044..7017f4bb54 100644
--- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
+++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
@@ -1597,7 +1597,10 @@ void MoveGroupInterface::setStartState(const moveit_msgs::RobotState& start_stat
   if (start_state.is_diff)
     impl_->getCurrentState(rs);
   else
+  {
     rs = std::make_shared<moveit::core::RobotState>(getRobotModel());
+    rs->setToDefaultValues();  // initialize robot state values for conversion
+  }
   moveit::core::robotStateMsgToRobotState(start_state, *rs);
   setStartState(*rs);
 }

From dffc178a754d5bae3b99a19e2b45c6104994df22 Mon Sep 17 00:00:00 2001
From: pvanlaar <22322118+pvanlaar@users.noreply.github.com>
Date: Tue, 28 Dec 2021 08:46:24 +0100
Subject: [PATCH 045/114] RobotState::attachBody: Migrate to unique_ptr
 argument (#3011)

... to indicate transfer of ownership and simplify pointer handling
---
 MIGRATION.md                                  |  1 +
 .../test/test_collision_distance_field.cpp    | 10 +--
 .../include/moveit/robot_state/robot_state.h  | 24 +++--
 moveit_core/robot_state/src/robot_state.cpp   | 88 +++++++++----------
 .../robot_state/test/robot_state_test.cpp     |  8 +-
 .../test/test_kinematic_complex.cpp           | 20 ++---
 .../src/motion_planning_frame_objects.cpp     |  9 +-
 7 files changed, 81 insertions(+), 79 deletions(-)

diff --git a/MIGRATION.md b/MIGRATION.md
index 432fafe365..6d275e5871 100644
--- a/MIGRATION.md
+++ b/MIGRATION.md
@@ -28,6 +28,7 @@ API changes in MoveIt releases
 - Removed deprecated header `moveit/macros/deprecation.h`. Use `[[deprecated]]` instead.
 - All uses of `MOVEIT_CLASS_FORWARD` et. al. must now be followed by a semicolon for consistency (and to get -pedantic builds to pass for the codebase).
 - In case you start RViz in a namespace, the default topic for the trajectory visualization display now uses the relative instead of the absolute namespace (i.e. `<ns>/move_group/display_planned_path` instead of `/move_group/display_planned_path`).
+- `RobotState::attachBody()` now takes a unique_ptr instead of an owning raw pointer.
 
 ## ROS Melodic
 
diff --git a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp
index 663911659c..991418cb2d 100644
--- a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp
+++ b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp
@@ -299,10 +299,9 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester)
   poses.push_back(identity);
   std::set<std::string> touch_links;
   trajectory_msgs::JointTrajectory empty_state;
-  moveit::core::AttachedBody* attached_body = new moveit::core::AttachedBody(
-      robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state);
 
-  robot_state.attachBody(attached_body);
+  robot_state.attachBody(std::make_unique<moveit::core::AttachedBody>(
+      robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state));
 
   res = collision_detection::CollisionResult();
   cenv_->checkSelfCollision(req, res, robot_state, *acm_);
@@ -314,9 +313,8 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester)
   touch_links.insert("r_gripper_palm_link");
   shapes[0] = std::make_shared<shapes::Box>(.1, .1, .1);
 
-  moveit::core::AttachedBody* attached_body_1 = new moveit::core::AttachedBody(
-      robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state);
-  robot_state.attachBody(attached_body_1);
+  robot_state.attachBody(std::make_unique<moveit::core::AttachedBody>(
+      robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state));
 
   res = collision_detection::CollisionResult();
   cenv_->checkSelfCollision(req, res, robot_state, *acm_);
diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h
index dd8859dbad..2998821eb0 100644
--- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h
+++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h
@@ -1068,12 +1068,12 @@ class RobotState
       @param timeout The timeout passed to the kinematics solver on each attempt
       @param constraint A state validity constraint to be required for IK solutions */
   bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
-                 const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
+                 const std::vector<std::string>& tips, const std::vector<std::vector<double>>& consistency_limits,
                  double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
                  const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
   [[deprecated("The attempts argument is not supported anymore.")]] bool
   setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
-            const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
+            const std::vector<std::string>& tips, const std::vector<std::vector<double>>& consistency_limits,
             unsigned int /* attempts */, double timeout = 0.0,
             const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
             const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions())
@@ -1091,12 +1091,12 @@ class RobotState
       @param constraint A state validity constraint to be required for IK solutions */
   bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
                           const std::vector<std::string>& tips,
-                          const std::vector<std::vector<double> >& consistency_limits, double timeout = 0.0,
+                          const std::vector<std::vector<double>>& consistency_limits, double timeout = 0.0,
                           const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
                           const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions());
   [[deprecated("The attempts argument is not supported anymore.")]] bool
   setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
-                     const std::vector<std::string>& tips, const std::vector<std::vector<double> >& consistency_limits,
+                     const std::vector<std::string>& tips, const std::vector<std::vector<double>>& consistency_limits,
                      unsigned int /* attempts */, double timeout = 0.0,
                      const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
                      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions())
@@ -1628,6 +1628,18 @@ class RobotState
    *  @{
    */
 
+  /** \brief Add an attached body to this state.
+   *
+   * This only adds the given body to this RobotState
+   * instance.  It does not change anything about other
+   * representations of the object elsewhere in the system.  So if the
+   * body represents an object in a collision_detection::World (like
+   * from a planning_scene::PlanningScene), you will likely need to remove the
+   * corresponding object from that world to avoid having collisions
+   * detected against it.
+   **/
+  void attachBody(std::unique_ptr<AttachedBody> attached_body);
+
   /** \brief Add an attached body to this state. Ownership of the
    * memory for the attached body is assumed by the state.
    *
@@ -1647,7 +1659,7 @@ class RobotState
    * the body positions will get corrupted.  You need to make a fresh
    * copy of the AttachedBody object for each RobotState you attach it
    * to.*/
-  void attachBody(AttachedBody* attached_body);
+  [[deprecated("Deprecated. Pass a unique_ptr instead")]] void attachBody(AttachedBody* attached_body);
 
   /** @brief Add an attached body to a link
    * @param id The string id associated with the attached body
@@ -1951,7 +1963,7 @@ class RobotState
   unsigned char* dirty_joint_transforms_;
 
   /** \brief All attached bodies that are part of this state, indexed by their name */
-  std::map<std::string, AttachedBody*> attached_body_map_;
+  std::map<std::string, std::unique_ptr<AttachedBody>> attached_body_map_;
 
   /** \brief This event is called when there is a change in the attached bodies for this state;
       The event specifies the body that changed and whether it was just attached or about to be detached. */
diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
index c870d945e8..8ac0e1561a 100644
--- a/moveit_core/robot_state/src/robot_state.cpp
+++ b/moveit_core/robot_state/src/robot_state.cpp
@@ -176,8 +176,8 @@ void RobotState::copyFrom(const RobotState& other)
 
   // copy attached bodies
   clearAttachedBodies();
-  for (const std::pair<const std::string, AttachedBody*>& it : other.attached_body_map_)
-    attachBody(new AttachedBody(*it.second));
+  for (const auto& attached_body : other.attached_body_map_)
+    attachBody(std::make_unique<AttachedBody>(*attached_body.second));
 }
 
 bool RobotState::checkJointTransforms(const JointModel* joint) const
@@ -740,9 +740,9 @@ void RobotState::updateLinkTransformsInternal(const JointModel* start)
   }
 
   // update attached bodies tf; these are usually very few, so we update them all
-  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
-       it != attached_body_map_.end(); ++it)
-    it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]);
+  for (const auto& attached_body : attached_body_map_)
+    attached_body.second->computeTransform(
+        global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]);
 }
 
 void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward)
@@ -792,9 +792,9 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome
   }
 
   // update attached bodies tf; these are usually very few, so we update them all
-  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
-       it != attached_body_map_.end(); ++it)
-    it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]);
+  for (const auto& attached_body : attached_body_map_)
+    attached_body.second->computeTransform(
+        global_link_transforms_[attached_body.second->getAttachedLink()->getLinkIndex()]);
 }
 
 const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const
@@ -978,25 +978,30 @@ bool RobotState::hasAttachedBody(const std::string& id) const
 
 const AttachedBody* RobotState::getAttachedBody(const std::string& id) const
 {
-  std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.find(id);
+  const auto it = attached_body_map_.find(id);
   if (it == attached_body_map_.end())
   {
     ROS_ERROR_NAMED(LOGNAME, "Attached body '%s' not found", id.c_str());
     return nullptr;
   }
   else
-    return it->second;
+    return it->second.get();
 }
 
-void RobotState::attachBody(AttachedBody* attached_body)
+void RobotState::attachBody(std::unique_ptr<AttachedBody> attached_body)
 {
   // If an attached body with the same id exists, remove it
   clearAttachedBody(attached_body->getName());
 
-  attached_body_map_[attached_body->getName()] = attached_body;
   attached_body->computeTransform(getGlobalLinkTransform(attached_body->getAttachedLink()));
   if (attached_body_update_callback_)
-    attached_body_update_callback_(attached_body, true);
+    attached_body_update_callback_(attached_body.get(), true);
+  attached_body_map_[attached_body->getName()] = std::move(attached_body);
+}
+
+void RobotState::attachBody(AttachedBody* attached_body)
+{
+  attachBody(std::unique_ptr<AttachedBody>(attached_body));
 }
 
 void RobotState::attachBody(const std::string& id, const Eigen::Isometry3d& pose,
@@ -1005,90 +1010,81 @@ void RobotState::attachBody(const std::string& id, const Eigen::Isometry3d& pose
                             const std::string& link, const trajectory_msgs::JointTrajectory& detach_posture,
                             const moveit::core::FixedTransformsMap& subframe_poses)
 {
-  const LinkModel* l = robot_model_->getLinkModel(link);
-  attachBody(new AttachedBody(l, id, pose, shapes, shape_poses, touch_links, detach_posture, subframe_poses));
+  attachBody(std::make_unique<AttachedBody>(robot_model_->getLinkModel(link), id, pose, shapes, shape_poses,
+                                            touch_links, detach_posture, subframe_poses));
 }
 
 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const
 {
   attached_bodies.clear();
   attached_bodies.reserve(attached_body_map_.size());
-  for (const std::pair<const std::string, AttachedBody*>& it : attached_body_map_)
-    attached_bodies.push_back(it.second);
+  for (const auto& it : attached_body_map_)
+    attached_bodies.push_back(it.second.get());
 }
 
 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const
 {
   attached_bodies.clear();
-  for (const std::pair<const std::string, AttachedBody*>& it : attached_body_map_)
+  for (const auto& it : attached_body_map_)
     if (group->hasLinkModel(it.second->getAttachedLinkName()))
-      attached_bodies.push_back(it.second);
+      attached_bodies.push_back(it.second.get());
 }
 
 void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* link_model) const
 {
   attached_bodies.clear();
-  for (const std::pair<const std::string, AttachedBody*>& it : attached_body_map_)
+  for (const auto& it : attached_body_map_)
     if (it.second->getAttachedLink() == link_model)
-      attached_bodies.push_back(it.second);
+      attached_bodies.push_back(it.second.get());
 }
 
 void RobotState::clearAttachedBodies()
 {
-  for (std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.begin();
-       it != attached_body_map_.end(); ++it)
+  for (const auto& it : attached_body_map_)
   {
     if (attached_body_update_callback_)
-      attached_body_update_callback_(it->second, false);
-    delete it->second;
+      attached_body_update_callback_(it.second.get(), false);
   }
   attached_body_map_.clear();
 }
 
 void RobotState::clearAttachedBodies(const LinkModel* link)
 {
-  std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.begin();
-  while (it != attached_body_map_.end())
+  for (auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
   {
     if (it->second->getAttachedLink() != link)
     {
-      ++it;
       continue;
     }
     if (attached_body_update_callback_)
-      attached_body_update_callback_(it->second, false);
-    delete it->second;
-    std::map<std::string, AttachedBody*>::iterator del = it++;
+      attached_body_update_callback_(it->second.get(), false);
+    const auto del = it++;
     attached_body_map_.erase(del);
   }
 }
 
 void RobotState::clearAttachedBodies(const JointModelGroup* group)
 {
-  std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.begin();
-  while (it != attached_body_map_.end())
+  for (auto it = attached_body_map_.cbegin(); it != attached_body_map_.cend(); ++it)
   {
     if (!group->hasLinkModel(it->second->getAttachedLinkName()))
     {
-      ++it;
       continue;
     }
     if (attached_body_update_callback_)
-      attached_body_update_callback_(it->second, false);
-    delete it->second;
-    std::map<std::string, AttachedBody*>::iterator del = it++;
+      attached_body_update_callback_(it->second.get(), false);
+    const auto del = it++;
     attached_body_map_.erase(del);
   }
 }
 
 bool RobotState::clearAttachedBody(const std::string& id)
 {
-  std::map<std::string, AttachedBody*>::iterator it = attached_body_map_.find(id);
+  const auto it = attached_body_map_.find(id);
   if (it != attached_body_map_.end())
   {
     if (attached_body_update_callback_)
-      attached_body_update_callback_(it->second, false);
-    delete it->second;
+      attached_body_update_callback_(it->second.get(), false);
     attached_body_map_.erase(it);
     return true;
   }
@@ -1137,7 +1133,7 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c
   robot_link = nullptr;
 
   // Check names of the attached bodies
-  std::map<std::string, AttachedBody*>::const_iterator jt = attached_body_map_.find(frame_id);
+  const auto jt = attached_body_map_.find(frame_id);
   if (jt != attached_body_map_.end())
   {
     const Eigen::Isometry3d& transform = jt->second->getGlobalPose();
@@ -1148,7 +1144,7 @@ const Eigen::Isometry3d& RobotState::getFrameInfo(const std::string& frame_id, c
   }
 
   // Check if an AttachedBody has a subframe with name frame_id
-  for (const std::pair<const std::string, AttachedBody*>& body : attached_body_map_)
+  for (const auto& body : attached_body_map_)
   {
     const Eigen::Isometry3d& transform = body.second->getGlobalSubframeTransform(frame_id, &frame_found);
     if (frame_found)
@@ -1172,12 +1168,12 @@ bool RobotState::knowsFrameTransform(const std::string& frame_id) const
     return true;
 
   // Check if an AttachedBody with name frame_id exists
-  std::map<std::string, AttachedBody*>::const_iterator it = attached_body_map_.find(frame_id);
+  const auto it = attached_body_map_.find(frame_id);
   if (it != attached_body_map_.end())
     return !it->second->getGlobalCollisionBodyTransforms().empty();
 
   // Check if an AttachedBody has a subframe with name frame_id
-  for (const std::pair<const std::string, AttachedBody*>& body : attached_body_map_)
+  for (const auto& body : attached_body_map_)
   {
     if (body.second->hasSubframeTransform(frame_id))
       return true;
@@ -1212,7 +1208,7 @@ void RobotState::getRobotMarkers(visualization_msgs::MarkerArray& arr, const std
     if (!link_model)
       continue;
     if (include_attached)
-      for (const std::pair<const std::string, AttachedBody*>& it : attached_body_map_)
+      for (const auto& it : attached_body_map_)
         if (it.second->getAttachedLink() == link_model)
         {
           for (std::size_t j = 0; j < it.second->getShapes().size(); ++j)
@@ -2055,7 +2051,7 @@ void RobotState::computeAABB(std::vector<double>& aabb) const
     transform.translate(link->getCenteredBoundingBoxOffset());
     bounding_box.extendWithTransformedBox(transform, extents);
   }
-  for (const std::pair<const std::string, AttachedBody*>& it : attached_body_map_)
+  for (const auto& it : attached_body_map_)
   {
     const EigenSTL::vector_Isometry3d& transforms = it.second->getGlobalCollisionBodyTransforms();
     const std::vector<shapes::ShapeConstPtr>& shapes = it.second->getShapes();
diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp
index 7313a94aed..f156275ce2 100644
--- a/moveit_core/robot_state/test/robot_state_test.cpp
+++ b/moveit_core/robot_state/test/robot_state_test.cpp
@@ -170,10 +170,8 @@ TEST(LoadingAndFK, SimpleRobot)
 
   state.setVariableAcceleration("base_joint/x", 0.0);
 
-  // making sure that values get copied
-  moveit::core::RobotState* new_state = new moveit::core::RobotState(state);
+  const auto new_state = std::make_unique<moveit::core::RobotState>(state);  // making sure that values get copied
   EXPECT_NEAR_TRACED(state.getGlobalLinkTransform("base_link").translation(), Eigen::Vector3d(10, 8, 0));
-  delete new_state;
 
   std::vector<double> jv(state.getVariableCount(), 0.0);
   jv[state.getRobotModel()->getVariableIndex("base_joint/x")] = 5.0;
@@ -364,7 +362,7 @@ class OneRobot : public testing::Test
         "</robot>";
 
     urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
-    srdf::ModelSharedPtr srdf_model(new srdf::Model());
+    srdf::ModelSharedPtr srdf_model = std::make_shared<srdf::Model>();
     srdf_model->initString(*urdf_model, SMODEL2);
     robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
   }
@@ -692,7 +690,7 @@ TEST_F(OneRobot, rigidlyConnectedParent)
   EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a);
 
   // attach "object" with "subframe" to link_b
-  state.attachBody(new moveit::core::AttachedBody(
+  state.attachBody(std::make_unique<moveit::core::AttachedBody>(
       link_b, "object", Eigen::Isometry3d::Identity(), std::vector<shapes::ShapeConstPtr>{},
       EigenSTL::vector_Isometry3d{}, std::set<std::string>{}, trajectory_msgs::JointTrajectory{},
       moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d::Identity() } }));
diff --git a/moveit_core/robot_state/test/test_kinematic_complex.cpp b/moveit_core/robot_state/test/test_kinematic_complex.cpp
index 4d8ba22afe..b95a1ccb57 100644
--- a/moveit_core/robot_state/test/test_kinematic_complex.cpp
+++ b/moveit_core/robot_state/test/test_kinematic_complex.cpp
@@ -243,9 +243,9 @@ TEST_F(LoadPlanningModelsPr2, FullTest)
   std::set<std::string> touch_links;
 
   trajectory_msgs::JointTrajectory empty_state;
-  moveit::core::AttachedBody* attached_body = new moveit::core::AttachedBody(
-      robot_model->getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state);
-  ks.attachBody(attached_body);
+
+  ks.attachBody(std::make_unique<moveit::core::AttachedBody>(robot_model->getLinkModel("r_gripper_palm_link"), "box",
+                                                             identity, shapes, poses, touch_links, empty_state));
 
   std::vector<const moveit::core::AttachedBody*> attached_bodies_1;
   ks.getAttachedBodies(attached_bodies_1);
@@ -286,14 +286,12 @@ TEST_F(LoadPlanningModelsPr2, ObjectPoseAndSubframes)
   subframes["frame1"] = Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1));
 
   trajectory_msgs::JointTrajectory empty_state;
-  moveit::core::AttachedBody* attached_body_a =
-      new moveit::core::AttachedBody(robot_model->getLinkModel("r_gripper_palm_link"), "boxA", pose_a, shapes, poses,
-                                     touch_links, empty_state, subframes);
-  moveit::core::AttachedBody* attached_body_b =
-      new moveit::core::AttachedBody(robot_model->getLinkModel("r_gripper_palm_link"), "boxB", pose_b, shapes, poses,
-                                     touch_links, empty_state, subframes);
-  ks.attachBody(attached_body_a);
-  ks.attachBody(attached_body_b);
+  ks.attachBody(std::make_unique<moveit::core::AttachedBody>(robot_model->getLinkModel("r_gripper_palm_link"), "boxA",
+                                                             pose_a, shapes, poses, touch_links, empty_state,
+                                                             subframes));
+  ks.attachBody(std::make_unique<moveit::core::AttachedBody>(robot_model->getLinkModel("r_gripper_palm_link"), "boxB",
+                                                             pose_b, shapes, poses, touch_links, empty_state,
+                                                             subframes));
 
   // Check position of shape in each body
   Eigen::Isometry3d p;
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp
index d3a0e706dc..aeee8c5b2e 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp
@@ -860,12 +860,11 @@ void MotionPlanningFrame::renameCollisionObject(QListWidgetItem* item)
     if (ab)
     {
       known_collision_objects_[item->type()].first = item_text;
-      moveit::core::AttachedBody* new_ab =
-          new moveit::core::AttachedBody(ab->getAttachedLink(), known_collision_objects_[item->type()].first,
-                                         ab->getPose(), ab->getShapes(), ab->getShapePoses(), ab->getTouchLinks(),
-                                         ab->getDetachPosture(), ab->getSubframes());
+      auto new_ab = std::make_unique<moveit::core::AttachedBody>(
+          ab->getAttachedLink(), known_collision_objects_[item->type()].first, ab->getPose(), ab->getShapes(),
+          ab->getShapePoses(), ab->getTouchLinks(), ab->getDetachPosture(), ab->getSubframes());
       cs.clearAttachedBody(ab->getName());
-      cs.attachBody(new_ab);
+      cs.attachBody(std::move(new_ab));
     }
   }
   setLocalSceneEdited();

From 872d33dd20f1b8e947549d6535d3d46c95450136 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@users.noreply.github.com>
Date: Tue, 28 Dec 2021 17:05:26 +0100
Subject: [PATCH 046/114] Disable (flaky) timing tests in DEBUG mode (#3012)

---
 .../test_collision_common_pr2.h               | 20 ++++++++++++-------
 1 file changed, 13 insertions(+), 7 deletions(-)

diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h
index 0deef007d7..9abeeae064 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h
@@ -87,6 +87,12 @@ class CollisionDetectorTest : public ::testing::Test
   std::string kinect_dae_resource_;
 };
 
+#ifdef NDEBUG
+#define EXPECT_TIME_LT(EXPR, VAL) EXPECT_LT(EXPR, VAL)
+#else  // Don't perform timing checks in Debug mode (but evaluate expression)
+#define EXPECT_TIME_LT(EXPR, VAL) (void)(EXPR)
+#endif
+
 TYPED_TEST_CASE_P(CollisionDetectorTest);
 
 TYPED_TEST_P(CollisionDetectorTest, InitOK)
@@ -361,7 +367,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester)
   new_cenv->checkSelfCollision(req, res, robot_state);
   double second_check = (ros::WallTime::now() - before).toSec();
 
-  EXPECT_LT(fabs(first_check - second_check), .05);
+  EXPECT_TIME_LT(fabs(first_check - second_check), .05);
 
   std::vector<shapes::ShapeConstPtr> shapes;
   shapes.resize(1);
@@ -382,7 +388,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester)
   second_check = (ros::WallTime::now() - before).toSec();
 
   // the first check is going to take a while, as data must be constructed
-  EXPECT_LT(second_check, .1);
+  EXPECT_TIME_LT(second_check, .1);
 
   collision_detection::CollisionEnvPtr other_new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld());
   before = ros::WallTime::now();
@@ -392,7 +398,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester)
   new_cenv->checkSelfCollision(req, res, robot_state);
   second_check = (ros::WallTime::now() - before).toSec();
 
-  EXPECT_LT(fabs(first_check - second_check), .05);
+  EXPECT_TIME_LT(fabs(first_check - second_check), .05);
 }
 
 TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached)
@@ -418,7 +424,7 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached)
   this->cenv_->checkRobotCollision(req, res, robot_state);
   double second_check = (ros::WallTime::now() - before).toSec();
 
-  EXPECT_LT(second_check, .05);
+  EXPECT_TIME_LT(second_check, .05);
 
   collision_detection::CollisionEnv::ObjectConstPtr object = this->cenv_->getWorld()->getObject("kinect");
   this->cenv_->getWorld()->removeObject("kinect");
@@ -457,8 +463,8 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached)
   this->cenv_->checkSelfCollision(req, res, robot_state2, *this->acm_);
   second_check = (ros::WallTime::now() - before).toSec();
 
-  EXPECT_LT(first_check, .05);
-  EXPECT_LT(fabs(first_check - second_check), .1);
+  EXPECT_TIME_LT(first_check, .05);
+  EXPECT_TIME_LT(fabs(first_check - second_check), .1);
 }
 
 TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed)
@@ -474,7 +480,7 @@ TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed)
   this->cenv_->getWorld()->addToObject("map", shapes, poses);
   double t = (ros::WallTime::now() - start).toSec();
   // TODO (j-petit): investigate why bullet collision checking is considerably slower here
-  EXPECT_GE(5.0, t);
+  EXPECT_TIME_LT(t, 5.0);
   // this is not really a failure; it is just that slow;
   // looking into doing collision checking with a voxel grid.
   ROS_INFO_NAMED("collision_detection.bullet", "Adding boxes took %g", t);

From a47e2c70ae1f7ad4fd91a89441f8f94fa490146c Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 29 Oct 2021 15:19:14 +0200
Subject: [PATCH 047/114] Pass xacro_args to both, urdf and srdf loading

---
 .../src/widgets/start_screen_widget.cpp              | 12 ++++--------
 .../src/widgets/start_screen_widget.h                |  2 +-
 2 files changed, 5 insertions(+), 9 deletions(-)

diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp
index 404acc8943..c9d38a1142 100644
--- a/moveit_setup_assistant/src/widgets/start_screen_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/start_screen_widget.cpp
@@ -395,7 +395,7 @@ bool StartScreenWidget::loadExistingFiles()
   QApplication::processEvents();
 
   // Load the SRDF
-  if (!loadSRDFFile(config_data_->srdf_path_))
+  if (!loadSRDFFile(config_data_->srdf_path_, config_data_->xacro_args_))
     return false;  // error occured
 
   // Progress Indicator
@@ -546,9 +546,7 @@ bool StartScreenWidget::loadNewFiles()
 // ******************************************************************************************
 bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args)
 {
-  const std::vector<std::string> vec_xacro_args = { xacro_args };
-
-  if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, vec_xacro_args))
+  if (!rdf_loader::RDFLoader::loadXmlFileToString(config_data_->urdf_string_, urdf_file_path, { xacro_args }))
   {
     QMessageBox::warning(this, "Error Loading Files",
                          QString("URDF/COLLADA file not found: ").append(urdf_file_path.c_str()));
@@ -592,12 +590,10 @@ bool StartScreenWidget::loadURDFFile(const std::string& urdf_file_path, const st
 // ******************************************************************************************
 // Load SRDF File to Parameter Server
 // ******************************************************************************************
-bool StartScreenWidget::loadSRDFFile(const std::string& srdf_file_path)
+bool StartScreenWidget::loadSRDFFile(const std::string& srdf_file_path, const std::string& xacro_args)
 {
-  const std::vector<std::string> xacro_args;
-
   std::string srdf_string;
-  if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_file_path, xacro_args))
+  if (!rdf_loader::RDFLoader::loadXmlFileToString(srdf_string, srdf_file_path, { xacro_args }))
   {
     QMessageBox::warning(this, "Error Loading Files", QString("SRDF file not found: ").append(srdf_file_path.c_str()));
     return false;
diff --git a/moveit_setup_assistant/src/widgets/start_screen_widget.h b/moveit_setup_assistant/src/widgets/start_screen_widget.h
index b7c28ddef9..af5b1eb320 100644
--- a/moveit_setup_assistant/src/widgets/start_screen_widget.h
+++ b/moveit_setup_assistant/src/widgets/start_screen_widget.h
@@ -146,7 +146,7 @@ private Q_SLOTS:
   bool loadURDFFile(const std::string& urdf_file_path, const std::string& xacro_args);
 
   /// Load SRDF File
-  bool loadSRDFFile(const std::string& srdf_file_path);
+  bool loadSRDFFile(const std::string& srdf_file_path, const std::string& xacro_args);
 
   /// Put SRDF File on Parameter Server
   bool setSRDFFile(const std::string& srdf_string);

From a334d8b6ab3fbd772b63d63e947a076b030860d0 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 29 Oct 2021 17:17:41 +0200
Subject: [PATCH 048/114] Modernize loops

---
 .../src/widgets/setup_assistant_widget.cpp           | 12 +++++-------
 1 file changed, 5 insertions(+), 7 deletions(-)

diff --git a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp
index 98bd321f37..0c64555566 100644
--- a/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/setup_assistant_widget.cpp
@@ -448,11 +448,9 @@ void SetupAssistantWidget::highlightGroup(const std::string& group_name)
       config_data_->getRobotModel()->getJointModelGroup(group_name);
   if (joint_model_group)
   {
-    const std::vector<const moveit::core::LinkModel*>& link_models = joint_model_group->getLinkModels();
     // Iterate through the links
-    for (std::vector<const moveit::core::LinkModel*>::const_iterator link_it = link_models.begin();
-         link_it < link_models.end(); ++link_it)
-      highlightLink((*link_it)->getName(), QColor(255, 0, 0));
+    for (const moveit::core::LinkModel* lm : joint_model_group->getLinkModels())
+      highlightLink(lm->getName(), QColor(255, 0, 0));
   }
 }
 
@@ -477,12 +475,12 @@ void SetupAssistantWidget::unhighlightAll()
   }
 
   // Iterate through the links
-  for (std::vector<std::string>::const_iterator link_it = links.begin(); link_it < links.end(); ++link_it)
+  for (const std::string& link : links)
   {
-    if ((*link_it).empty())
+    if (link.empty())
       continue;
 
-    robot_state_display_->unsetLinkColor(*link_it);
+    robot_state_display_->unsetLinkColor(link);
   }
 }
 

From 1249c2b183e557d502a50c135fa327f55f26fedf Mon Sep 17 00:00:00 2001
From: Jafar Abdi <cafer.abdi@gmail.com>
Date: Tue, 28 Dec 2021 19:24:27 +0300
Subject: [PATCH 049/114] Move MoveItErrorCode class to moveit_core (#3009)

... reducing code duplication and facilitating re-use
---
 MIGRATION.md                                  |  1 +
 .../include/moveit/utils/moveit_error_code.h  | 76 ++++++++++++++++
 .../test/chomp_moveit_test.cpp                | 20 ++---
 .../moveit/moveit_cpp/planning_component.h    | 32 +------
 .../move_group_interface.h                    | 72 +++++----------
 .../src/move_group_interface.cpp              | 87 ++++++++++---------
 .../src/wrap_python_move_group.cpp            | 20 ++---
 .../test/move_group_interface_cpp_test.cpp    |  6 +-
 .../test/move_group_pick_place_test.cpp       |  4 +-
 .../src/motion_planning_frame_planning.cpp    |  6 +-
 10 files changed, 176 insertions(+), 148 deletions(-)
 create mode 100644 moveit_core/utils/include/moveit/utils/moveit_error_code.h

diff --git a/MIGRATION.md b/MIGRATION.md
index 6d275e5871..d59340fc77 100644
--- a/MIGRATION.md
+++ b/MIGRATION.md
@@ -29,6 +29,7 @@ API changes in MoveIt releases
 - All uses of `MOVEIT_CLASS_FORWARD` et. al. must now be followed by a semicolon for consistency (and to get -pedantic builds to pass for the codebase).
 - In case you start RViz in a namespace, the default topic for the trajectory visualization display now uses the relative instead of the absolute namespace (i.e. `<ns>/move_group/display_planned_path` instead of `/move_group/display_planned_path`).
 - `RobotState::attachBody()` now takes a unique_ptr instead of an owning raw pointer.
+- Moved the class `MoveItErrorCode` from both `moveit_ros_planning` and `moveit_ros_planning_interface` to `moveit_core`. The class now is in namespace `moveit::core`, access via `moveit::planning_interface` or `moveit_cpp::PlanningComponent` is deprecated.
 
 ## ROS Melodic
 
diff --git a/moveit_core/utils/include/moveit/utils/moveit_error_code.h b/moveit_core/utils/include/moveit/utils/moveit_error_code.h
new file mode 100644
index 0000000000..5e9aa4bb11
--- /dev/null
+++ b/moveit_core/utils/include/moveit/utils/moveit_error_code.h
@@ -0,0 +1,76 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT 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.
+ *********************************************************************/
+
+#pragma once
+
+#include <moveit_msgs/MoveItErrorCodes.h>
+
+namespace moveit
+{
+namespace core
+{
+/**
+ * @brief a wrapper around moveit_msgs::MoveItErrorCodes to make it easier to return an error code message from a function
+ */
+class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes
+{
+public:
+  MoveItErrorCode()
+  {
+    val = 0;
+  }
+  MoveItErrorCode(int code)
+  {
+    val = code;
+  }
+  MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code)
+  {
+    val = code.val;
+  }
+  explicit operator bool() const
+  {
+    return val == moveit_msgs::MoveItErrorCodes::SUCCESS;
+  }
+  bool operator==(const int c) const
+  {
+    return val == c;
+  }
+  bool operator!=(const int c) const
+  {
+    return val != c;
+  }
+};
+
+}  // namespace core
+}  // namespace moveit
diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp
index 7ec6538be9..7ea3e9ba65 100644
--- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp
+++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp
@@ -58,9 +58,9 @@ TEST_F(CHOMPMoveitTest, jointSpaceGoodGoal)
   move_group_.setStartState(*(move_group_.getCurrentState()));
   move_group_.setJointValueTarget(std::vector<double>({ 1.0, 1.0 }));
 
-  moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_);
+  moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_);
   EXPECT_GT(my_plan_.trajectory_.joint_trajectory.points.size(), 0u);
-  EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS);
 }
 
 TEST_F(CHOMPMoveitTest, jointSpaceBadGoal)
@@ -69,8 +69,8 @@ TEST_F(CHOMPMoveitTest, jointSpaceBadGoal)
   // joint2 is limited to [-PI/2, PI/2]
   move_group_.setJointValueTarget(std::vector<double>({ 100.0, 2 * M_PI / 3.0 }));
 
-  moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_);
-  EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_ROBOT_STATE);
+  moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_);
+  EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_ROBOT_STATE);
 }
 
 TEST_F(CHOMPMoveitTest, cartesianGoal)
@@ -83,17 +83,17 @@ TEST_F(CHOMPMoveitTest, cartesianGoal)
   target_pose1.position.z = 10000.;
   move_group_.setPoseTarget(target_pose1);
 
-  moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_);
+  moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_);
   // CHOMP doesn't support Cartesian-space goals at the moment
-  EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS);
+  EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS);
 }
 
 TEST_F(CHOMPMoveitTest, noStartState)
 {
   move_group_.setJointValueTarget(std::vector<double>({ 0.2, 0.2 }));
 
-  moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_);
-  EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_);
+  EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS);
 }
 
 TEST_F(CHOMPMoveitTest, collisionAtEndOfPath)
@@ -101,8 +101,8 @@ TEST_F(CHOMPMoveitTest, collisionAtEndOfPath)
   move_group_.setStartState(*(move_group_.getCurrentState()));
   move_group_.setJointValueTarget(std::vector<double>({ M_PI / 2.0, 0 }));
 
-  moveit::planning_interface::MoveItErrorCode error_code = move_group_.plan(my_plan_);
-  EXPECT_EQ(error_code.val, moveit::planning_interface::MoveItErrorCode::INVALID_MOTION_PLAN);
+  moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_);
+  EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN);
 }
 
 int main(int argc, char** argv)
diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h
index 14873f9011..0b5469472d 100644
--- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h
+++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/planning_component.h
@@ -40,6 +40,7 @@
 #include <ros/ros.h>
 #include <moveit/moveit_cpp/moveit_cpp.h>
 #include <moveit_msgs/MoveItErrorCodes.h>
+#include <moveit/utils/moveit_error_code.h>
 
 namespace moveit_cpp
 {
@@ -50,34 +51,7 @@ class PlanningComponent
 public:
   MOVEIT_STRUCT_FORWARD(PlanSolution);
 
-  class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes
-  {
-  public:
-    MoveItErrorCode()
-    {
-      val = 0;
-    }
-    MoveItErrorCode(int code)
-    {
-      val = code;
-    }
-    MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code)
-    {
-      val = code.val;
-    }
-    explicit operator bool() const
-    {
-      return val == moveit_msgs::MoveItErrorCodes::SUCCESS;
-    }
-    bool operator==(const int code) const
-    {
-      return val == code;
-    }
-    bool operator!=(const int code) const
-    {
-      return val != code;
-    }
-  };
+  using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode;
 
   /// The representation of a plan solution
   struct PlanSolution
@@ -89,7 +63,7 @@ class PlanningComponent
     robot_trajectory::RobotTrajectoryPtr trajectory;
 
     /// Reason why the plan failed
-    MoveItErrorCode error_code;
+    moveit::core::MoveItErrorCode error_code;
 
     explicit operator bool() const
     {
diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h
index 8838bcc411..9ffb162e82 100644
--- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h
+++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h
@@ -39,6 +39,7 @@
 
 #include <moveit/macros/class_forward.h>
 #include <moveit/robot_state/robot_state.h>
+#include <moveit/utils/moveit_error_code.h>
 #include <moveit_msgs/RobotTrajectory.h>
 #include <moveit_msgs/RobotState.h>
 #include <moveit_msgs/PlannerInterfaceDescription.h>
@@ -60,34 +61,7 @@ namespace moveit
 /** \brief Simple interface to MoveIt components */
 namespace planning_interface
 {
-class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes
-{
-public:
-  MoveItErrorCode()
-  {
-    val = 0;
-  }
-  MoveItErrorCode(int code)
-  {
-    val = code;
-  }
-  MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code)
-  {
-    val = code.val;
-  }
-  explicit operator bool() const
-  {
-    return val == moveit_msgs::MoveItErrorCodes::SUCCESS;
-  }
-  bool operator==(const int c) const
-  {
-    return val == c;
-  }
-  bool operator!=(const int c) const
-  {
-    return val != c;
-  }
-};
+using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode;
 
 MOVEIT_CLASS_FORWARD(MoveGroupInterface);  // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc
 
@@ -719,7 +693,7 @@ class MoveGroupInterface
   /** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified
      target.
       This call is not blocking (does not wait for the execution of the trajectory to complete). */
-  MoveItErrorCode asyncMove();
+  moveit::core::MoveItErrorCode asyncMove();
 
   /** \brief Get the move_group action client used by the \e MoveGroupInterface.
       The client can be used for querying the execution state of the trajectory and abort trajectory execution
@@ -730,24 +704,24 @@ class MoveGroupInterface
      target.
       This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous
      spinner to be started.*/
-  MoveItErrorCode move();
+  moveit::core::MoveItErrorCode move();
 
   /** \brief Compute a motion plan that takes the group declared in the constructor from the current state to the
      specified
       target. No execution is performed. The resulting plan is stored in \e plan*/
-  MoveItErrorCode plan(Plan& plan);
+  moveit::core::MoveItErrorCode plan(Plan& plan);
 
   /** \brief Given a \e plan, execute it without waiting for completion. */
-  MoveItErrorCode asyncExecute(const Plan& plan);
+  moveit::core::MoveItErrorCode asyncExecute(const Plan& plan);
 
   /** \brief Given a \e robot trajectory, execute it without waiting for completion. */
-  MoveItErrorCode asyncExecute(const moveit_msgs::RobotTrajectory& trajectory);
+  moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::RobotTrajectory& trajectory);
 
   /** \brief Given a \e plan, execute it while waiting for completion. */
-  MoveItErrorCode execute(const Plan& plan);
+  moveit::core::MoveItErrorCode execute(const Plan& plan);
 
   /** \brief Given a \e robot trajectory, execute it while waiting for completion. */
-  MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory);
+  moveit::core::MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory);
 
   /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters
       between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the
@@ -824,19 +798,20 @@ class MoveGroupInterface
   /** \brief Pick up an object
 
       This applies a number of hard-coded default grasps */
-  MoveItErrorCode pick(const std::string& object, bool plan_only = false)
+  moveit::core::MoveItErrorCode pick(const std::string& object, bool plan_only = false)
   {
     return pick(constructPickupGoal(object, std::vector<moveit_msgs::Grasp>(), plan_only));
   }
 
   /** \brief Pick up an object given a grasp pose */
-  MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false)
+  moveit::core::MoveItErrorCode pick(const std::string& object, const moveit_msgs::Grasp& grasp, bool plan_only = false)
   {
     return pick(constructPickupGoal(object, { grasp }, plan_only));
   }
 
   /** \brief Pick up an object given possible grasp poses */
-  MoveItErrorCode pick(const std::string& object, std::vector<moveit_msgs::Grasp> grasps, bool plan_only = false)
+  moveit::core::MoveItErrorCode pick(const std::string& object, std::vector<moveit_msgs::Grasp> grasps,
+                                     bool plan_only = false)
   {
     return pick(constructPickupGoal(object, std::move(grasps), plan_only));
   }
@@ -845,40 +820,41 @@ class MoveGroupInterface
 
       Use as follows: first create the goal with constructPickupGoal(), then set \e possible_grasps and any other
       desired variable in the goal, and finally pass it on to this function */
-  MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal);
+  moveit::core::MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal);
 
   /** \brief Pick up an object
 
       calls the external moveit_msgs::GraspPlanning service "plan_grasps" to compute possible grasps */
-  MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false);
+  moveit::core::MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false);
 
   /** \brief Pick up an object
 
       calls the external moveit_msgs::GraspPlanning service "plan_grasps" to compute possible grasps */
-  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false);
+  moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false);
 
   /** \brief Place an object somewhere safe in the world (a safe location will be detected) */
-  MoveItErrorCode place(const std::string& object, bool plan_only = false)
+  moveit::core::MoveItErrorCode place(const std::string& object, bool plan_only = false)
   {
     return place(constructPlaceGoal(object, std::vector<moveit_msgs::PlaceLocation>(), plan_only));
   }
 
   /** \brief Place an object at one of the specified possible locations */
-  MoveItErrorCode place(const std::string& object, std::vector<moveit_msgs::PlaceLocation> locations,
-                        bool plan_only = false)
+  moveit::core::MoveItErrorCode place(const std::string& object, std::vector<moveit_msgs::PlaceLocation> locations,
+                                      bool plan_only = false)
   {
     return place(constructPlaceGoal(object, std::move(locations), plan_only));
   }
 
   /** \brief Place an object at one of the specified possible locations */
-  MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses,
-                        bool plan_only = false)
+  moveit::core::MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::PoseStamped>& poses,
+                                      bool plan_only = false)
   {
     return place(constructPlaceGoal(object, posesToPlaceLocations(poses), plan_only));
   }
 
   /** \brief Place an object at one of the specified possible location */
-  MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose, bool plan_only = false)
+  moveit::core::MoveItErrorCode place(const std::string& object, const geometry_msgs::PoseStamped& pose,
+                                      bool plan_only = false)
   {
     return place(constructPlaceGoal(object, posesToPlaceLocations({ pose }), plan_only));
   }
@@ -887,7 +863,7 @@ class MoveGroupInterface
 
       Use as follows: first create the goal with constructPlaceGoal(), then set \e place_locations and any other
       desired variable in the goal, and finally pass it on to this function */
-  MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal);
+  moveit::core::MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal);
 
   /** \brief Given the name of an object in the planning scene, make
       the object attached to a link of the robot.  If no link name is
diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
index 7017f4bb54..197dd966df 100644
--- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
+++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
@@ -680,17 +680,17 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     return locations;
   }
 
-  MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal)
+  moveit::core::MoveItErrorCode place(const moveit_msgs::PlaceGoal& goal)
   {
     if (!place_action_client_)
     {
       ROS_ERROR_STREAM_NAMED(LOGNAME, "place action client not found");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
     if (!place_action_client_->isServerConnected())
     {
       ROS_WARN_STREAM_NAMED(LOGNAME, "place action server not connected");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
+      return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
     }
 
     place_action_client_->sendGoal(goal);
@@ -701,27 +701,27 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     }
     if (place_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
     {
-      return MoveItErrorCode(place_action_client_->getResult()->error_code);
+      return place_action_client_->getResult()->error_code;
     }
     else
     {
       ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << place_action_client_->getState().toString() << ": "
                                               << place_action_client_->getState().getText());
-      return MoveItErrorCode(place_action_client_->getResult()->error_code);
+      return place_action_client_->getResult()->error_code;
     }
   }
 
-  MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal)
+  moveit::core::MoveItErrorCode pick(const moveit_msgs::PickupGoal& goal)
   {
     if (!pick_action_client_)
     {
       ROS_ERROR_STREAM_NAMED(LOGNAME, "pick action client not found");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
     if (!pick_action_client_->isServerConnected())
     {
       ROS_WARN_STREAM_NAMED(LOGNAME, "pick action server not connected");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
+      return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
     }
 
     pick_action_client_->sendGoal(goal);
@@ -731,17 +731,17 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     }
     if (pick_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
     {
-      return MoveItErrorCode(pick_action_client_->getResult()->error_code);
+      return pick_action_client_->getResult()->error_code;
     }
     else
     {
       ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << pick_action_client_->getState().toString() << ": "
                                               << pick_action_client_->getState().getText());
-      return MoveItErrorCode(pick_action_client_->getResult()->error_code);
+      return pick_action_client_->getResult()->error_code;
     }
   }
 
-  MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false)
+  moveit::core::MoveItErrorCode planGraspsAndPick(const std::string& object, bool plan_only = false)
   {
     if (object.empty())
     {
@@ -755,13 +755,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     {
       ROS_ERROR_STREAM_NAMED(LOGNAME,
                              "Asked for grasps for the object '" << object << "', but the object could not be found");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME);
+      return moveit::core::MoveItErrorCode::INVALID_OBJECT_NAME;
     }
 
     return planGraspsAndPick(objects[object], plan_only);
   }
 
-  MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false)
+  moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only = false)
   {
     if (!plan_grasps_service_.exists())
     {
@@ -769,7 +769,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
                                           << GRASP_PLANNING_SERVICE_NAME
                                           << "' is not available."
                                              " This has to be implemented and started separately.");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
+      return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
     }
 
     moveit_msgs::GraspPlanning::Request request;
@@ -784,23 +784,23 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
         response.error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
     {
       ROS_ERROR_NAMED(LOGNAME, "Grasp planning failed. Unable to pick.");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
 
     return pick(constructPickupGoal(object.id, std::move(response.grasps), plan_only));
   }
 
-  MoveItErrorCode plan(Plan& plan)
+  moveit::core::MoveItErrorCode plan(Plan& plan)
   {
     if (!move_action_client_)
     {
       ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
     if (!move_action_client_->isServerConnected())
     {
       ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
+      return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
     }
 
     moveit_msgs::MoveGroupGoal goal;
@@ -821,27 +821,27 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
       plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
       plan.start_state_ = move_action_client_->getResult()->trajectory_start;
       plan.planning_time_ = move_action_client_->getResult()->planning_time;
-      return MoveItErrorCode(move_action_client_->getResult()->error_code);
+      return move_action_client_->getResult()->error_code;
     }
     else
     {
       ROS_WARN_STREAM_NAMED(LOGNAME, "Fail: " << move_action_client_->getState().toString() << ": "
                                               << move_action_client_->getState().getText());
-      return MoveItErrorCode(move_action_client_->getResult()->error_code);
+      return move_action_client_->getResult()->error_code;
     }
   }
 
-  MoveItErrorCode move(bool wait)
+  moveit::core::MoveItErrorCode move(bool wait)
   {
     if (!move_action_client_)
     {
       ROS_ERROR_STREAM_NAMED(LOGNAME, "move action client not found");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
     if (!move_action_client_->isServerConnected())
     {
       ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
+      return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
     }
 
     moveit_msgs::MoveGroupGoal goal;
@@ -856,7 +856,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     move_action_client_->sendGoal(goal);
     if (!wait)
     {
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
+      return moveit::core::MoveItErrorCode::SUCCESS;
     }
 
     if (!move_action_client_->waitForResult())
@@ -866,27 +866,27 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
 
     if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
     {
-      return MoveItErrorCode(move_action_client_->getResult()->error_code);
+      return move_action_client_->getResult()->error_code;
     }
     else
     {
       ROS_INFO_STREAM_NAMED(LOGNAME, move_action_client_->getState().toString()
                                          << ": " << move_action_client_->getState().getText());
-      return MoveItErrorCode(move_action_client_->getResult()->error_code);
+      return move_action_client_->getResult()->error_code;
     }
   }
 
-  MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait)
+  moveit::core::MoveItErrorCode execute(const moveit_msgs::RobotTrajectory& trajectory, bool wait)
   {
     if (!execute_action_client_)
     {
       ROS_ERROR_STREAM_NAMED(LOGNAME, "execute action client not found");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+      return moveit::core::MoveItErrorCode::FAILURE;
     }
     if (!execute_action_client_->isServerConnected())
     {
       ROS_WARN_STREAM_NAMED(LOGNAME, "execute action server not connected");
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::COMMUNICATION_FAILURE);
+      return moveit::core::MoveItErrorCode::COMMUNICATION_FAILURE;
     }
 
     moveit_msgs::ExecuteTrajectoryGoal goal;
@@ -895,7 +895,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     execute_action_client_->sendGoal(goal);
     if (!wait)
     {
-      return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS);
+      return moveit::core::MoveItErrorCode::SUCCESS;
     }
 
     if (!execute_action_client_->waitForResult())
@@ -905,13 +905,13 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
 
     if (execute_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
     {
-      return MoveItErrorCode(execute_action_client_->getResult()->error_code);
+      return execute_action_client_->getResult()->error_code;
     }
     else
     {
       ROS_INFO_STREAM_NAMED(LOGNAME, execute_action_client_->getState().toString()
                                          << ": " << execute_action_client_->getState().getText());
-      return MoveItErrorCode(execute_action_client_->getResult()->error_code);
+      return execute_action_client_->getResult()->error_code;
     }
   }
 
@@ -1479,7 +1479,7 @@ void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration
   impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
 }
 
-MoveItErrorCode MoveGroupInterface::asyncMove()
+moveit::core::MoveItErrorCode MoveGroupInterface::asyncMove()
 {
   return impl_->move(false);
 }
@@ -1489,32 +1489,32 @@ actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>& MoveGroupInterface:
   return impl_->getMoveGroupClient();
 }
 
-MoveItErrorCode MoveGroupInterface::move()
+moveit::core::MoveItErrorCode MoveGroupInterface::move()
 {
   return impl_->move(true);
 }
 
-MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan)
+moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const Plan& plan)
 {
   return impl_->execute(plan.trajectory_, false);
 }
 
-MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::RobotTrajectory& trajectory)
+moveit::core::MoveItErrorCode MoveGroupInterface::asyncExecute(const moveit_msgs::RobotTrajectory& trajectory)
 {
   return impl_->execute(trajectory, false);
 }
 
-MoveItErrorCode MoveGroupInterface::execute(const Plan& plan)
+moveit::core::MoveItErrorCode MoveGroupInterface::execute(const Plan& plan)
 {
   return impl_->execute(plan.trajectory_, true);
 }
 
-MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::RobotTrajectory& trajectory)
+moveit::core::MoveItErrorCode MoveGroupInterface::execute(const moveit_msgs::RobotTrajectory& trajectory)
 {
   return impl_->execute(trajectory, true);
 }
 
-MoveItErrorCode MoveGroupInterface::plan(Plan& plan)
+moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan)
 {
   return impl_->plan(plan);
 }
@@ -1539,22 +1539,23 @@ MoveGroupInterface::posesToPlaceLocations(const std::vector<geometry_msgs::PoseS
   return impl_->posesToPlaceLocations(poses);
 }
 
-MoveItErrorCode MoveGroupInterface::pick(const moveit_msgs::PickupGoal& goal)
+moveit::core::MoveItErrorCode MoveGroupInterface::pick(const moveit_msgs::PickupGoal& goal)
 {
   return impl_->pick(goal);
 }
 
-MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only)
+moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only)
 {
   return impl_->planGraspsAndPick(object, plan_only);
 }
 
-MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const moveit_msgs::CollisionObject& object, bool plan_only)
+moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const moveit_msgs::CollisionObject& object,
+                                                                    bool plan_only)
 {
   return impl_->planGraspsAndPick(object, plan_only);
 }
 
-MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::PlaceGoal& goal)
+moveit::core::MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::PlaceGoal& goal)
 {
   return impl_->place(goal);
 }
diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp
index 445dc80ba9..34cfdbee27 100644
--- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp
+++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp
@@ -263,7 +263,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     msg.header.frame_id = getPoseReferenceFrame();
     msg.header.stamp = ros::Time::now();
     GILReleaser gr;
-    return place(object_name, msg, plan_only) == MoveItErrorCode::SUCCESS;
+    return place(object_name, msg, plan_only) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool placePoses(const std::string& object_name, const bp::list& poses_list, bool plan_only = false)
@@ -273,7 +273,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     for (int i = 0; i < l; ++i)
       py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(poses_list[i]), poses[i]);
     GILReleaser gr;
-    return place(object_name, poses, plan_only) == MoveItErrorCode::SUCCESS;
+    return place(object_name, poses, plan_only) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool placeLocation(const std::string& object_name, const py_bindings_tools::ByteString& location_str,
@@ -282,7 +282,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     std::vector<moveit_msgs::PlaceLocation> locations(1);
     py_bindings_tools::deserializeMsg(location_str, locations[0]);
     GILReleaser gr;
-    return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS;
+    return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool placeLocations(const std::string& object_name, const bp::list& location_list, bool plan_only = false)
@@ -292,13 +292,13 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     for (int i = 0; i < l; ++i)
       py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(location_list[i]), locations[i]);
     GILReleaser gr;
-    return place(object_name, std::move(locations), plan_only) == MoveItErrorCode::SUCCESS;
+    return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool placeAnywhere(const std::string& object_name, bool plan_only = false)
   {
     GILReleaser gr;
-    return place(object_name, plan_only) == MoveItErrorCode::SUCCESS;
+    return place(object_name, plan_only) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   void convertListToArrayOfPoses(const bp::list& poses, std::vector<geometry_msgs::Pose>& msg)
@@ -441,12 +441,12 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
   bool movePython()
   {
     GILReleaser gr;
-    return move() == MoveItErrorCode::SUCCESS;
+    return move() == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool asyncMovePython()
   {
-    return asyncMove() == MoveItErrorCode::SUCCESS;
+    return asyncMove() == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool attachObjectPython(const std::string& object_name, const std::string& link_name, const bp::list& touch_links)
@@ -459,14 +459,14 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
     MoveGroupInterface::Plan plan;
     py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_);
     GILReleaser gr;
-    return execute(plan) == MoveItErrorCode::SUCCESS;
+    return execute(plan) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bool asyncExecutePython(const py_bindings_tools::ByteString& plan_str)
   {
     MoveGroupInterface::Plan plan;
     py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_);
-    return asyncExecute(plan) == MoveItErrorCode::SUCCESS;
+    return asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS;
   }
 
   bp::tuple planPython()
@@ -668,7 +668,7 @@ static void wrap_move_group_interface()
   move_group_interface_class.def("move", &MoveGroupInterfaceWrapper::movePython);
   move_group_interface_class.def("execute", &MoveGroupInterfaceWrapper::executePython);
   move_group_interface_class.def("async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython);
-  moveit::planning_interface::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) =
+  moveit::core::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) =
       &MoveGroupInterfaceWrapper::pick;
   move_group_interface_class.def("pick", pick_1);
   move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasp);
diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp
index b481aa609b..ed673425ef 100644
--- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp
+++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp
@@ -126,8 +126,8 @@ class MoveGroupTestFixture : public ::testing::Test
   {
     SCOPED_TRACE("planAndMove");
     moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-    ASSERT_EQ(move_group_->plan(my_plan), moveit::planning_interface::MoveItErrorCode::SUCCESS);
-    ASSERT_EQ(move_group_->move(), moveit::planning_interface::MoveItErrorCode::SUCCESS);
+    ASSERT_EQ(move_group_->plan(my_plan), moveit::core::MoveItErrorCode::SUCCESS);
+    ASSERT_EQ(move_group_->move(), moveit::core::MoveItErrorCode::SUCCESS);
   }
 
   void testEigenPose(const Eigen::Isometry3d& expected, const Eigen::Isometry3d& actual)
@@ -318,7 +318,7 @@ TEST_F(MoveGroupTestFixture, CartPathTest)
   ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory), 1.0);
 
   // Execute trajectory
-  EXPECT_EQ(move_group_->execute(trajectory), moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  EXPECT_EQ(move_group_->execute(trajectory), moveit::core::MoveItErrorCode::SUCCESS);
 
   // get the pose after the movement
   testPose(target_waypoint);
diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp
index 4b2d906448..d0814f7037 100644
--- a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp
+++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp
@@ -219,7 +219,7 @@ TEST_F(PickPlaceTestFixture, PickPlaceTest)
   // Set support surface as table1.
   move_group_->setSupportSurfaceName("table1");
   // Call pick to pick up the object using the grasps given
-  ASSERT_EQ(move_group_->pick("object", grasps), moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  ASSERT_EQ(move_group_->pick("object", grasps), moveit::core::MoveItErrorCode::SUCCESS);
 
   // Ideally, you would create a vector of place locations to be attempted although in this example, we only create
   // a single place location.
@@ -274,7 +274,7 @@ TEST_F(PickPlaceTestFixture, PickPlaceTest)
   // Set support surface as table2.
   move_group_->setSupportSurfaceName("table2");
   // Call place to place the object using the place locations given.
-  ASSERT_EQ(move_group_->place("object", place_location), moveit::planning_interface::MoveItErrorCode::SUCCESS);
+  ASSERT_EQ(move_group_->place("object", place_location), moveit::core::MoveItErrorCode::SUCCESS);
 }
 
 int main(int argc, char** argv)
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
index c8767bef3a..272873b053 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
@@ -162,7 +162,7 @@ bool MotionPlanningFrame::computeCartesianPlan()
 bool MotionPlanningFrame::computeJointSpacePlan()
 {
   current_plan_ = std::make_shared<moveit::planning_interface::MoveGroupInterface::Plan>();
-  return move_group_->plan(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
+  return move_group_->plan(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS;
 }
 
 void MotionPlanningFrame::computePlanButtonClicked()
@@ -199,7 +199,7 @@ void MotionPlanningFrame::computeExecuteButtonClicked()
   if (mgi && current_plan_)
   {
     ui_->stop_button->setEnabled(true);  // enable stopping
-    bool success = mgi->execute(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
+    bool success = mgi->execute(*current_plan_) == moveit::core::MoveItErrorCode::SUCCESS;
     onFinishedExecution(success);
   }
 }
@@ -221,7 +221,7 @@ void MotionPlanningFrame::computePlanAndExecuteButtonClicked()
   }
   else
   {
-    bool success = move_group_->move() == moveit::planning_interface::MoveItErrorCode::SUCCESS;
+    bool success = move_group_->move() == moveit::core::MoveItErrorCode::SUCCESS;
     onFinishedExecution(success);
   }
   ui_->plan_and_execute_button->setEnabled(true);

From a956b2ae523c4618943d4bd9f0818b15320d35f6 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sat, 30 Oct 2021 00:03:15 +0200
Subject: [PATCH 050/114] Adapt to API changes in srdfdom

---
 .../test_collision_common_panda.h             |  8 +++--
 ...t_bullet_continuous_collision_checking.cpp |  8 +++--
 .../planning_scene/src/planning_scene.cpp     |  8 +++--
 .../src/collisions_updater.cpp                |  2 +-
 .../src/tools/moveit_config_data.cpp          | 36 +++++++++----------
 .../widgets/configuration_files_widget.cpp    |  2 +-
 .../src/widgets/default_collisions_widget.cpp | 11 +++---
 7 files changed, 41 insertions(+), 34 deletions(-)

diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
index ca840ac763..84ec2c9897 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
@@ -83,10 +83,12 @@ class CollisionDetectorPandaTest : public testing::Test
     const std::vector<std::string>& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry();
     acm_->setEntry(collision_links, collision_links, false);
 
+    // load collision defaults
+    for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks())
+      acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER);
     // allow collisions for pairs that have been disabled
-    const std::vector<srdf::Model::DisabledCollision>& dc = robot_model_->getSRDF()->getDisabledCollisionPairs();
-    for (const srdf::Model::DisabledCollision& it : dc)
-      acm_->setEntry(it.link1_, it.link2_, true);
+    for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs())
+      acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_);
 
     cenv_ = value_->allocateEnv(robot_model_);
 
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 0334fa531f..0d72ad7b6a 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
@@ -81,10 +81,12 @@ class BulletCollisionDetectionTester : public testing::Test
     const std::vector<std::string>& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry();
     acm_->setEntry(collision_links, collision_links, false);
 
+    // load collision defaults
+    for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks())
+      acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER);
     // allow collisions for pairs that have been disabled
-    const std::vector<srdf::Model::DisabledCollision>& dc = robot_model_->getSRDF()->getDisabledCollisionPairs();
-    for (const srdf::Model::DisabledCollision& it : dc)
-      acm_->setEntry(it.link1_, it.link2_, true);
+    for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs())
+      acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_);
 
     cenv_ = std::make_shared<collision_detection::CollisionEnvBullet>(robot_model_);
 
diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
index 96db06e223..2daac721cb 100644
--- a/moveit_core/planning_scene/src/planning_scene.cpp
+++ b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -156,10 +156,12 @@ void PlanningScene::initialize()
   const std::vector<std::string>& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry();
   acm_->setEntry(collision_links, collision_links, false);
 
+  // load collision defaults
+  for (const std::string& name : getRobotModel()->getSRDF()->getNoDefaultCollisionLinks())
+    acm_->setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS);
   // allow collisions for pairs that have been disabled
-  const std::vector<srdf::Model::DisabledCollision>& dc = getRobotModel()->getSRDF()->getDisabledCollisionPairs();
-  for (const srdf::Model::DisabledCollision& it : dc)
-    acm_->setEntry(it.link1_, it.link2_, true);
+  for (auto const& collision : getRobotModel()->getSRDF()->getCollisionPairs())
+    acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_);
 
   setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
 }
diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp
index 89b418e96e..b79f5cbd0e 100644
--- a/moveit_setup_assistant/src/collisions_updater.cpp
+++ b/moveit_setup_assistant/src/collisions_updater.cpp
@@ -97,7 +97,7 @@ bool setup(moveit_setup_assistant::MoveItConfigData& config_data, bool keep_old,
   }
 
   if (!keep_old)
-    config_data.srdf_->disabled_collisions_.clear();
+    config_data.srdf_->collision_pairs_.clear();
 
   return true;
 }
diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
index 4535622c36..3f8d65e4dd 100644
--- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp
+++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
@@ -144,9 +144,9 @@ void MoveItConfigData::loadAllowedCollisionMatrix()
   allowed_collision_matrix_.clear();
 
   // Update the allowed collision matrix, in case there has been a change
-  for (const auto& disabled_collision : srdf_->disabled_collisions_)
+  for (const auto& collision : srdf_->collision_pairs_)
   {
-    allowed_collision_matrix_.setEntry(disabled_collision.link1_, disabled_collision.link2_, true);
+    allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, collision.disabled_);
   }
 }
 
@@ -1204,36 +1204,36 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path)
 // Set list of collision link pairs in SRDF; sorted; with optional filter
 // ******************************************************************************************
 
-class SortableDisabledCollision
+class SortableCollisionPair
 {
 public:
-  SortableDisabledCollision(const srdf::Model::DisabledCollision& dc)
-    : dc_(dc), key_(dc.link1_ < dc.link2_ ? (dc.link1_ + "|" + dc.link2_) : (dc.link2_ + "|" + dc.link1_))
+  SortableCollisionPair(const srdf::Model::CollisionPair& p)
+    : pair_(p), key_(p.link1_ < p.link2_ ? (p.link1_ + "|" + p.link2_) : (p.link2_ + "|" + p.link1_))
   {
   }
-  operator const srdf::Model::DisabledCollision &() const
+  operator const srdf::Model::CollisionPair &() const
   {
-    return dc_;
+    return pair_;
   }
-  bool operator<(const SortableDisabledCollision& other) const
+  bool operator<(const SortableCollisionPair& other) const
   {
     return key_ < other.key_;
   }
 
 private:
-  const srdf::Model::DisabledCollision dc_;
+  const srdf::Model::CollisionPair pair_;
   const std::string key_;
 };
 
 void MoveItConfigData::setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap& link_pairs, size_t skip_mask)
 {
   // Create temp disabled collision
-  srdf::Model::DisabledCollision dc;
+  srdf::Model::CollisionPair pair;
 
-  std::set<SortableDisabledCollision> disabled_collisions;
-  disabled_collisions.insert(srdf_->disabled_collisions_.begin(), srdf_->disabled_collisions_.end());
+  std::set<SortableCollisionPair> collision_pairs;
+  collision_pairs.insert(srdf_->collision_pairs_.begin(), srdf_->collision_pairs_.end());
 
-  // copy the data in this class's LinkPairMap datastructure to srdf::Model::DisabledCollision format
+  // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format
   for (const std::pair<const std::pair<std::string, std::string>, LinkPairData>& link_pair : link_pairs)
   {
     // Only copy those that are actually disabled
@@ -1242,15 +1242,15 @@ void MoveItConfigData::setCollisionLinkPairs(const moveit_setup_assistant::LinkP
       if ((1 << link_pair.second.reason) & skip_mask)
         continue;
 
-      dc.link1_ = link_pair.first.first;
-      dc.link2_ = link_pair.first.second;
-      dc.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason);
+      pair.link1_ = link_pair.first.first;
+      pair.link2_ = link_pair.first.second;
+      pair.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason);
 
-      disabled_collisions.insert(SortableDisabledCollision(dc));
+      collision_pairs.insert(SortableCollisionPair(pair));
     }
   }
 
-  srdf_->disabled_collisions_.assign(disabled_collisions.begin(), disabled_collisions.end());
+  srdf_->collision_pairs_.assign(collision_pairs.begin(), collision_pairs.end());
 }
 
 // ******************************************************************************************
diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
index 2685d6d1d8..39f23c2e8d 100644
--- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
@@ -641,7 +641,7 @@ bool ConfigurationFilesWidget::checkDependencies()
   }
 
   // Check that at least 1 link pair is disabled from collision checking
-  if (config_data_->srdf_->disabled_collisions_.empty())
+  if (config_data_->srdf_->collision_pairs_.empty())
   {
     dependencies << "No self-collisions have been disabled";
   }
diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp
index cb548414e4..3d80e6efa7 100644
--- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp
@@ -677,12 +677,12 @@ void DefaultCollisionsWidget::checkedFilterChanged()
 void DefaultCollisionsWidget::linkPairsToSRDF()
 {
   // reset the data in the SRDF Writer class
-  config_data_->srdf_->disabled_collisions_.clear();
+  config_data_->srdf_->collision_pairs_.clear();
 
   // Create temp disabled collision
-  srdf::Model::DisabledCollision dc;
+  srdf::Model::CollisionPair dc;
 
-  // copy the data in this class's LinkPairMap datastructure to srdf::Model::DisabledCollision format
+  // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format
   for (moveit_setup_assistant::LinkPairMap::const_iterator pair_it = link_pairs_.begin(); pair_it != link_pairs_.end();
        ++pair_it)
   {
@@ -692,7 +692,8 @@ void DefaultCollisionsWidget::linkPairsToSRDF()
       dc.link1_ = pair_it->first.first;
       dc.link2_ = pair_it->first.second;
       dc.reason_ = moveit_setup_assistant::disabledReasonToString(pair_it->second.reason);
-      config_data_->srdf_->disabled_collisions_.push_back(dc);
+      dc.disabled_ = pair_it->second.disable_check;
+      config_data_->srdf_->collision_pairs_.push_back(dc);
     }
   }
 
@@ -719,7 +720,7 @@ void DefaultCollisionsWidget::linkPairsFromSRDF()
   std::pair<std::string, std::string> link_pair;
 
   // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created
-  for (const auto& disabled_collision : config_data_->srdf_->disabled_collisions_)
+  for (const auto& disabled_collision : config_data_->srdf_->collision_pairs_)
   {
     // Set the link names
     link_pair.first = disabled_collision.link1_;

From 2c9d26b58543b7e1b8a769ab7423c3e75bb6e1c9 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sat, 30 Oct 2021 00:05:24 +0200
Subject: [PATCH 051/114] Don't fill all ACM entries by default

---
 .../moveit/collision_detection/test_collision_common_panda.h   | 3 ---
 .../test/test_bullet_continuous_collision_checking.cpp         | 3 ---
 moveit_core/planning_scene/src/planning_scene.cpp              | 3 ---
 3 files changed, 9 deletions(-)

diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
index 84ec2c9897..9b0bf7097f 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
@@ -79,9 +79,6 @@ class CollisionDetectorPandaTest : public testing::Test
     robot_model_ok_ = static_cast<bool>(robot_model_);
 
     acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
-    // Use default collision operations in the SRDF to setup the acm
-    const std::vector<std::string>& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry();
-    acm_->setEntry(collision_links, collision_links, false);
 
     // load collision defaults
     for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks())
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 0d72ad7b6a..06c6300f14 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
@@ -77,9 +77,6 @@ class BulletCollisionDetectionTester : public testing::Test
     robot_model_ok_ = static_cast<bool>(robot_model_);
 
     acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
-    // Use default collision operations in the SRDF to setup the acm
-    const std::vector<std::string>& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry();
-    acm_->setEntry(collision_links, collision_links, false);
 
     // load collision defaults
     for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks())
diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
index 2daac721cb..6180b1d3ae 100644
--- a/moveit_core/planning_scene/src/planning_scene.cpp
+++ b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -152,9 +152,6 @@ void PlanningScene::initialize()
   robot_state_->update();
 
   acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
-  // Use default collision operations in the SRDF to setup the acm
-  const std::vector<std::string>& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry();
-  acm_->setEntry(collision_links, collision_links, false);
 
   // load collision defaults
   for (const std::string& name : getRobotModel()->getSRDF()->getNoDefaultCollisionLinks())

From 9e268e79353ac639c5c55340d15a2b283a763821 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sat, 30 Oct 2021 17:31:56 +0200
Subject: [PATCH 052/114] Improve formatting of comments

---
 .../collision_detection/collision_matrix.h    | 53 ++++++++-----------
 1 file changed, 23 insertions(+), 30 deletions(-)

diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
index 4b7a72910b..234ee5d7ff 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
@@ -94,37 +94,36 @@ class AllowedCollisionMatrix
   /** @brief Copy constructor */
   AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) = default;
 
-  /** @brief Get the type of the allowed collision between two elements. Return true if the entry is included in the
-   * collision matrix.
-   * Return false if the entry is not found.
+  /** @brief Get the type of the allowed collision between two elements.
+   *  Return true if the entry is included in the collision matrix. Return false if the entry is not found.
    *  @param name1 name of first element
    *  @param name2 name of second element
    *  @param allowed_collision_type The allowed collision type will be filled here */
   bool getEntry(const std::string& name1, const std::string& name2,
                 AllowedCollision::Type& allowed_collision_type) const;
 
-  /** @brief Get the allowed collision predicate between two elements. Return true if a predicate for entry is included
-   * in the collision matrix
-   * (if the type is AllowedCollision::CONDITIONAL). Return false if the entry is not found.
+  /** @brief Get the allowed collision predicate between two elements.
+   *  Return true if a predicate for this entry is available in the collision matrix.
+   *  Return false if the entry is not found.
    *  @param name1 name of first element
    *  @param name2 name of second element
    *  @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled
    * here */
   bool getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const;
 
-  /** @brief Check if the allowed collision matrix has an entry at all for an element. Returns true if the element is
-   * included.
+  /** @brief Check if the allowed collision matrix has an entry at all for an element.
+   *  Returns true if the element is included.
    *  @param name name of the element */
   bool hasEntry(const std::string& name) const;
 
-  /** @brief Check if the allowed collision matrix has an entry for a pair of elements. Returns true if the pair is
-   * included.
+  /** @brief Check if the allowed collision matrix has an entry for a pair of elements.
+   *  Returns true if the pair is included.
    *  @param name1 name of first element
    *  @param name2 name of second element*/
   bool hasEntry(const std::string& name1, const std::string& name2) const;
 
-  /** @brief Remove an entry corresponding to a pair of elements. Nothing happens if the pair does not exist in the
-   * collision matrix.
+  /** @brief Remove an entry corresponding to a pair of elements.
+   *  Nothing happens if the pair does not exist in the collision matrix.
    *  @param name1 name of first element
    *  @param name2 name of second element*/
   void removeEntry(const std::string& name1, const std::string& name2);
@@ -138,27 +137,24 @@ class AllowedCollisionMatrix
    *  @param name2 name of second element
    *  @param allowed If false, indicates that collisions between two elements must be checked for and no collisions
    *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an
-   * explicit collision
-   *  computation is not necessary (AllowedCollision::ALWAYS).*/
+   *  explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
   void setEntry(const std::string& name1, const std::string& name2, bool allowed);
 
-  /** @brief Set an entry corresponding to a pair of elements. This sets the type of the entry to
-   * AllowedCollision::CONDITIONAL.
+  /** @brief Set an entry corresponding to a pair of elements.
+   *
+   *  This sets the type of the entry to AllowedCollision::CONDITIONAL.
    *  @param name1 name of first element
    *  @param name2 name of second element
-   *  @param fn A callback function that is used to decide if collisions are allowed between the two elements is
-   * expected here */
+   *  @param fn A callback function that is used to decide if collisions are allowed between the two elements */
   void setEntry(const std::string& name1, const std::string& name2, const DecideContactFn& fn);
 
-  /** @brief Set the entries corresponding to a name. With each of the the known names in the collision matrix, form a
-   * pair using the name
-   * specified as argument to this function and set the entry as indicated by \e allowed.
+  /** @brief Set the entries corresponding to a name.
+   *  With each of the the known names in the collision matrix, form a pair using the name
+   *  specified as argument to this function and set the entry as indicated by \e allowed.
    *  @param name the object name
    *  @param allowed If false, indicates that collisions between two elements must be checked for and no collisions
    *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an
-   * explicit collision
-   *  computation is not necessary (AllowedCollision::ALWAYS).*/
-  void setEntry(const std::string& name, bool allowed);
+   *  explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
 
   /** @brief Set multiple entries. Pairs of names are formed using \e name and \e other_names
    *  @param name name of first element
@@ -166,8 +162,7 @@ class AllowedCollisionMatrix
    *  matrix entries will be set for all such pairs.
    *  @param allowed If false, indicates that collisions between two elements must be checked for and no collisions
    *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an
-   * explicit collision
-   *  computation is not necessary (AllowedCollision::ALWAYS).*/
+   *  explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
   void setEntry(const std::string& name, const std::vector<std::string>& other_names, bool allowed);
 
   /** @brief Set an entry corresponding to all possible pairs between two sets of elements
@@ -175,15 +170,13 @@ class AllowedCollisionMatrix
    *  @param names2 Second set of names
    *  @param allowed If false, indicates that collisions between two elements must be checked for and no collisions
    *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an
-   * explicit collision
-   *  computation is not necessary (AllowedCollision::ALWAYS).*/
+   *  explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
   void setEntry(const std::vector<std::string>& names1, const std::vector<std::string>& names2, bool allowed);
 
   /** @brief Set an entry corresponding to all known pairs
    *  @param allowed If false, indicates that collisions between two elements must be checked for and no collisions
    *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an
-   * explicit collision
-   *  computation is not necessary (AllowedCollision::ALWAYS).*/
+   *  explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
   void setEntry(bool allowed);
 
   /** @brief Get all the names known to the collision matrix */

From 341e71d299fceb952c8834aabc001d82c486f6a6 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sat, 30 Oct 2021 18:01:03 +0200
Subject: [PATCH 053/114] ACM: specific pair entries take precedence over
 defaults

Reverts c72a8570d420a23a9fe4715705ed617f18836634
---
 .../collision_detection/collision_matrix.h    | 57 ++++++-------------
 .../src/collision_matrix.cpp                  | 28 ++++-----
 2 files changed, 29 insertions(+), 56 deletions(-)

diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
index 234ee5d7ff..0e975d6b71 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
@@ -194,65 +194,42 @@ class AllowedCollisionMatrix
     return entries_.size();
   }
 
-  /** @brief Set the default value for entries that include \e name. If such a default value is set, queries to
-   * getAllowedCollision() that include
-   *  \e name will return this value instead, @b even if a pair that includes \e name was previously specified with
-   * setEntry().
+  /** @brief Set the default value for entries that include \e name but are not set explicity with setEntry().
    *  @param name The name of the element for which to set the default value
    *  @param allowed If false, indicates that collisions between \e name and any other element must be checked for and
-   * no collisions
-   *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between \e name and any other
-   * element are ok and
-   *  an explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
+   *  no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between \e name and
+   *  any other element are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
   void setDefaultEntry(const std::string& name, bool allowed);
 
-  /** @brief Set the default value for entries that include \e name to be AllowedCollision::CONDITIONAL and specify the
-   * allowed contact predicate to be \e fn.
-   *  If this function is called, queries to getAllowedCollision() that include \e name will return this value instead,
-   * @b even if a pair that includes \e name
-   *  was previously specified with setEntry().
+  /** @brief Set the default value for entries that include \e name but are not set explicity with setEntry().
    *  @param name The name of the element for which to set the default value
    *  @param fn A callback function that is used to decide if collisions are allowed between \e name and some other
-   * element is expected here. */
+   *  element is expected here. */
   void setDefaultEntry(const std::string& name, const DecideContactFn& fn);
 
-  /** @brief Get the type of the allowed collision between to be considered by default for an element. Return true if a
-   * default value was
-   *  found for the specified element. Return false otherwise.
+  /** @brief Get the type of the allowed collision to be considered by default for an element.
+   *  Return true if a default value was found for the specified element. Return false otherwise.
    *  @param name name of the element
    *  @param allowed_collision The default allowed collision type will be filled here */
   bool getDefaultEntry(const std::string& name, AllowedCollision::Type& allowed_collision) const;
 
-  /** @brief Get the type of the allowed collision between to be considered by default for an element. Return true if a
-   * default value was
-   *  found for the specified element. Return false otherwise.
+  /** @brief Get the type of the allowed collision between to be considered by default for an element.
+   *  Return true if a default value was found for the specified element. Return false otherwise.
    *  @param name name of the element
-   *  @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled
-   * here. */
+   *  @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */
   bool getDefaultEntry(const std::string& name, DecideContactFn& fn) const;
 
-  /** @brief Get the allowed collision predicate between two elements. Return true if a predicate for entry is included
-   * in the collision matrix
-   *  (if the type is AllowedCollision::CONDITIONAL) or if one was computed from defaults. Return false if the entry is
-   * not found.
-   *  Default values take precedence. If a default value is specified  for either \e name1 or \e name2, that value is
-   * returned instead (if both
-   *  elements have default values specified, both predicates must be satisfied). If no default values are specified,
-   * getEntry() is used to look
-   *  for the pair (\e name1, \e name2).
+  /** @brief Get the allowed collision predicate between two elements.
+   *  Return true if a predicate for entry is included in the collision matrix (if the type is AllowedCollision::CONDITIONAL)
+   *  or if one was computed from defaults. Return false if the entry is not found.
    *  @param name1 name of first element
    *  @param name2 name of second element
-   *  @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled
-   * here */
+   *  @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */
   bool getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn& fn) const;
 
-  /** @brief Get the type of the allowed collision between two elements. Return true if the entry is included in the
-   * collision matrix or if
-   *  specified defaults were found. Return false if the entry is not found. Default values take precedence. If a
-   * default value is specified
-   *  for either \e name1 or \e name2, that value is returned instead (if both elements have default values specified,
-   * AllowedCollision::NEVER takes
-   *  precedence). If no default values are specified, getEntry() is used to look for the pair (\e name1, \e name2).
+  /** @brief Get the type of the allowed collision between two elements.
+   *  Return true if the entry is included in the collision matrix or if specified defaults were found.
+   *  Return false if the entry is not found.
    *  @param name1 name of first element
    *  @param name2 name of second element
    *  @param allowed_collision The allowed collision type will be filled here */
diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp
index 369774ef18..093a457c17 100644
--- a/moveit_core/collision_detection/src/collision_matrix.cpp
+++ b/moveit_core/collision_detection/src/collision_matrix.cpp
@@ -255,14 +255,12 @@ static bool andDecideContact(const DecideContactFn& f1, const DecideContactFn& f
 bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
                                                  DecideContactFn& fn) const
 {
-  DecideContactFn fn1, fn2;
-  bool found1 = getDefaultEntry(name1, fn1);
-  bool found2 = getDefaultEntry(name2, fn2);
-
-  if (!found1 && !found2)
-    return getEntry(name1, name2, fn);
-  else
+  bool found = getEntry(name1, name2, fn);
+  if (!found)
   {
+    DecideContactFn fn1, fn2;
+    bool found1 = getDefaultEntry(name1, fn1);
+    bool found2 = getDefaultEntry(name2, fn2);
     if (found1 && !found2)
       fn = fn1;
     else if (!found1 && found2)
@@ -271,21 +269,19 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const
       fn = std::bind(&andDecideContact, fn1, fn2, std::placeholders::_1);
     else
       return false;
-    return true;
   }
+  return true;
 }
 
 bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
                                                  AllowedCollision::Type& allowed_collision) const
 {
-  AllowedCollision::Type t1, t2;
-  bool found1 = getDefaultEntry(name1, t1);
-  bool found2 = getDefaultEntry(name2, t2);
-
-  if (!found1 && !found2)
-    return getEntry(name1, name2, allowed_collision);
-  else
+  bool found = getEntry(name1, name2, allowed_collision);
+  if (!found)
   {
+    AllowedCollision::Type t1, t2;
+    bool found1 = getDefaultEntry(name1, t1);
+    bool found2 = getDefaultEntry(name2, t2);
     if (found1 && !found2)
       allowed_collision = t1;
     else if (!found1 && found2)
@@ -301,8 +297,8 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const
     }
     else
       return false;
-    return true;
   }
+  return true;
 }
 
 void AllowedCollisionMatrix::clear()

From cb1ea39fbaf3212bcfa6cafb40dacefad3df6c41 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sat, 30 Oct 2021 18:03:39 +0200
Subject: [PATCH 054/114] Add comment to prefer setDefaultEntry() over
 setEntry()

... because the former will consider future collision entries as well.
---
 .../include/moveit/collision_detection/collision_matrix.h     | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
index 0e975d6b71..703f2c2dce 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
@@ -149,12 +149,14 @@ class AllowedCollisionMatrix
   void setEntry(const std::string& name1, const std::string& name2, const DecideContactFn& fn);
 
   /** @brief Set the entries corresponding to a name.
-   *  With each of the the known names in the collision matrix, form a pair using the name
+   *  With each of the *known names* in the collision matrix, form a pair using the name
    *  specified as argument to this function and set the entry as indicated by \e allowed.
+   *  As the set of known names might change in future, consider using setDefaultEntry() instead!
    *  @param name the object name
    *  @param allowed If false, indicates that collisions between two elements must be checked for and no collisions
    *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an
    *  explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
+  void setEntry(const std::string& name, bool allowed);
 
   /** @brief Set multiple entries. Pairs of names are formed using \e name and \e other_names
    *  @param name name of first element

From 9c7d8dde27184d5c67b08c5d032f84837e1c68e3 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sun, 31 Oct 2021 12:22:19 +0100
Subject: [PATCH 055/114] Optimization: Check for most common case first

---
 moveit_core/collision_detection/src/collision_matrix.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp
index 093a457c17..e486f4cd58 100644
--- a/moveit_core/collision_detection/src/collision_matrix.cpp
+++ b/moveit_core/collision_detection/src/collision_matrix.cpp
@@ -282,7 +282,9 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const
     AllowedCollision::Type t1, t2;
     bool found1 = getDefaultEntry(name1, t1);
     bool found2 = getDefaultEntry(name2, t2);
-    if (found1 && !found2)
+    if (!found1 && !found2)
+      return false;
+    else if (found1 && !found2)
       allowed_collision = t1;
     else if (!found1 && found2)
       allowed_collision = t2;
@@ -295,8 +297,6 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const
       else  // ALWAYS is the only remaining case
         allowed_collision = AllowedCollision::ALWAYS;
     }
-    else
-      return false;
   }
   return true;
 }

From e44e8e2913d3d1affb820d0c99ab8ce21d609a13 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sun, 31 Oct 2021 21:20:14 +0100
Subject: [PATCH 056/114] Adapt message passing of AllowedCollisionMatrix

- Serialize full current state (previously pairs with a default, but no entry were skipped)
- Only initialize matrix entries that deviate from the default.
---
 .../collision_detection/collision_matrix.h    |  3 +
 .../src/collision_matrix.cpp                  | 87 +++++++++++--------
 2 files changed, 53 insertions(+), 37 deletions(-)

diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
index 703f2c2dce..eabfd12c18 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
@@ -242,6 +242,9 @@ class AllowedCollisionMatrix
   void print(std::ostream& out) const;
 
 private:
+  bool getDefaultEntry(const std::string& name1, const std::string& name2,
+                       AllowedCollision::Type& allowed_collision) const;
+
   std::map<std::string, std::map<std::string, AllowedCollision::Type> > entries_;
   std::map<std::string, std::map<std::string, DecideContactFn> > allowed_contacts_;
 
diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp
index e486f4cd58..4a4105f080 100644
--- a/moveit_core/collision_detection/src/collision_matrix.cpp
+++ b/moveit_core/collision_detection/src/collision_matrix.cpp
@@ -55,21 +55,33 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisi
 {
   if (msg.entry_names.size() != msg.entry_values.size() ||
       msg.default_entry_names.size() != msg.default_entry_values.size())
+  {
     ROS_ERROR_NAMED("collision_detection", "The number of links does not match the number of entries "
                                            "in AllowedCollisionMatrix message");
-  else
+    return;
+  }
+  for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i)
+    setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]);
+
+  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
   {
-    for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
-      if (msg.entry_values[i].enabled.size() != msg.entry_names.size())
-        ROS_ERROR_NAMED("collision_detection",
-                        "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
-                        msg.entry_names[i].c_str());
-      else
-        for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j)
-          setEntry(msg.entry_names[i], msg.entry_names[j], msg.entry_values[i].enabled[j]);
+    if (msg.entry_values[i].enabled.size() != msg.entry_names.size())
+    {
+      ROS_ERROR_NAMED("collision_detection",
+                      "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
+                      msg.entry_names[i].c_str());
+      return;
+    }
+    for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j)
+    {
+      AllowedCollision::Type allowed_default, allowed_entry;
+      if (!getDefaultEntry(msg.entry_names[i], msg.entry_names[j], allowed_default))
+        allowed_default = AllowedCollision::NEVER;
+      allowed_entry = msg.entry_values[i].enabled[j] ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
 
-    for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i)
-      setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]);
+      if (allowed_entry != allowed_default)
+        setEntry(msg.entry_names[i], msg.entry_names[j], allowed_entry);
+    }
   }
 }
 
@@ -273,34 +285,36 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const
   return true;
 }
 
-bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
-                                                 AllowedCollision::Type& allowed_collision) const
+bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name1, const std::string& name2,
+                                             AllowedCollision::Type& allowed_collision) const
 {
-  bool found = getEntry(name1, name2, allowed_collision);
-  if (!found)
+  AllowedCollision::Type t1, t2;
+  bool found1 = getDefaultEntry(name1, t1);
+  bool found2 = getDefaultEntry(name2, t2);
+  if (!found1 && !found2)
+    return false;
+  else if (found1 && !found2)
+    allowed_collision = t1;
+  else if (!found1 && found2)
+    allowed_collision = t2;
+  else if (found1 && found2)
   {
-    AllowedCollision::Type t1, t2;
-    bool found1 = getDefaultEntry(name1, t1);
-    bool found2 = getDefaultEntry(name2, t2);
-    if (!found1 && !found2)
-      return false;
-    else if (found1 && !found2)
-      allowed_collision = t1;
-    else if (!found1 && found2)
-      allowed_collision = t2;
-    else if (found1 && found2)
-    {
-      if (t1 == AllowedCollision::NEVER || t2 == AllowedCollision::NEVER)
-        allowed_collision = AllowedCollision::NEVER;
-      else if (t1 == AllowedCollision::CONDITIONAL || t2 == AllowedCollision::CONDITIONAL)
-        allowed_collision = AllowedCollision::CONDITIONAL;
-      else  // ALWAYS is the only remaining case
-        allowed_collision = AllowedCollision::ALWAYS;
-    }
+    if (t1 == AllowedCollision::NEVER || t2 == AllowedCollision::NEVER)
+      allowed_collision = AllowedCollision::NEVER;
+    else if (t1 == AllowedCollision::CONDITIONAL || t2 == AllowedCollision::CONDITIONAL)
+      allowed_collision = AllowedCollision::CONDITIONAL;
+    else  // ALWAYS is the only remaining case
+      allowed_collision = AllowedCollision::ALWAYS;
   }
   return true;
 }
 
+bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
+                                                 AllowedCollision::Type& allowed_collision) const
+{
+  return getEntry(name1, name2, allowed_collision) || getDefaultEntry(name1, name2, allowed_collision);
+}
+
 void AllowedCollisionMatrix::clear()
 {
   entries_.clear();
@@ -347,10 +361,9 @@ void AllowedCollisionMatrix::getMessage(moveit_msgs::AllowedCollisionMatrix& msg
 
     for (std::size_t j = i; j < msg.entry_names.size(); ++j)
     {
-      AllowedCollision::Type type;
-      bool found = getEntry(msg.entry_names[i], msg.entry_names[j], type);
-      if (found)
-        msg.entry_values[i].enabled[j] = msg.entry_values[j].enabled[i] = type == AllowedCollision::ALWAYS;
+      AllowedCollision::Type type = AllowedCollision::NEVER;
+      getAllowedCollision(msg.entry_names[i], msg.entry_names[j], type);
+      msg.entry_values[i].enabled[j] = msg.entry_values[j].enabled[i] = (type == AllowedCollision::ALWAYS);
     }
   }
 }

From 66066b52a038b5221da4d0853322c6cd775a22ed Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Tue, 2 Nov 2021 11:56:02 +0100
Subject: [PATCH 057/114] ACM:print(): show default value

---
 .../collision_detection/src/collision_matrix.cpp   | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp
index 4a4105f080..367977991f 100644
--- a/moveit_core/collision_detection/src/collision_matrix.cpp
+++ b/moveit_core/collision_detection/src/collision_matrix.cpp
@@ -390,7 +390,7 @@ void AllowedCollisionMatrix::print(std::ostream& out) const
   // print indices along the top of the matrix
   for (std::size_t j = 0; j < number_digits; ++j)
   {
-    out << std::setw(spacing + number_digits + 4) << "";
+    out << std::setw(spacing + number_digits + 8) << "";
     for (std::size_t i = 0; i < names.size(); ++i)
     {
       std::stringstream ss;
@@ -400,17 +400,25 @@ void AllowedCollisionMatrix::print(std::ostream& out) const
     out << std::endl;
   }
 
+  const char* indicator = "01?";  // ALWAYS / NEVER / CONDITIONAL
   for (std::size_t i = 0; i < names.size(); ++i)
   {
     out << std::setw(spacing) << names[i];
     out << std::setw(number_digits + 1) << i;
     out << " | ";
+    // print default value
+    AllowedCollision::Type type;
+    if (getDefaultEntry(names[i], type))
+      out << indicator[type];
+    else
+      out << '-';
+    out << " | ";
+    // print pairs
     for (std::size_t j = 0; j < names.size(); ++j)
     {
-      AllowedCollision::Type type;
       bool found = getAllowedCollision(names[i], names[j], type);
       if (found)
-        out << std::setw(3) << (type == AllowedCollision::ALWAYS ? '1' : (type == AllowedCollision::NEVER ? '0' : '?'));
+        out << std::setw(3) << indicator[type];
       else
         out << std::setw(3) << '-';
     }

From 6679901a7e8d20315d33c3164b027747230ec7ff Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Wed, 22 Dec 2021 16:55:55 +0100
Subject: [PATCH 058/114] Adapt to API changes in srdfdom

@v4hn requested splitting of collision_pairs into (re)enabled and disabled.
---
 moveit_core/planning_scene/src/planning_scene.cpp         | 8 +++++---
 moveit_setup_assistant/src/collisions_updater.cpp         | 6 +++++-
 .../src/widgets/configuration_files_widget.cpp            | 2 +-
 .../src/widgets/default_collisions_widget.cpp             | 7 +++----
 4 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
index 6180b1d3ae..d209e89e75 100644
--- a/moveit_core/planning_scene/src/planning_scene.cpp
+++ b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -156,9 +156,11 @@ void PlanningScene::initialize()
   // load collision defaults
   for (const std::string& name : getRobotModel()->getSRDF()->getNoDefaultCollisionLinks())
     acm_->setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS);
-  // allow collisions for pairs that have been disabled
-  for (auto const& collision : getRobotModel()->getSRDF()->getCollisionPairs())
-    acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_);
+  // load collision pairs
+  for (auto const& collision : getRobotModel()->getSRDF()->getEnabledCollisionPairs())
+    acm_->setEntry(collision.link1_, collision.link2_, false);
+  for (auto const& collision : getRobotModel()->getSRDF()->getDisabledCollisionPairs())
+    acm_->setEntry(collision.link1_, collision.link2_, true);
 
   setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
 }
diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp
index b79f5cbd0e..48e2418cf7 100644
--- a/moveit_setup_assistant/src/collisions_updater.cpp
+++ b/moveit_setup_assistant/src/collisions_updater.cpp
@@ -97,7 +97,11 @@ bool setup(moveit_setup_assistant::MoveItConfigData& config_data, bool keep_old,
   }
 
   if (!keep_old)
-    config_data.srdf_->collision_pairs_.clear();
+  {
+    config_data.srdf_->no_default_collision_links_.clear();
+    config_data.srdf_->enabled_collision_pairs_.clear();
+    config_data.srdf_->disabled_collision_pairs_.clear();
+  }
 
   return true;
 }
diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
index 39f23c2e8d..a334b80b2f 100644
--- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
@@ -641,7 +641,7 @@ bool ConfigurationFilesWidget::checkDependencies()
   }
 
   // Check that at least 1 link pair is disabled from collision checking
-  if (config_data_->srdf_->collision_pairs_.empty())
+  if (config_data_->srdf_->disabled_collision_pairs_.empty())
   {
     dependencies << "No self-collisions have been disabled";
   }
diff --git a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp
index 3d80e6efa7..938d54505f 100644
--- a/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/default_collisions_widget.cpp
@@ -677,7 +677,7 @@ void DefaultCollisionsWidget::checkedFilterChanged()
 void DefaultCollisionsWidget::linkPairsToSRDF()
 {
   // reset the data in the SRDF Writer class
-  config_data_->srdf_->collision_pairs_.clear();
+  config_data_->srdf_->disabled_collision_pairs_.clear();
 
   // Create temp disabled collision
   srdf::Model::CollisionPair dc;
@@ -692,8 +692,7 @@ void DefaultCollisionsWidget::linkPairsToSRDF()
       dc.link1_ = pair_it->first.first;
       dc.link2_ = pair_it->first.second;
       dc.reason_ = moveit_setup_assistant::disabledReasonToString(pair_it->second.reason);
-      dc.disabled_ = pair_it->second.disable_check;
-      config_data_->srdf_->collision_pairs_.push_back(dc);
+      config_data_->srdf_->disabled_collision_pairs_.push_back(dc);
     }
   }
 
@@ -720,7 +719,7 @@ void DefaultCollisionsWidget::linkPairsFromSRDF()
   std::pair<std::string, std::string> link_pair;
 
   // Loop through all disabled collisions in SRDF and update the comprehensive list that has already been created
-  for (const auto& disabled_collision : config_data_->srdf_->collision_pairs_)
+  for (const auto& disabled_collision : config_data_->srdf_->disabled_collision_pairs_)
   {
     // Set the link names
     link_pair.first = disabled_collision.link1_;

From 47a66152d5b3559d147b0cf57c4c8bcca50a3931 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Tue, 28 Dec 2021 11:24:09 +0100
Subject: [PATCH 059/114] Unify initialization of ACM from SRDF

---
 .../moveit/collision_detection/collision_matrix.h |  3 +++
 .../test_collision_common_panda.h                 |  9 +--------
 .../collision_detection/src/collision_matrix.cpp  | 13 +++++++++++++
 .../test_bullet_continuous_collision_checking.cpp |  9 +--------
 moveit_core/planning_scene/src/planning_scene.cpp | 11 +----------
 .../src/tools/moveit_config_data.cpp              | 15 +++++++++------
 6 files changed, 28 insertions(+), 32 deletions(-)

diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
index eabfd12c18..b754a5bb48 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
@@ -88,6 +88,9 @@ class AllowedCollisionMatrix
    *  will be ignored. */
   AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed = false);
 
+  /** @brief Construct from an SRDF representation */
+  AllowedCollisionMatrix(const srdf::Model& srdf);
+
   /** @brief Construct the structure from a message representation */
   AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg);
 
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
index 9b0bf7097f..ac2c2ebf45 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
@@ -78,14 +78,7 @@ class CollisionDetectorPandaTest : public testing::Test
     robot_model_ = moveit::core::loadTestingRobotModel("panda");
     robot_model_ok_ = static_cast<bool>(robot_model_);
 
-    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
-
-    // load collision defaults
-    for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks())
-      acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER);
-    // allow collisions for pairs that have been disabled
-    for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs())
-      acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_);
+    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*robot_model_->getSRDF());
 
     cenv_ = value_->allocateEnv(robot_model_);
 
diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp
index 367977991f..08b192b486 100644
--- a/moveit_core/collision_detection/src/collision_matrix.cpp
+++ b/moveit_core/collision_detection/src/collision_matrix.cpp
@@ -51,6 +51,19 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& n
       setEntry(names[i], names[j], allowed);
 }
 
+AllowedCollisionMatrix::AllowedCollisionMatrix(const srdf::Model& srdf)
+{
+  // load collision defaults
+  for (const std::string& name : srdf.getNoDefaultCollisionLinks())
+    setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS);
+  // re-enable specific collision pairs
+  for (auto const& collision : srdf.getEnabledCollisionPairs())
+    setEntry(collision.link1_, collision.link2_, false);
+  // *finally* disable selected collision pairs
+  for (auto const& collision : srdf.getDisabledCollisionPairs())
+    setEntry(collision.link1_, collision.link2_, true);
+}
+
 AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg)
 {
   if (msg.entry_names.size() != msg.entry_values.size() ||
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 06c6300f14..ffda13ee80 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
@@ -76,14 +76,7 @@ class BulletCollisionDetectionTester : public testing::Test
     robot_model_ = moveit::core::loadTestingRobotModel("panda");
     robot_model_ok_ = static_cast<bool>(robot_model_);
 
-    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
-
-    // load collision defaults
-    for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks())
-      acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER);
-    // allow collisions for pairs that have been disabled
-    for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs())
-      acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_);
+    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*robot_model_->getSRDF());
 
     cenv_ = std::make_shared<collision_detection::CollisionEnvBullet>(robot_model_);
 
diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
index d209e89e75..9715267682 100644
--- a/moveit_core/planning_scene/src/planning_scene.cpp
+++ b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -151,16 +151,7 @@ void PlanningScene::initialize()
   robot_state_->setToDefaultValues();
   robot_state_->update();
 
-  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
-
-  // load collision defaults
-  for (const std::string& name : getRobotModel()->getSRDF()->getNoDefaultCollisionLinks())
-    acm_->setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS);
-  // load collision pairs
-  for (auto const& collision : getRobotModel()->getSRDF()->getEnabledCollisionPairs())
-    acm_->setEntry(collision.link1_, collision.link2_, false);
-  for (auto const& collision : getRobotModel()->getSRDF()->getDisabledCollisionPairs())
-    acm_->setEntry(collision.link1_, collision.link2_, true);
+  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*getRobotModel()->getSRDF());
 
   setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
 }
diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
index 3f8d65e4dd..a9f20cf945 100644
--- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp
+++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
@@ -140,14 +140,17 @@ planning_scene::PlanningScenePtr MoveItConfigData::getPlanningScene()
 // ******************************************************************************************
 void MoveItConfigData::loadAllowedCollisionMatrix()
 {
-  // Clear the allowed collision matrix
   allowed_collision_matrix_.clear();
 
-  // Update the allowed collision matrix, in case there has been a change
-  for (const auto& collision : srdf_->collision_pairs_)
-  {
-    allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, collision.disabled_);
-  }
+  // load collision defaults
+  for (const std::string& name : srdf_->no_default_collision_links_)
+    allowed_collision_matrix_.setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS);
+  // re-enable specific collision pairs
+  for (auto const& collision : srdf_->enabled_collision_pairs_)
+    allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, false);
+  // *finally* disable selected collision pairs
+  for (auto const& collision : srdf_->disabled_collision_pairs_)
+    allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, true);
 }
 
 // ******************************************************************************************

From 2a6641392208f1ac28b4420d6c1323e4713c6c56 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Tue, 28 Dec 2021 12:07:30 +0100
Subject: [PATCH 060/114] Move MoveItConfigData::setCollisionLinkPairs to
 collisions_updater.cpp

This method is only used there to update disabled collision entries.
---
 .../tools/moveit_config_data.h                |  7 ---
 .../src/collisions_updater.cpp                | 48 ++++++++++++++++-
 .../src/tools/moveit_config_data.cpp          | 53 -------------------
 3 files changed, 47 insertions(+), 61 deletions(-)

diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h
index 7ce6f0caae..050ec293bf 100644
--- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h
+++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h
@@ -330,13 +330,6 @@ class MoveItConfigData
    */
   std::string getGazeboCompatibleURDF();
 
-  /**
-   * \brief Set list of collision link pairs in SRDF; sorted; with optional filter
-   * \param link_pairs list of collision link pairs
-   * \param skip_mask mask of shifted moveit_setup_assistant::DisabledReason values that will be skipped
-   */
-  void setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap& link_pairs, size_t skip_mask = 0);
-
   /**
    * \brief Decide the best two joints to be used for the projection evaluator
    * \param planning_group name of group to use
diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp
index 48e2418cf7..3e6f36ad52 100644
--- a/moveit_setup_assistant/src/collisions_updater.cpp
+++ b/moveit_setup_assistant/src/collisions_updater.cpp
@@ -115,6 +115,52 @@ moveit_setup_assistant::LinkPairMap compute(moveit_setup_assistant::MoveItConfig
                                                           trials > 0, trials, min_collision_fraction, verbose);
 }
 
+// less operation for two CollisionPairs
+struct CollisionPairLess
+{
+  bool operator()(const srdf::Model::CollisionPair& left, const srdf::Model::CollisionPair& right) const
+  {
+    return left.link1_ < right.link1_ && left.link2_ < right.link2_;
+  }
+};
+
+// Update collision pairs
+void updateCollisionLinkPairs(std::vector<srdf::Model::CollisionPair>& target_pairs,
+                              const moveit_setup_assistant::LinkPairMap& source_pairs, size_t skip_mask)
+{
+  // remove duplicates
+  std::set<srdf::Model::CollisionPair, CollisionPairLess> filtered;
+  for (auto& p : target_pairs)
+  {
+    if (p.link1_ >= p.link2_)
+      std::swap(p.link1_, p.link2_);  // unify link1, link2 sorting
+    filtered.insert(p);
+  }
+
+  // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format
+  for (const auto& link_pair : source_pairs)
+  {
+    // Only copy those that are actually disabled
+    if (!link_pair.second.disable_check)
+      continue;
+
+    // Filter out pairs matching the skip_mask
+    if ((1 << link_pair.second.reason) & skip_mask)
+      continue;
+
+    srdf::Model::CollisionPair pair;
+    pair.link1_ = link_pair.first.first;
+    pair.link2_ = link_pair.first.second;
+    if (pair.link1_ >= pair.link2_)
+      std::swap(pair.link1_, pair.link2_);
+    pair.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason);
+
+    filtered.insert(pair);
+  }
+
+  target_pairs.assign(filtered.begin(), filtered.end());
+}
+
 int main(int argc, char* argv[])
 {
   std::string config_pkg_path;
@@ -205,7 +251,7 @@ int main(int argc, char* argv[])
   if (!include_always)
     skip_mask |= (1 << moveit_setup_assistant::ALWAYS);
 
-  config_data.setCollisionLinkPairs(link_pairs, skip_mask);
+  updateCollisionLinkPairs(config_data.srdf_->disabled_collision_pairs_, link_pairs, skip_mask);
 
   config_data.srdf_->writeSRDF(output_path.empty() ? config_data.srdf_path_ : output_path);
 
diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
index a9f20cf945..77208dc5bb 100644
--- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp
+++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
@@ -1203,59 +1203,6 @@ bool MoveItConfigData::outputJointLimitsYAML(const std::string& file_path)
   return true;  // file created successfully
 }
 
-// ******************************************************************************************
-// Set list of collision link pairs in SRDF; sorted; with optional filter
-// ******************************************************************************************
-
-class SortableCollisionPair
-{
-public:
-  SortableCollisionPair(const srdf::Model::CollisionPair& p)
-    : pair_(p), key_(p.link1_ < p.link2_ ? (p.link1_ + "|" + p.link2_) : (p.link2_ + "|" + p.link1_))
-  {
-  }
-  operator const srdf::Model::CollisionPair &() const
-  {
-    return pair_;
-  }
-  bool operator<(const SortableCollisionPair& other) const
-  {
-    return key_ < other.key_;
-  }
-
-private:
-  const srdf::Model::CollisionPair pair_;
-  const std::string key_;
-};
-
-void MoveItConfigData::setCollisionLinkPairs(const moveit_setup_assistant::LinkPairMap& link_pairs, size_t skip_mask)
-{
-  // Create temp disabled collision
-  srdf::Model::CollisionPair pair;
-
-  std::set<SortableCollisionPair> collision_pairs;
-  collision_pairs.insert(srdf_->collision_pairs_.begin(), srdf_->collision_pairs_.end());
-
-  // copy the data in this class's LinkPairMap datastructure to srdf::Model::CollisionPair format
-  for (const std::pair<const std::pair<std::string, std::string>, LinkPairData>& link_pair : link_pairs)
-  {
-    // Only copy those that are actually disabled
-    if (link_pair.second.disable_check)
-    {
-      if ((1 << link_pair.second.reason) & skip_mask)
-        continue;
-
-      pair.link1_ = link_pair.first.first;
-      pair.link2_ = link_pair.first.second;
-      pair.reason_ = moveit_setup_assistant::disabledReasonToString(link_pair.second.reason);
-
-      collision_pairs.insert(SortableCollisionPair(pair));
-    }
-  }
-
-  srdf_->collision_pairs_.assign(collision_pairs.begin(), collision_pairs.end());
-}
-
 // ******************************************************************************************
 // Decide the best two joints to be used for the projection evaluator
 // ******************************************************************************************

From 890a80e31a85336c4df734d9a33b51eb034cb0fa Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Michael=20G=C3=B6rner?= <me@v4hn.de>
Date: Wed, 29 Dec 2021 15:38:23 +0100
Subject: [PATCH 061/114] Disable slow robot_interaction tests in DEBUG mode
 (#3014)

These tests are known to run for a very long time in debug builds. So let's disable them in this case.
If you still insist to run them, you can do so via `locked_robot_state_test --gtest_also_run_disabled_tests`
---
 .../test/locked_robot_state_test.cpp          | 32 ++++++++++++-------
 1 file changed, 20 insertions(+), 12 deletions(-)

diff --git a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp
index 3e5b155768..f3448bb38b 100644
--- a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp
+++ b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp
@@ -551,62 +551,70 @@ TEST(LockedRobotState, set1)
   runThreads(1, 1, 0);
 }
 
-TEST(LockedRobotState, set2)
+// skip all more complex locking checks when optimizations are disabled
+// they can easily outrun 20 minutes with Debug builds
+#ifdef NDEBUG
+#define OPT_TEST(F, N) TEST(F, N)
+#else
+#define OPT_TEST(F, N) TEST(F, DISABLED_##N)
+#endif
+
+OPT_TEST(LockedRobotState, set2)
 {
   runThreads(1, 2, 0);
 }
 
-TEST(LockedRobotState, set3)
+OPT_TEST(LockedRobotState, set3)
 {
   runThreads(1, 3, 0);
 }
 
-TEST(LockedRobotState, mod1)
+OPT_TEST(LockedRobotState, mod1)
 {
   runThreads(1, 0, 1);
 }
 
-TEST(LockedRobotState, mod2)
+OPT_TEST(LockedRobotState, mod2)
 {
   runThreads(1, 0, 1);
 }
 
-TEST(LockedRobotState, mod3)
+OPT_TEST(LockedRobotState, mod3)
 {
   runThreads(1, 0, 1);
 }
 
-TEST(LockedRobotState, set1mod1)
+OPT_TEST(LockedRobotState, set1mod1)
 {
   runThreads(1, 1, 1);
 }
 
-TEST(LockedRobotState, set2mod1)
+OPT_TEST(LockedRobotState, set2mod1)
 {
   runThreads(1, 2, 1);
 }
 
-TEST(LockedRobotState, set1mod2)
+OPT_TEST(LockedRobotState, set1mod2)
 {
   runThreads(1, 1, 2);
 }
 
-TEST(LockedRobotState, set3mod1)
+OPT_TEST(LockedRobotState, set3mod1)
 {
   runThreads(1, 3, 1);
 }
 
-TEST(LockedRobotState, set1mod3)
+OPT_TEST(LockedRobotState, set1mod3)
 {
   runThreads(1, 1, 3);
 }
 
-TEST(LockedRobotState, set3mod3)
+OPT_TEST(LockedRobotState, set3mod3)
 {
   runThreads(1, 3, 3);
 }
 
-TEST(LockedRobotState, set3mod3c3)
+OPT_TEST(LockedRobotState, set3mod3c3)
 {
   runThreads(3, 3, 3);
 }

From 155eefa46e30d44caac9a7d6ec2ebae6eda88ffb Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 31 Dec 2021 02:07:03 +0100
Subject: [PATCH 062/114] 1.1.7

---
 moveit/CHANGELOG.rst                              |  3 +++
 moveit/package.xml                                |  2 +-
 moveit_commander/CHANGELOG.rst                    |  3 +++
 moveit_commander/package.xml                      |  2 +-
 moveit_core/CHANGELOG.rst                         | 15 +++++++++++++++
 moveit_core/package.xml                           |  2 +-
 moveit_kinematics/CHANGELOG.rst                   |  6 ++++++
 moveit_kinematics/package.xml                     |  2 +-
 .../chomp/chomp_interface/CHANGELOG.rst           |  5 +++++
 moveit_planners/chomp/chomp_interface/package.xml |  2 +-
 .../chomp/chomp_motion_planner/CHANGELOG.rst      |  3 +++
 .../chomp/chomp_motion_planner/package.xml        |  2 +-
 .../chomp/chomp_optimizer_adapter/CHANGELOG.rst   |  3 +++
 .../chomp/chomp_optimizer_adapter/package.xml     |  2 +-
 moveit_planners/moveit_planners/CHANGELOG.rst     |  5 +++++
 moveit_planners/moveit_planners/package.xml       |  2 +-
 moveit_planners/ompl/CHANGELOG.rst                |  6 ++++++
 moveit_planners/ompl/package.xml                  |  2 +-
 .../pilz_industrial_motion_planner/CHANGELOG.rst  |  6 ++++++
 .../pilz_industrial_motion_planner/package.xml    |  2 +-
 .../CHANGELOG.rst                                 |  5 +++++
 .../package.xml                                   |  2 +-
 .../moveit_fake_controller_manager/CHANGELOG.rst  |  5 +++++
 .../moveit_fake_controller_manager/package.xml    |  2 +-
 moveit_plugins/moveit_plugins/CHANGELOG.rst       |  3 +++
 moveit_plugins/moveit_plugins/package.xml         |  2 +-
 .../moveit_ros_control_interface/CHANGELOG.rst    |  3 +++
 .../moveit_ros_control_interface/package.xml      |  2 +-
 .../CHANGELOG.rst                                 |  6 ++++++
 .../moveit_simple_controller_manager/package.xml  |  2 +-
 moveit_ros/benchmarks/CHANGELOG.rst               |  6 ++++++
 moveit_ros/benchmarks/package.xml                 |  2 +-
 moveit_ros/manipulation/CHANGELOG.rst             |  5 +++++
 moveit_ros/manipulation/package.xml               |  2 +-
 moveit_ros/move_group/CHANGELOG.rst               |  5 +++++
 moveit_ros/move_group/package.xml                 |  2 +-
 moveit_ros/moveit_ros/CHANGELOG.rst               |  3 +++
 moveit_ros/moveit_ros/package.xml                 |  2 +-
 moveit_ros/moveit_servo/CHANGELOG.rst             |  3 +++
 moveit_ros/moveit_servo/package.xml               |  2 +-
 moveit_ros/occupancy_map_monitor/CHANGELOG.rst    |  5 +++++
 moveit_ros/occupancy_map_monitor/package.xml      |  2 +-
 moveit_ros/perception/CHANGELOG.rst               |  6 ++++++
 moveit_ros/perception/package.xml                 |  2 +-
 moveit_ros/planning/CHANGELOG.rst                 |  9 +++++++++
 moveit_ros/planning/package.xml                   |  2 +-
 moveit_ros/planning_interface/CHANGELOG.rst       |  7 +++++++
 moveit_ros/planning_interface/package.xml         |  2 +-
 moveit_ros/robot_interaction/CHANGELOG.rst        |  6 ++++++
 moveit_ros/robot_interaction/package.xml          |  2 +-
 moveit_ros/visualization/CHANGELOG.rst            | 13 +++++++++++++
 moveit_ros/visualization/package.xml              |  2 +-
 moveit_ros/warehouse/CHANGELOG.rst                |  5 +++++
 moveit_ros/warehouse/package.xml                  |  2 +-
 moveit_runtime/CHANGELOG.rst                      |  3 +++
 moveit_runtime/package.xml                        |  2 +-
 moveit_setup_assistant/CHANGELOG.rst              |  8 ++++++++
 moveit_setup_assistant/package.xml                |  2 +-
 58 files changed, 190 insertions(+), 29 deletions(-)

diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst
index ba31fd2f92..c227ce07f1 100644
--- a/moveit/CHANGELOG.rst
+++ b/moveit/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+
 1.1.6 (2021-11-06)
 ------------------
 
diff --git a/moveit/package.xml b/moveit/package.xml
index 9c6ee7c1b7..c168d1f847 100644
--- a/moveit/package.xml
+++ b/moveit/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Meta package that contains all essential package of MoveIt. Until Summer 2016 MoveIt had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See <a href = "http://discourse.ros.org/t/migration-to-one-github-repo-for-moveit/266/34">the detailed discussion for the merge of several repositories</a>.</description>
   <maintainer email="dave@picknik.ai">Dave Coleman</maintainer>
   <maintainer email="mferguson@fetchrobotics.com">Michael Ferguson</maintainer>
diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst
index 05021c05db..7c02f1e54d 100644
--- a/moveit_commander/CHANGELOG.rst
+++ b/moveit_commander/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_commander
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+
 1.1.6 (2021-11-06)
 ------------------
 * Use relative imports (`#2912 <https://github.com/ros-planning/moveit/issues/2912>`_)
diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml
index 68268eb3be..4970b27b70 100644
--- a/moveit_commander/package.xml
+++ b/moveit_commander/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_commander</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Python interfaces to MoveIt</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst
index f381b2f6ee..e79d02a577 100644
--- a/moveit_core/CHANGELOG.rst
+++ b/moveit_core/CHANGELOG.rst
@@ -2,6 +2,21 @@
 Changelog for package moveit_core
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 <https://github.com/ros-planning/moveit/issues/3009>`_)
+* Disable (flaky) timing tests in ``DEBUG`` mode (`#3012 <https://github.com/ros-planning/moveit/issues/3012>`_)
+* ``RobotState::attachBody``: Migrate to unique_ptr argument (`#3011 <https://github.com/ros-planning/moveit/issues/3011>`_)
+* Add API stress tests for ``TOTG``, fix undefined behavior (`#2957 <https://github.com/ros-planning/moveit/issues/2957>`_)
+* Do not assert on printTransform with non-isometry (`#3005 <https://github.com/ros-planning/moveit/issues/3005>`_)
+* Provide ``MOVEIT_VERSION_CHECK`` macro (`#2997 <https://github.com/ros-planning/moveit/issues/2997>`_)
+* Quietly use backward_cpp/ros if available (`#2988 <https://github.com/ros-planning/moveit/issues/2988>`_)
+* Allow restricting collision pairs to a group (`#2987 <https://github.com/ros-planning/moveit/issues/2987>`_)
+* Add backwards compatibility for old scene serialization format (`#2986 <https://github.com/ros-planning/moveit/issues/2986>`_)
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Add waypoint duration to the trajectory deep copy unit test (`#2961 <https://github.com/ros-planning/moveit/issues/2961>`_)
+* Contributors: AndyZe, Henning Kayser, Jafar Abdi, Jochen Sprickerhof, Michael Görner, Robert Haschke, Simon Schmeisser, Wolfgang Merkt, pvanlaar
+
 1.1.6 (2021-11-06)
 ------------------
 * Silent warning about invalid ``virtual_joint`` in Gazebo setups
diff --git a/moveit_core/package.xml b/moveit_core/package.xml
index 5e983e1c0f..a931da247a 100644
--- a/moveit_core/package.xml
+++ b/moveit_core/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_core</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Core libraries used by MoveIt</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="robot.moveit@gmail.com">Sachin Chitta</author>
diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst
index 042f1dc1bb..dc2d697b8f 100644
--- a/moveit_kinematics/CHANGELOG.rst
+++ b/moveit_kinematics/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_kinematics
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* ``round_collada_numbers.py``: python 2/3 compatibility (`#2983 <https://github.com/ros-planning/moveit/issues/2983>`_)
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof, Tomislav Bazina
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml
index 9ecae6dcf1..5584069d43 100644
--- a/moveit_kinematics/package.xml
+++ b/moveit_kinematics/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_kinematics</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Package for all inverse kinematics solvers in MoveIt</description>
 
   <author email="dave@picknik.ai">Dave Coleman</author>
diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst
index 7ce1926fe2..7d22b68618 100644
--- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst
+++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package chomp_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 <https://github.com/ros-planning/moveit/issues/3009>`_)
+* Contributors: Jafar Abdi
+
 1.1.6 (2021-11-06)
 ------------------
 * Use test_environment.launch in unittests (`#2949 <https://github.com/ros-planning/moveit/issues/2949>`_)
diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml
index a540ef8a77..a0c96bc8bf 100644
--- a/moveit_planners/chomp/chomp_interface/package.xml
+++ b/moveit_planners/chomp/chomp_interface/package.xml
@@ -2,7 +2,7 @@
   <name>moveit_planners_chomp</name>
   <description>The interface for using CHOMP within MoveIt</description>
 
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <author email="gjones@willowgarage.com">Gil Jones</author>
 
   <maintainer email="chitt@live.in">Chittaranjan Srinivas Swaminathan</maintainer>
diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst
index a0e95d0ff1..12ae16ab37 100644
--- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst
+++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package chomp_motion_planner
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml
index 3ddc837c28..6970ae123d 100644
--- a/moveit_planners/chomp/chomp_motion_planner/package.xml
+++ b/moveit_planners/chomp/chomp_motion_planner/package.xml
@@ -2,7 +2,7 @@
   <name>chomp_motion_planner</name>
   <description>chomp_motion_planner</description>
 
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <author email="gjones@willowgarage.com">Gil Jones</author>
   <author email="mail@mrinal.net">Mrinal Kalakrishnan</author>
 
diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst
index f157e261ab..1812ac2e32 100644
--- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst
+++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_chomp_optimizer_adapter
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml
index 964630594c..51f53726e7 100644
--- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml
+++ b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml
@@ -3,7 +3,7 @@
   <name>moveit_chomp_optimizer_adapter</name>
   <description>MoveIt planning request adapter utilizing chomp for solution optimization</description>
 
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <author email="raghavendersahdev@gmail.com">Raghavender Sahdev</author>
   <maintainer email="raghavendersahdev@gmail.com">Raghavender Sahdev</maintainer>
   <maintainer email="moveit_releasers@googlegroups.com">MoveIt Release Team</maintainer>
diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst
index 88bc0f818b..9aecb4ba65 100644
--- a/moveit_planners/moveit_planners/CHANGELOG.rst
+++ b/moveit_planners/moveit_planners/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package moveit_planners
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Add pilz planner to ``moveit_planners`` dependency
+* Contributors: v4hn
+
 1.1.6 (2021-11-06)
 ------------------
 
diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml
index 84d8c332db..9391ca11b7 100644
--- a/moveit_planners/moveit_planners/package.xml
+++ b/moveit_planners/moveit_planners/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_planners</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Metapacakge that installs all available planners for MoveIt</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="sachinc@willowgarage.com">Sachin Chitta</author>
diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst
index 74380e604a..81a6e61d70 100644
--- a/moveit_planners/ompl/CHANGELOG.rst
+++ b/moveit_planners/ompl/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_planners_ompl
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Use termination condition for simplification step (`#2981 <https://github.com/ros-planning/moveit/issues/2981>`_)
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof, Simon Schmeisser
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml
index cdd71debcc..f5ea3680b4 100644
--- a/moveit_planners/ompl/package.xml
+++ b/moveit_planners/ompl/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_planners_ompl</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>MoveIt interface to OMPL</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst
index 6db7860dbc..3074492399 100644
--- a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst
+++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package pilz_industrial_motion_planner
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Pilz planner: improve reporting of invalid start joints (`#3000 <https://github.com/ros-planning/moveit/issues/3000>`_)
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof, Robert Haschke, v4hn
+
 1.1.6 (2021-11-06)
 ------------------
 * Fix calculation of subframe offset (`#2890 <https://github.com/ros-planning/moveit/issues/2890>`_)
diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml
index 8c1b056703..5f41b9b65a 100644
--- a/moveit_planners/pilz_industrial_motion_planner/package.xml
+++ b/moveit_planners/pilz_industrial_motion_planner/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>pilz_industrial_motion_planner</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof.</description>
 
   <maintainer email="a.gutenkunst@pilz.de">Alexander Gutenkunst</maintainer>
diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst
index 3a52f57b77..584145a42c 100644
--- a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst
+++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package pilz_industrial_motion_planner_testutils
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof
+
 1.1.6 (2021-11-06)
 ------------------
 * clang-tidy: modernize-make-shared, modernize-make-unique (`#2762 <https://github.com/ros-planning/moveit/issues/2762>`_)
diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml
index 425b86d9b5..d004a501ed 100644
--- a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml
+++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>pilz_industrial_motion_planner_testutils</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Helper scripts and functionality to test industrial motion generation</description>
 
   <maintainer email="a.gutenkunst@pilz.de">Alexander Gutenkunst</maintainer>
diff --git a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst
index bd397a6b35..24b55d5018 100644
--- a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst
+++ b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package moveit_fake_controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_plugins/moveit_fake_controller_manager/package.xml b/moveit_plugins/moveit_fake_controller_manager/package.xml
index 90fb3a88ab..a8ce23a50f 100644
--- a/moveit_plugins/moveit_fake_controller_manager/package.xml
+++ b/moveit_plugins/moveit_fake_controller_manager/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_fake_controller_manager</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>A fake controller manager plugin for MoveIt.</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst
index 1f4b2c84ee..f4b6307e38 100644
--- a/moveit_plugins/moveit_plugins/CHANGELOG.rst
+++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_plugins
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+
 1.1.6 (2021-11-06)
 ------------------
 
diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml
index 322d113cbd..0ca1854d4a 100644
--- a/moveit_plugins/moveit_plugins/package.xml
+++ b/moveit_plugins/moveit_plugins/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_plugins</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Metapackage for MoveIt plugins.</description>
 
   <author email="mferguson@fetchrobotics.com">Michael Ferguson</author>
diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst
index 8fa38fd676..6f2019fe8b 100644
--- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst
+++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_ros_control_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml
index d8f3ad47bc..3b928cbe84 100644
--- a/moveit_plugins/moveit_ros_control_interface/package.xml
+++ b/moveit_plugins/moveit_ros_control_interface/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit_ros_control_interface</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>ros_control controller manager interface for MoveIt</description>
 
   <author email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</author>
diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst
index f6dff94a2f..054a0d18b3 100644
--- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst
+++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_simple_controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* ``simple_controller_manager``: add ``max_effort`` parameter to ``GripperCommand`` action (`#2984 <https://github.com/ros-planning/moveit/issues/2984>`_)
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof, Rick Staa
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml
index 86f1fa2e3b..45d89919bf 100644
--- a/moveit_plugins/moveit_simple_controller_manager/package.xml
+++ b/moveit_plugins/moveit_simple_controller_manager/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_simple_controller_manager</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>A generic, simple controller manager plugin for MoveIt.</description>
 
   <author email="mferguson@fetchrobotics.com">Michael Ferguson</author>
diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst
index 60ef11f3cf..4b074ba918 100644
--- a/moveit_ros/benchmarks/CHANGELOG.rst
+++ b/moveit_ros/benchmarks/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_ros_benchmarks
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Provide ``MOVEIT_VERSION_CHECK`` macro (`#2997 <https://github.com/ros-planning/moveit/issues/2997>`_)
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof, Robert Haschke
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml
index 8073661820..de34c71a84 100644
--- a/moveit_ros/benchmarks/package.xml
+++ b/moveit_ros/benchmarks/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_benchmarks</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
 
   <description>Enhanced tools for benchmarks in MoveIt</description>
 
diff --git a/moveit_ros/manipulation/CHANGELOG.rst b/moveit_ros/manipulation/CHANGELOG.rst
index 412b0702c4..99498e8c33 100644
--- a/moveit_ros/manipulation/CHANGELOG.rst
+++ b/moveit_ros/manipulation/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package moveit_ros_manipulation
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml
index 30792de339..ae6ab2c1d0 100644
--- a/moveit_ros/manipulation/package.xml
+++ b/moveit_ros/manipulation/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_manipulation</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Components of MoveIt used for manipulation</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst
index 2fd523bd35..1636a92805 100644
--- a/moveit_ros/move_group/CHANGELOG.rst
+++ b/moveit_ros/move_group/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package moveit_ros_move_group
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml
index b8a1bca95e..8d710c25c1 100644
--- a/moveit_ros/move_group/package.xml
+++ b/moveit_ros/move_group/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_move_group</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>The move_group node for MoveIt</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="robot.moveit@gmail.com">Sachin Chitta</author>
diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst
index 0fc302e0a1..15d87a877c 100644
--- a/moveit_ros/moveit_ros/CHANGELOG.rst
+++ b/moveit_ros/moveit_ros/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+
 1.1.6 (2021-11-06)
 ------------------
 
diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml
index f33313f1a4..5f4c52cfcd 100644
--- a/moveit_ros/moveit_ros/package.xml
+++ b/moveit_ros/moveit_ros/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Components of MoveIt that use ROS</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="robot.moveit@gmail.com">Sachin Chitta</author>
diff --git a/moveit_ros/moveit_servo/CHANGELOG.rst b/moveit_ros/moveit_servo/CHANGELOG.rst
index 48af78cadf..15d6a564f1 100644
--- a/moveit_ros/moveit_servo/CHANGELOG.rst
+++ b/moveit_ros/moveit_servo/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_servo
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+
 1.1.6 (2021-11-06)
 ------------------
 * Backport position limit enforcement from MoveIt2 (`#2898 <https://github.com/ros-planning/moveit/issues/2898>`_)
diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml
index bc27fc32de..baf4fcbd39 100644
--- a/moveit_ros/moveit_servo/package.xml
+++ b/moveit_ros/moveit_servo/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit_servo</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
 
   <description>Provides real-time manipulator Cartesian and joint servoing.</description>
 
diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst
index 8783014b85..e69d4cabc8 100644
--- a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst
+++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package moveit_ros_occupancy_map_monitor
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml
index 2daf9b3638..ef2dc0f6d1 100644
--- a/moveit_ros/occupancy_map_monitor/package.xml
+++ b/moveit_ros/occupancy_map_monitor/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit_ros_occupancy_map_monitor</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Components of MoveIt connecting to occupancy map</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst
index 2a07ed4e95..8f908dfd1c 100644
--- a/moveit_ros/perception/CHANGELOG.rst
+++ b/moveit_ros/perception/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_ros_perception
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Add ns for depth image & pointcloud octomap updaters (`#2916 <https://github.com/ros-planning/moveit/issues/2916>`_)
+* Contributors: Jochen Sprickerhof, Tim Redick
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml
index 7509d0f741..66ecf21bf0 100644
--- a/moveit_ros/perception/package.xml
+++ b/moveit_ros/perception/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_perception</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Components of MoveIt connecting to perception</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst
index 5d62ec94d7..a0a7957d4b 100644
--- a/moveit_ros/planning/CHANGELOG.rst
+++ b/moveit_ros/planning/CHANGELOG.rst
@@ -2,6 +2,15 @@
 Changelog for package moveit_ros_planning
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 <https://github.com/ros-planning/moveit/issues/3009>`_)
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* ``MoveitCpp``: Added ability to set path constraints for ``PlanningComponent`` (`#2959 <https://github.com/ros-planning/moveit/issues/2959>`_)
+* ``RDFLoader``: clear buffer before reading content (`#2963 <https://github.com/ros-planning/moveit/issues/2963>`_)
+* Reset markers on ``display_contacts`` topic for a new planning attempt (`#2944 <https://github.com/ros-planning/moveit/issues/2944>`_)
+* Contributors: Colin Kohler, Jafar Abdi, Jochen Sprickerhof, Rick Staa, Robert Haschke
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml
index aaae5c0805..9e054bf46a 100644
--- a/moveit_ros/planning/package.xml
+++ b/moveit_ros/planning/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="utf-8"?>
 <package format="2">
   <name>moveit_ros_planning</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Planning components of MoveIt that use ROS</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="robot.moveit@gmail.com">Sachin Chitta</author>
diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst
index 18f34c1b38..6188648952 100644
--- a/moveit_ros/planning_interface/CHANGELOG.rst
+++ b/moveit_ros/planning_interface/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package moveit_ros_planning_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 <https://github.com/ros-planning/moveit/issues/3009>`_)
+* Fix uninitialized ``RobotState`` in ``MoveGroupInterface`` (`#3008 <https://github.com/ros-planning/moveit/issues/3008>`_)
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Captain Yoshi, Jafar Abdi, Jochen Sprickerhof
+
 1.1.6 (2021-11-06)
 ------------------
 * Use test_environment.launch in unittests (`#2949 <https://github.com/ros-planning/moveit/issues/2949>`_)
diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml
index 058d0cf408..e3e7af3c6e 100644
--- a/moveit_ros/planning_interface/package.xml
+++ b/moveit_ros/planning_interface/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_ros_planning_interface</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Components of MoveIt that offer simpler interfaces to planning and execution</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst
index 5e4d171fdf..e4783aeade 100644
--- a/moveit_ros/robot_interaction/CHANGELOG.rst
+++ b/moveit_ros/robot_interaction/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package moveit_ros_robot_interaction
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Add marker for subgroups even if no endeffector is defined for them (`#2977 <https://github.com/ros-planning/moveit/issues/2977>`_)
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof, Michael Görner
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml
index a6f592ef81..0ed25abf0a 100644
--- a/moveit_ros/robot_interaction/package.xml
+++ b/moveit_ros/robot_interaction/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_robot_interaction</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Components of MoveIt that offer interaction via interactive markers</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst
index 4d1935f035..3a62029f4a 100644
--- a/moveit_ros/visualization/CHANGELOG.rst
+++ b/moveit_ros/visualization/CHANGELOG.rst
@@ -2,6 +2,19 @@
 Changelog for package moveit_ros_visualization
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Move ``MoveItErrorCode`` class to ``moveit_core`` (`#3009 <https://github.com/ros-planning/moveit/issues/3009>`_)
+* ``RobotState::attachBody``: Migrate to ``unique_ptr`` argument (`#3011 <https://github.com/ros-planning/moveit/issues/3011>`_)
+* Fix "ClassLoader: SEVERE WARNING" on reset of MPD (`#2925 <https://github.com/ros-planning/moveit/issues/2925>`_)
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Various fixes to MotionPlanning display (`#2944 <https://github.com/ros-planning/moveit/issues/2944>`_)
+
+  * Avoid flickering of the progress bar
+  * Joints widget: avoid flickering of the nullspace slider
+* Modernize: std::make_shared
+* Contributors: Jafar Abdi, JafarAbdi, Jochen Sprickerhof, Robert Haschke, pvanlaar
+
 1.1.6 (2021-11-06)
 ------------------
 * Re-initialize params, subscribers, and topics when the ``MoveGroupNS`` has changed (`#2922 <https://github.com/ros-planning/moveit/issues/2922>`_)
diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml
index f4933f51b4..4c795e7a2c 100644
--- a/moveit_ros/visualization/package.xml
+++ b/moveit_ros/visualization/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_visualization</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Components of MoveIt that offer visualization</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst
index 03a4958608..301e9bbf8a 100644
--- a/moveit_ros/warehouse/CHANGELOG.rst
+++ b/moveit_ros/warehouse/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package moveit_ros_warehouse
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Contributors: Jochen Sprickerhof
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml
index 0e56a433cd..6d797a33e3 100644
--- a/moveit_ros/warehouse/package.xml
+++ b/moveit_ros/warehouse/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_ros_warehouse</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>Components of MoveIt connecting to MongoDB</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>
diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst
index 87365e3bc5..37f472be78 100644
--- a/moveit_runtime/CHANGELOG.rst
+++ b/moveit_runtime/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_runtime
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+
 1.1.6 (2021-11-06)
 ------------------
 
diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml
index 0416e7fcd2..3408d3a568 100644
--- a/moveit_runtime/package.xml
+++ b/moveit_runtime/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit_runtime</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
   <description>moveit_runtime meta package contains MoveIt packages that are essential for its runtime (e.g. running MoveIt on robots).</description>
 
   <author email="gm130s@gmail.com">Isaac I. Y. Saito</author>
diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/CHANGELOG.rst
index 9486988fc9..b522b008f2 100644
--- a/moveit_setup_assistant/CHANGELOG.rst
+++ b/moveit_setup_assistant/CHANGELOG.rst
@@ -2,6 +2,14 @@
 Changelog for package moveit_setup_assistant
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.7 (2021-12-31)
+------------------
+* Pass xacro_args to both, urdf and srdf loading (`#3013 <https://github.com/ros-planning/moveit/issues/3013>`_)
+* Switch to ``std::bind`` (`#2967 <https://github.com/ros-planning/moveit/issues/2967>`_)
+* Update timestamp of .setup_assistant file when writing files (`#2964 <https://github.com/ros-planning/moveit/issues/2964>`_)
+* Upload controller_list for simple controller manager (`#2954 <https://github.com/ros-planning/moveit/issues/2954>`_)
+* Contributors: Jochen Sprickerhof, Rick Staa, Robert Haschke
+
 1.1.6 (2021-11-06)
 ------------------
 * Use newly introduced cmake macro ``moveit_build_options()`` from ``moveit_core``
diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml
index 456bb0559a..dee33a6a1a 100644
--- a/moveit_setup_assistant/package.xml
+++ b/moveit_setup_assistant/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_setup_assistant</name>
-  <version>1.1.6</version>
+  <version>1.1.7</version>
 
   <description>Generates a configuration package that makes it easy to use MoveIt</description>
 

From 2a71c9c936a2cdf3fe7a8cbd76a5ab01b2c67318 Mon Sep 17 00:00:00 2001
From: Jafar Abdi <cafer.abdi@gmail.com>
Date: Fri, 31 Dec 2021 14:56:33 +0300
Subject: [PATCH 063/114] collision_distance_field: Fix undefined behavior
 vector insertion (#3017)

Co-authored-by: andreas-botbuilt <94128674+andreas-botbuilt@users.noreply.github.com>
---
 .../src/collision_env_distance_field.cpp                      | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
index 5e2c34bfbe..135451c8c3 100644
--- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
+++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
@@ -958,8 +958,8 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache
       for (collision_detection::PosedBodyPointDecompositionVectorPtr& non_group_attached_body_decomposition :
            non_group_attached_body_decompositions)
       {
-        all_points.insert(all_points.end(), non_group_attached_body_decomposition->getCollisionPoints().begin(),
-                          non_group_attached_body_decomposition->getCollisionPoints().end());
+        const EigenSTL::vector_Vector3d collision_points = non_group_attached_body_decomposition->getCollisionPoints();
+        all_points.insert(all_points.end(), collision_points.begin(), collision_points.end());
       }
 
       dfce->distance_field_->addPointsToField(all_points);

From ac6b7d6be82fc272a1e65c557e227e5a4a2447c8 Mon Sep 17 00:00:00 2001
From: Jeroen <jeroendemaeyer@live.be>
Date: Tue, 4 Jan 2022 23:16:33 +0100
Subject: [PATCH 064/114] Fix deprecation warning in moveit_cpp (#3019)

Fixup for #3009.
---
 moveit_ros/planning/moveit_cpp/src/planning_component.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp
index 515e186394..bdb4e6bbf0 100644
--- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp
+++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp
@@ -111,7 +111,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet
   if (!joint_model_group_)
   {
     ROS_ERROR_NAMED(LOGNAME, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str());
-    last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME);
+    last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GROUP_NAME;
     return *last_plan_solution_;
   }
 
@@ -148,7 +148,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet
   if (current_goal_constraints_.empty())
   {
     ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request");
-    last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+    last_plan_solution_->error_code = moveit::core::MoveItErrorCode::INVALID_GOAL_CONSTRAINTS;
     return *last_plan_solution_;
   }
   req.goal_constraints = current_goal_constraints_;
@@ -161,7 +161,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet
   if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end())
   {
     ROS_ERROR_NAMED(LOGNAME, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str());
-    last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+    last_plan_solution_->error_code = moveit::core::MoveItErrorCode::FAILURE;
     return *last_plan_solution_;
   }
   const planning_pipeline::PlanningPipelinePtr pipeline =

From 30139c155ea6ccdf349c9f04dca6d09fd61a519e Mon Sep 17 00:00:00 2001
From: Martin Oehler <oehler@sim.tu-darmstadt.de>
Date: Thu, 6 Jan 2022 18:33:59 +0100
Subject: [PATCH 065/114] Fix wrong transform in distance fields'
 determineCollisionSpheres() (#3022)

---
 .../src/collision_distance_field_types.cpp                      | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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 2f80e15e38..6bdec239f4 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
@@ -52,7 +52,7 @@ collision_detection::determineCollisionSpheres(const bodies::Body* body, Eigen::
   body->computeBoundingCylinder(cyl);
   unsigned int num_points = ceil(cyl.length / (cyl.radius / 2.0));
   double spacing = cyl.length / ((num_points * 1.0) - 1.0);
-  relative_transform = body->getPose().inverse() * cyl.pose;
+  relative_transform = cyl.pose;
 
   for (unsigned int i = 1; i < num_points - 1; i++)
   {

From 834ebb9e976432509d8dc89d69a8abd90d312b25 Mon Sep 17 00:00:00 2001
From: Jafar Abdi <cafer.abdi@gmail.com>
Date: Thu, 6 Jan 2022 23:27:04 +0300
Subject: [PATCH 066/114] Make TimeParameterization classes polymorphic (#3021)

---
 .../iterative_spline_parameterization.h        |  6 +++---
 .../iterative_time_parameterization.h          |  6 +++---
 .../time_optimal_trajectory_generation.h       |  6 +++---
 .../time_parameterization.h                    | 18 ++++++++++++++++++
 .../src/time_optimal_trajectory_generation.cpp |  4 ----
 5 files changed, 27 insertions(+), 13 deletions(-)
 create mode 100644 moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h

diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h
index 92f14befdb..139989e8ab 100644
--- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h
+++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h
@@ -38,6 +38,7 @@
 #pragma once
 
 #include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/trajectory_processing/time_parameterization.h>
 
 namespace trajectory_processing
 {
@@ -67,14 +68,13 @@ namespace trajectory_processing
 /// velocity and acceleration limits, will result in a longer trajectory.
 /// If this is a problem, try retuning (increasing) the limits.
 ///
-class IterativeSplineParameterization
+class IterativeSplineParameterization : public TimeParameterization
 {
 public:
   IterativeSplineParameterization(bool add_points = true);
-  ~IterativeSplineParameterization() = default;
 
   bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0,
-                         const double max_acceleration_scaling_factor = 1.0) const;
+                         const double max_acceleration_scaling_factor = 1.0) const override;
 
 private:
   bool add_points_;  /// @brief If true, add two points to trajectory (first and last segments).
diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h
index b142f987a5..9863badda5 100644
--- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h
+++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h
@@ -37,19 +37,19 @@
 #pragma once
 
 #include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/trajectory_processing/time_parameterization.h>
 
 namespace trajectory_processing
 {
 /// \brief This class  modifies the timestamps of a trajectory to respect
 /// velocity and acceleration constraints.
-class IterativeParabolicTimeParameterization
+class IterativeParabolicTimeParameterization : public TimeParameterization
 {
 public:
   IterativeParabolicTimeParameterization(unsigned int max_iterations = 100, double max_time_change_per_it = .01);
-  ~IterativeParabolicTimeParameterization() = default;
 
   bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0,
-                         const double max_acceleration_scaling_factor = 1.0) const;
+                         const double max_acceleration_scaling_factor = 1.0) const override;
 
 private:
   unsigned int max_iterations_;    /// @brief maximum number of iterations to find solution
diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h
index 0ee4d13f41..cbbc6cd739 100644
--- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h
+++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h
@@ -41,6 +41,7 @@
 #include <Eigen/Core>
 #include <list>
 #include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/trajectory_processing/time_parameterization.h>
 
 namespace trajectory_processing
 {
@@ -159,15 +160,14 @@ class Trajectory
   mutable std::list<TrajectoryStep>::const_iterator cached_trajectory_segment_;
 };
 
-class TimeOptimalTrajectoryGeneration
+class TimeOptimalTrajectoryGeneration : public TimeParameterization
 {
 public:
   TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1,
                                   const double min_angle_change = 0.001);
-  ~TimeOptimalTrajectoryGeneration();
 
   bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0,
-                         const double max_acceleration_scaling_factor = 1.0) const;
+                         const double max_acceleration_scaling_factor = 1.0) const override;
 
 private:
   const double path_tolerance_;
diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h
new file mode 100644
index 0000000000..3a64960e54
--- /dev/null
+++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <moveit/robot_trajectory/robot_trajectory.h>
+
+namespace trajectory_processing
+{
+/**
+ * @brief Base class for trajectory parameterization algorithms
+ */
+class TimeParameterization
+{
+public:
+  virtual ~TimeParameterization() = default;
+  virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
+                                 const double max_velocity_scaling_factor = 1.0,
+                                 const double max_acceleration_scaling_factor = 1.0) const = 0;
+};
+}  // namespace trajectory_processing
diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
index 15aff331cd..016f0c4cfb 100644
--- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
+++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -861,10 +861,6 @@ TimeOptimalTrajectoryGeneration::TimeOptimalTrajectoryGeneration(const double pa
 {
 }
 
-TimeOptimalTrajectoryGeneration::~TimeOptimalTrajectoryGeneration()
-{
-}
-
 bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
                                                         const double max_velocity_scaling_factor,
                                                         const double max_acceleration_scaling_factor) const

From ea86018d20c2e558033d0f231c192d0f7626e03e Mon Sep 17 00:00:00 2001
From: Rick Staa <rick.staa@outlook.com>
Date: Fri, 7 Jan 2022 10:54:13 +0100
Subject: [PATCH 067/114] MSA: Add STOMP + OMPL-CHOMP configs (#2955)

- Add stomp planner to MSA
- Add OMPL-CHOMP planner to MSA
- Remove obsolete CHOMP parameters
- Update CHOMP config parameters to match code defaults
- Create CHOMP config via template (instead of code)

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
---
 .../chomp_interface/src/chomp_interface.cpp   |   6 -
 .../chomp_interface/test/chomp_planning.yaml  |   6 -
 .../chomp_motion_planner/chomp_parameters.h   |   6 -
 .../src/chomp_parameters.cpp                  |   6 -
 .../tools/moveit_config_data.h                |   2 +-
 .../src/tools/moveit_config_data.cpp          | 144 +++++++++++++++---
 .../widgets/configuration_files_widget.cpp    |  35 ++++-
 .../config/chomp_planning.yaml                |  18 +++
 .../ompl-chomp_planning_pipeline.launch.xml   |  19 +++
 .../launch/stomp_planning_pipeline.launch.xml |  25 +++
 10 files changed, 220 insertions(+), 47 deletions(-)
 create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml
 create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml
 create mode 100644 moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml

diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp
index 42f5f9072c..53a278ed0f 100644
--- a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp
+++ b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp
@@ -52,14 +52,9 @@ void CHOMPInterface::loadParams()
   nh_.param("obstacle_cost_weight", params_.obstacle_cost_weight_, 1.0);
   nh_.param("learning_rate", params_.learning_rate_, 0.01);
 
-  // nh_.param("add_randomness", params_.add_randomness_, false);
   nh_.param("smoothness_cost_velocity", params_.smoothness_cost_velocity_, 0.0);
   nh_.param("smoothness_cost_acceleration", params_.smoothness_cost_acceleration_, 1.0);
   nh_.param("smoothness_cost_jerk", params_.smoothness_cost_jerk_, 0.0);
-  // nh_.param("hmc_discretization", params_.hmc_discretization_, 0.01);
-  // nh_.param("hmc_stochasticity", params_.hmc_stochasticity_, 0.01);
-  // nh_.param("hmc_annealing_factor", params_.hmc_annealing_factor_, 0.99);
-  // nh_.param("use_hamiltonian_monte_carlo", params_.use_hamiltonian_monte_carlo_, false);
   nh_.param("ridge_factor", params_.ridge_factor_, 0.0);
   nh_.param("use_pseudo_inverse", params_.use_pseudo_inverse_, false);
   nh_.param("pseudo_inverse_ridge_factor", params_.pseudo_inverse_ridge_factor_, 1e-4);
@@ -70,7 +65,6 @@ void CHOMPInterface::loadParams()
     ROS_WARN("The param 'collision_clearence' has been renamed to 'collision_clearance', please update your config!");
   nh_.param("collision_clearance", params_.min_clearance_, 0.2);
   nh_.param("collision_threshold", params_.collision_threshold_, 0.07);
-  // nh_.param("random_jump_amount", params_.random_jump_amount_, 1.0);
   nh_.param("use_stochastic_descent", params_.use_stochastic_descent_, true);
   {
     params_.trajectory_initialization_method_ = "quintic-spline";
diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml b/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml
index d6681c1519..b6b2a82f11 100644
--- a/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml
+++ b/moveit_planners/chomp/chomp_interface/test/chomp_planning.yaml
@@ -5,14 +5,9 @@ smoothness_cost_weight: 0.1
 obstacle_cost_weight: 1.0
 learning_rate: 0.01
 animate_path: true
-add_randomness: false
 smoothness_cost_velocity: 0.0
 smoothness_cost_acceleration: 1.0
 smoothness_cost_jerk: 0.0
-hmc_discretization: 0.01
-hmc_stochasticity: 0.01
-hmc_annealing_factor: 0.99
-use_hamiltonian_monte_carlo: false
 ridge_factor: 0.0
 use_pseudo_inverse: false
 pseudo_inverse_ridge_factor: 1e-4
@@ -21,5 +16,4 @@ animate_endeffector_segment: "link3"
 joint_update_limit: 0.1
 collision_clearance: 0.2
 collision_threshold: 0.07
-random_jump_amount: 1.0
 use_stochastic_descent: true
diff --git a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h
index f39dbf55f0..31b61b9c42 100644
--- a/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h
+++ b/moveit_planners/chomp/chomp_motion_planner/include/chomp_motion_planner/chomp_parameters.h
@@ -78,14 +78,9 @@ class ChompParameters
   double smoothness_cost_velocity_;      /// variables associated with the cost in velocity
   double smoothness_cost_acceleration_;  /// variables associated with the cost in acceleration
   double smoothness_cost_jerk_;          /// variables associated with the cost in jerk
-  // bool add_randomness_;
-  // bool use_hamiltonian_monte_carlo_;
   bool use_stochastic_descent_;  /// set this to true/false if you want to use stochastic descent while optimizing the
                                  /// cost.
 
-  // double hmc_stochasticity_;
-  // double hmc_discretization_;
-  // double hmc_annealing_factor_;
   double ridge_factor_;  /// the noise added to the diagnal of the total quadratic cost matrix in the objective function
   bool use_pseudo_inverse_;             /// enable pseudo inverse calculations or not.
   double pseudo_inverse_ridge_factor_;  /// set the ridge factor if pseudo inverse is enabled
@@ -94,7 +89,6 @@ class ChompParameters
   double min_clearance_;        /// the minimum distance that needs to be maintained to avoid obstacles
   double collision_threshold_;  /// the collision threshold cost that needs to be mainted to avoid collisions
   bool filter_mode_;
-  // double random_jump_amount_;
 
   static const std::vector<std::string> VALID_INITIALIZATION_METHODS;
   std::string trajectory_initialization_method_;  /// trajectory initialization method to be specified
diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp
index f3d4829d04..1df4c54a12 100644
--- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp
+++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp
@@ -47,14 +47,9 @@ ChompParameters::ChompParameters()
   obstacle_cost_weight_ = 1.0;
   learning_rate_ = 0.01;
 
-  // add_randomness_ = false;
   smoothness_cost_velocity_ = 0.0;
   smoothness_cost_acceleration_ = 1.0;
   smoothness_cost_jerk_ = 0.0;
-  // hmc_discretization_ = 0.01;
-  // hmc_stochasticity_ = 0.01;
-  // hmc_annealing_factor_ = 0.99;
-  // use_hamiltonian_monte_carlo_ = false;
   ridge_factor_ = 0.0;
   use_pseudo_inverse_ = false;
   pseudo_inverse_ridge_factor_ = 1e-4;
@@ -62,7 +57,6 @@ ChompParameters::ChompParameters()
   joint_update_limit_ = 0.1;
   min_clearance_ = 0.2;
   collision_threshold_ = 0.07;
-  // random_jump_amount_ = 1.0;
   use_stochastic_descent_ = true;
   filter_mode_ = false;
   trajectory_initialization_method_ = std::string("quintic-spline");
diff --git a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h
index 7ce6f0caae..b99bafb50f 100644
--- a/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h
+++ b/moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h
@@ -309,7 +309,7 @@ class MoveItConfigData
   std::map<std::string, double> getInitialJoints() const;
   bool outputSetupAssistantFile(const std::string& file_path);
   bool outputOMPLPlanningYAML(const std::string& file_path);
-  bool outputCHOMPPlanningYAML(const std::string& file_path);
+  bool outputSTOMPPlanningYAML(const std::string& file_path);
   bool outputKinematicsYAML(const std::string& file_path);
   bool outputJointLimitsYAML(const std::string& file_path);
   bool outputFakeControllersYAML(const std::string& file_path);
diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
index 4535622c36..69f64b2922 100644
--- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp
+++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
@@ -286,31 +286,133 @@ bool MoveItConfigData::outputOMPLPlanningYAML(const std::string& file_path)
 }
 
 // ******************************************************************************************
-// Output CHOMP Planning config files
+// Output STOMP Planning config files
 // ******************************************************************************************
-bool MoveItConfigData::outputCHOMPPlanningYAML(const std::string& file_path)
+bool MoveItConfigData::outputSTOMPPlanningYAML(const std::string& file_path)
 {
   YAML::Emitter emitter;
+  emitter << YAML::BeginMap;
+
+  // Add STOMP default for every group
+  for (srdf::Model::Group& group : srdf_->groups_)
+  {
+    emitter << YAML::Key << "stomp/" + group.name_;
+    emitter << YAML::BeginMap;
+    emitter << YAML::Key << "group_name";
+    emitter << YAML::Value << group.name_;
+
+    emitter << YAML::Key << "optimization";
+    emitter << YAML::BeginMap;
+    emitter << YAML::Key << "num_timesteps";
+    emitter << YAML::Value << "60";
+    emitter << YAML::Key << "num_iterations";
+    emitter << YAML::Value << "40";
+    emitter << YAML::Key << "num_iterations_after_valid";
+    emitter << YAML::Value << "0";
+    emitter << YAML::Key << "num_rollouts";
+    emitter << YAML::Value << "30";
+    emitter << YAML::Key << "max_rollouts";
+    emitter << YAML::Value << "30";
+    emitter << YAML::Key << "initialization_method";
+    emitter << YAML::Value << "1";
+    emitter << YAML::Comment("[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]");
+    emitter << YAML::Key << "control_cost_weight";
+    emitter << YAML::Value << "0.0";
+    emitter << YAML::EndMap;
+
+    emitter << YAML::Key << "task";
+    emitter << YAML::BeginMap;
+
+    emitter << YAML::Key << "noise_generator";
+    emitter << YAML::BeginSeq;
+    emitter << YAML::BeginMap;
+    emitter << YAML::Key << "class";
+    emitter << YAML::Value << "stomp_moveit/NormalDistributionSampling";
+    emitter << YAML::Key << "stddev";
+    emitter << YAML::Flow;
+    const moveit::core::JointModelGroup* joint_model_group = getRobotModel()->getJointModelGroup(group.name_);
+    const std::vector<const moveit::core::JointModel*>& joint_models = joint_model_group->getActiveJointModels();
+    std::vector<float> stddev(joint_models.size(), 0.05);
+    emitter << YAML::Value << stddev;
+    emitter << YAML::EndMap;
+    emitter << YAML::EndSeq;
+
+    emitter << YAML::Key << "cost_functions";
+    emitter << YAML::BeginSeq;
+    emitter << YAML::BeginMap;
+    emitter << YAML::Key << "class";
+    emitter << YAML::Value << "stomp_moveit/CollisionCheck";
+    emitter << YAML::Key << "collision_penalty";
+    emitter << YAML::Value << "1.0";
+    emitter << YAML::Key << "cost_weight";
+    emitter << YAML::Value << "1.0";
+    emitter << YAML::Key << "kernel_window_percentage";
+    emitter << YAML::Value << "0.2";
+    emitter << YAML::Key << "longest_valid_joint_move";
+    emitter << YAML::Value << "0.05";
+    emitter << YAML::EndMap;
+    emitter << YAML::EndSeq;
+
+    emitter << YAML::Key << "noisy_filters";
+    emitter << YAML::BeginSeq;
+    emitter << YAML::BeginMap;
+    emitter << YAML::Key << "class";
+    emitter << YAML::Value << "stomp_moveit/JointLimits";
+    emitter << YAML::Key << "lock_start";
+    emitter << YAML::Value << "True";
+    emitter << YAML::Key << "lock_goal";
+    emitter << YAML::Value << "True";
+    emitter << YAML::EndMap;
+    emitter << YAML::BeginMap;
+    emitter << YAML::Key << "class";
+    emitter << YAML::Value << "stomp_moveit/MultiTrajectoryVisualization";
+    emitter << YAML::Key << "line_width";
+    emitter << YAML::Value << "0.02";
+    emitter << YAML::Key << "rgb";
+    emitter << YAML::Flow;
+    std::vector<float> noisy_filters_rgb{ 255, 255, 0 };
+    emitter << YAML::Value << noisy_filters_rgb;
+    emitter << YAML::Key << "marker_array_topic";
+    emitter << YAML::Value << "stomp_trajectories";
+    emitter << YAML::Key << "marker_namespace";
+    emitter << YAML::Value << "noisy";
+    emitter << YAML::EndMap;
+    emitter << YAML::EndSeq;
+
+    emitter << YAML::Key << "update_filters";
+    emitter << YAML::BeginSeq;
+    emitter << YAML::BeginMap;
+    emitter << YAML::Key << "class";
+    emitter << YAML::Value << "stomp_moveit/PolynomialSmoother";
+    emitter << YAML::Key << "poly_order";
+    emitter << YAML::Value << "6";
+    emitter << YAML::EndMap;
+    emitter << YAML::BeginMap;
+    emitter << YAML::Key << "class";
+    emitter << YAML::Value << "stomp_moveit/TrajectoryVisualization";
+    emitter << YAML::Key << "line_width";
+    emitter << YAML::Value << "0.05";
+    emitter << YAML::Key << "rgb";
+    emitter << YAML::Flow;
+    std::vector<float> update_filters_rgb{ 0, 191, 255 };
+    emitter << YAML::Value << update_filters_rgb;
+    emitter << YAML::Key << "error_rgb";
+    emitter << YAML::Flow;
+    std::vector<float> update_filters_error_rgb{ 255, 0, 0 };
+    emitter << YAML::Value << update_filters_error_rgb;
+    emitter << YAML::Key << "publish_intermediate";
+    emitter << YAML::Value << "True";
+    emitter << YAML::Key << "marker_topic";
+    emitter << YAML::Value << "stomp_trajectory";
+    emitter << YAML::Key << "marker_namespace";
+    emitter << YAML::Value << "optimized";
+    emitter << YAML::EndMap;
+    emitter << YAML::EndSeq;
+
+    emitter << YAML::EndMap;
+    emitter << YAML::EndMap;
+  }
 
-  emitter << YAML::Value << YAML::BeginMap;
-  emitter << YAML::Key << "planning_time_limit" << YAML::Value << "10.0";
-  emitter << YAML::Key << "max_iterations" << YAML::Value << "200";
-  emitter << YAML::Key << "max_iterations_after_collision_free" << YAML::Value << "5";
-  emitter << YAML::Key << "smoothness_cost_weight" << YAML::Value << "0.1";
-  emitter << YAML::Key << "obstacle_cost_weight" << YAML::Value << "1.0";
-  emitter << YAML::Key << "learning_rate" << YAML::Value << "0.01";
-  emitter << YAML::Key << "smoothness_cost_velocity" << YAML::Value << "0.0";
-  emitter << YAML::Key << "smoothness_cost_acceleration" << YAML::Value << "1.0";
-  emitter << YAML::Key << "smoothness_cost_jerk" << YAML::Value << "0.0";
-  emitter << YAML::Key << "ridge_factor" << YAML::Value << "0.01";
-  emitter << YAML::Key << "use_pseudo_inverse" << YAML::Value << "false";
-  emitter << YAML::Key << "pseudo_inverse_ridge_factor" << YAML::Value << "1e-4";
-  emitter << YAML::Key << "joint_update_limit" << YAML::Value << "0.1";
-  emitter << YAML::Key << "collision_clearance" << YAML::Value << "0.2";
-  emitter << YAML::Key << "collision_threshold" << YAML::Value << "0.07";
-  emitter << YAML::Key << "use_stochastic_descent" << YAML::Value << "true";
-  emitter << YAML::Key << "enable_failure_recovery" << YAML::Value << "true";
-  emitter << YAML::Key << "max_recovery_attempts" << YAML::Value << "5";
   emitter << YAML::EndMap;
 
   std::ofstream output_stream(file_path.c_str(), std::ios_base::trunc);
diff --git a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
index 2685d6d1d8..2d12593bdc 100644
--- a/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
+++ b/moveit_setup_assistant/src/widgets/configuration_files_widget.cpp
@@ -287,8 +287,16 @@ bool ConfigurationFilesWidget::loadGenFiles()
   // chomp_planning.yaml  --------------------------------------------------------------------------------------
   file.file_name_ = "chomp_planning.yaml";
   file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_);
+  template_path = config_data_->appendPaths(config_data_->template_package_path_, file.rel_path_);
   file.description_ = "Specifies which chomp planning plugin parameters to be used for the CHOMP planner";
-  file.gen_func_ = std::bind(&MoveItConfigData::outputCHOMPPlanningYAML, config_data_, std::placeholders::_1);
+  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  gen_files_.push_back(file);
+
+  // stomp_planning.yaml  --------------------------------------------------------------------------------------
+  file.file_name_ = "stomp_planning.yaml";
+  file.rel_path_ = config_data_->appendPaths(config_path, file.file_name_);
+  file.description_ = "Specifies which stomp planning plugin parameters to be used for the STOMP planner";
+  file.gen_func_ = boost::bind(&MoveItConfigData::outputSTOMPPlanningYAML, config_data_, _1);
   file.write_on_changes = MoveItConfigData::GROUPS;  // need to double check if this is actually correct!
   gen_files_.push_back(file);
 
@@ -447,6 +455,31 @@ bool ConfigurationFilesWidget::loadGenFiles()
   file.write_on_changes = 0;
   gen_files_.push_back(file);
 
+  // stomp_planning_pipeline.launch
+  // --------------------------------------------------------------------------------------
+  file.file_name_ = "stomp_planning_pipeline.launch.xml";
+  file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
+  template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
+  file.description_ = "Intended to be included in other launch files that require the STOMP planning plugin. Defines "
+                      "the proper plugin name on the parameter server and a default selection of planning request "
+                      "adapters.";
+  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.write_on_changes = 0;
+  gen_files_.push_back(file);
+
+  // ompl-chomp_planning_pipeline.launch.xml
+  // --------------------------------------------------------------------------------------
+  file.file_name_ = "ompl-chomp_planning_pipeline.launch.xml";
+  file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
+  template_path = config_data_->appendPaths(template_launch_path, file.file_name_);
+  file.description_ =
+      "Intended to be included in other launch files that require the OMPL-CHOMP planning plugins. Defines "
+      "the proper plugin name on the parameter server and a default selection of planning request "
+      "adapters.";
+  file.gen_func_ = boost::bind(&ConfigurationFilesWidget::copyTemplate, this, template_path, _1);
+  file.write_on_changes = 0;
+  gen_files_.push_back(file);
+
   // planning_pipeline.launch --------------------------------------------------------------------------------------
   file.file_name_ = "planning_pipeline.launch.xml";
   file.rel_path_ = config_data_->appendPaths(launch_path, file.file_name_);
diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml
new file mode 100644
index 0000000000..eb9c91225b
--- /dev/null
+++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/config/chomp_planning.yaml
@@ -0,0 +1,18 @@
+planning_time_limit: 10.0
+max_iterations: 200
+max_iterations_after_collision_free: 5
+smoothness_cost_weight: 0.1
+obstacle_cost_weight: 1.0
+learning_rate: 0.01
+smoothness_cost_velocity: 0.0
+smoothness_cost_acceleration: 1.0
+smoothness_cost_jerk: 0.0
+ridge_factor: 0.0
+use_pseudo_inverse: false
+pseudo_inverse_ridge_factor: 1e-4
+joint_update_limit: 0.1
+collision_clearance: 0.2
+collision_threshold: 0.07
+use_stochastic_descent: true
+enable_failure_recovery: false
+max_recovery_attempts: 5
diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml
new file mode 100644
index 0000000000..b078992417
--- /dev/null
+++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/ompl-chomp_planning_pipeline.launch.xml
@@ -0,0 +1,19 @@
+<launch>
+  <!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
+  <include file="$(find panda_moveit_config)/launch/ompl_planning_pipeline.launch.xml">
+    <arg name="planning_adapters" value="
+       default_planner_request_adapters/AddTimeParameterization
+       default_planner_request_adapters/FixWorkspaceBounds
+       default_planner_request_adapters/FixStartStateBounds
+       default_planner_request_adapters/FixStartStateCollision
+       default_planner_request_adapters/FixStartStatePathConstraints
+       chomp/OptimizerAdapter"
+       />
+  </include>
+
+  <!-- load chomp config -->
+  <rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>
+
+  <!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
+  <param name="trajectory_initialization_method" value="fillTrajectory"/>
+</launch>
diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml
new file mode 100644
index 0000000000..c1de09f874
--- /dev/null
+++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/launch/stomp_planning_pipeline.launch.xml
@@ -0,0 +1,25 @@
+<launch>
+  <!-- Stomp Plugin for MoveIt -->
+  <arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+  <arg name="jiggle_fraction" value="0.05" />
+  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
+  <arg name="planning_adapters"
+       default="default_planner_request_adapters/AddTimeParameterization
+                default_planner_request_adapters/FixWorkspaceBounds
+                default_planner_request_adapters/FixStartStateBounds
+                default_planner_request_adapters/FixStartStateCollision
+                default_planner_request_adapters/FixStartStatePathConstraints" />
+
+
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
+
+  <!-- Add MoveGroup capabilities specific to this pipeline -->
+  <!-- <param name="capabilities" value="" /> -->
+
+  <rosparam command="load" file="$(find [GENERATED_PACKAGE_NAME])/config/stomp_planning.yaml"/>
+</launch>

From 7da00c19bf99f9f6cb28a588dba3fd86d9dd669a Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 14 Jan 2022 09:08:16 +0100
Subject: [PATCH 068/114] Fix CI: update apt packages before install

---
 moveit_kinematics/test/test_ikfast_plugins.sh | 1 +
 1 file changed, 1 insertion(+)

diff --git a/moveit_kinematics/test/test_ikfast_plugins.sh b/moveit_kinematics/test/test_ikfast_plugins.sh
index 17f3102989..ac8806d854 100755
--- a/moveit_kinematics/test/test_ikfast_plugins.sh
+++ b/moveit_kinematics/test/test_ikfast_plugins.sh
@@ -9,6 +9,7 @@ set -e # fail script on error
 
 sudo update-alternatives --install /usr/bin/python python /usr/bin/python2 1
 sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 2
+sudo apt-get -q update
 
 if [ "$ROS_DISTRO" == "noetic" ]; then
 	sudo update-alternatives --set python /usr/bin/python3

From 024e663b97159b7866be98dc61a89c8f7a6e2ea5 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Mon, 17 Jan 2022 09:52:52 +0100
Subject: [PATCH 069/114] CI: Fetch srdfdom from source

---
 .github/workflows/upstream.rosinstall | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/.github/workflows/upstream.rosinstall b/.github/workflows/upstream.rosinstall
index 4582a6f25b..504fea52f7 100644
--- a/.github/workflows/upstream.rosinstall
+++ b/.github/workflows/upstream.rosinstall
@@ -6,3 +6,7 @@
     local-name: geometric_shapes
     uri: https://github.com/ros-planning/geometric_shapes.git
     version: noetic-devel
+- git:
+    local-name: srdfdom
+    uri: https://github.com/ubi-agni/srdfdom
+    version: noetic-devel

From 5bd7d986a493e96c023dd86e33ff3defd40b3345 Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatan@picknik.ai>
Date: Fri, 18 Feb 2022 21:46:52 +0300
Subject: [PATCH 070/114] Use galactic in main CI (#1077)

---
 .github/workflows/ci.yaml | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index 04671ccf1e..f4da39a5d6 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -16,12 +16,12 @@ jobs:
       fail-fast: false
       matrix:
         env:
-          - IMAGE: rolling-ci
+          - IMAGE: galactic-ci
             CCOV: true
-            ROS_DISTRO: rolling
-          - IMAGE: rolling-ci-testing
+            ROS_DISTRO: galactic
+          - IMAGE: galactic-ci-testing
             IKFAST_TEST: true
-            ROS_DISTRO: rolling
+            ROS_DISTRO: galactic
             CLANG_TIDY: pedantic
     env:
       CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy"

From ce148886c14ba55bb15d5569eee7dca0b5f0d74d Mon Sep 17 00:00:00 2001
From: Jean-Christophe Ruel <ruelj2@gmail.com>
Date: Mon, 21 Feb 2022 11:10:43 -0500
Subject: [PATCH 071/114] Fix mixed-up implementations in TfSubscription
 creation (#1073)

Co-authored-by: Jean-Christophe Ruel <jeanchristophe.ruel@elmec.ca>
---
 .../src/current_state_monitor_middleware_handle.cpp       | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp
index 3f44e08703..ff72d429fd 100644
--- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp
+++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp
@@ -88,15 +88,15 @@ bool CurrentStateMonitorMiddlewareHandle::sleepFor(const std::chrono::nanosecond
   return rclcpp::sleep_for(nanoseconds);
 }
 
-void CurrentStateMonitorMiddlewareHandle::createStaticTfSubscription(TfCallback callback)
+void CurrentStateMonitorMiddlewareHandle::createDynamicTfSubscription(TfCallback callback)
 {
-  static_transform_subscriber_ =
+  transform_subscriber_ =
       node_->create_subscription<tf2_msgs::msg::TFMessage>("/tf", tf2_ros::DynamicListenerQoS(), callback);
 }
 
-void CurrentStateMonitorMiddlewareHandle::createDynamicTfSubscription(TfCallback callback)
+void CurrentStateMonitorMiddlewareHandle::createStaticTfSubscription(TfCallback callback)
 {
-  transform_subscriber_ =
+  static_transform_subscriber_ =
       node_->create_subscription<tf2_msgs::msg::TFMessage>("/tf_static", tf2_ros::StaticListenerQoS(), callback);
 }
 

From 92c3e848dc113a6d75680ba0dfa6547f2218c42b Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Wed, 23 Feb 2022 06:50:35 -0600
Subject: [PATCH 072/114] [hybrid planning] Adjust planning scene locking
 (#1087)

* Create a copy of the planning scene. const robot state.

* Use LockedPlanningSceneRO over lockSceneRead()

* Use lambda function
---
 .../src/local_planner_component.cpp                   | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp
index f4b2ca46d0..feb36fa16e 100644
--- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp
+++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp
@@ -235,14 +235,13 @@ void LocalPlannerComponent::executeIteration()
     // If the planner received an action request and a global solution it starts to plan locally
     case LocalPlannerState::LOCAL_PLANNING_ACTIVE:
     {
-      // Read current planning scene
       planning_scene_monitor_->updateSceneWithCurrentState();
-      planning_scene_monitor_->lockSceneRead();  // LOCK planning scene
-      planning_scene::PlanningScenePtr planning_scene = planning_scene_monitor_->getPlanningScene();
-      planning_scene_monitor_->unlockSceneRead();  // UNLOCK planning scene
 
-      // Get current state
-      auto current_robot_state = planning_scene->getCurrentStateNonConst();
+      // Read current robot state
+      const moveit::core::RobotState current_robot_state = [this] {
+        planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor_);
+        return ls->getCurrentState();
+      }();
 
       // Check if the global goal is reached
       if (trajectory_operator_instance_->getTrajectoryProgress(current_robot_state) > PROGRESS_THRESHOLD)

From 419db3b3b0245e9857e212ef86e441e60a4b10e4 Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatan@picknik.ai>
Date: Thu, 3 Mar 2022 17:07:54 +0000
Subject: [PATCH 073/114] Compilation fixes for Jammy and bring back Rolling CI
 (#1095)

* Use jammy dockers and clang-format-12
* Fix unused depend, and move to python3-lxml
* add ompl to repos, fix versions and ogre
* Remove ogre keys
* Fix boolean node operator
* Stop building dockers on branch and fix servo null pointer
* update pre-commit to clang-format-12 and pre-commit fixes
* clang-format workaround and more pre-commit fixes
---
 .docker/ci/Dockerfile                                     | 4 ++--
 .docker/release/Dockerfile                                | 2 +-
 .github/workflows/ci.yaml                                 | 8 ++++----
 .github/workflows/format.yaml                             | 4 ++--
 .pre-commit-config.yaml                                   | 2 +-
 moveit2.repos                                             | 6 +++++-
 moveit_kinematics/package.xml                             | 2 +-
 .../test/unit_tests/src/unittest_planning_context.cpp     | 7 ++++---
 .../src/unittest_trajectory_generator_common.cpp          | 2 +-
 .../pick_place/src/approach_and_translate_stage.cpp       | 6 +++---
 .../include/moveit_servo/parameter_descriptor_builder.hpp | 1 +
 moveit_ros/moveit_servo/test/servo_launch_test_common.hpp | 2 +-
 .../planning_scene_monitor/planning_scene_monitor.h       | 4 ++--
 moveit_ros/visualization/package.xml                      | 1 -
 moveit_ros/warehouse/warehouse/src/broadcast.cpp          | 3 ++-
 moveit_ros/warehouse/warehouse/src/import_from_text.cpp   | 2 ++
 moveit_setup_assistant/package.xml                        | 1 -
 moveit_setup_assistant/src/collisions_updater.cpp         | 2 ++
 moveit_setup_assistant/src/setup_assistant_main.cpp       | 2 ++
 .../src/tools/compute_default_collisions.cpp              | 4 ++--
 moveit_setup_assistant/src/tools/moveit_config_data.cpp   | 4 ++--
 21 files changed, 40 insertions(+), 29 deletions(-)

diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile
index a9be862e96..484f2ab84f 100644
--- a/.docker/ci/Dockerfile
+++ b/.docker/ci/Dockerfile
@@ -2,7 +2,7 @@
 # ROS base image augmented with all MoveIt dependencies to use for CI
 
 ARG ROS_DISTRO=rolling
-FROM ros:${ROS_DISTRO}-ros-base-focal
+FROM ros:${ROS_DISTRO}-ros-base
 MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de
 
 ENV TERM xterm
@@ -25,7 +25,7 @@ RUN \
         # Some basic requirements
         wget git sudo curl \
         # Preferred build tools
-        clang clang-format-10 clang-tidy clang-tools \
+        clang clang-format-12 clang-tidy clang-tools \
         ccache && \
     #
     # Fetch all dependencies from moveit2.repos
diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile
index bc8e7b2796..16ae7de119 100644
--- a/.docker/release/Dockerfile
+++ b/.docker/release/Dockerfile
@@ -2,7 +2,7 @@
 # Full debian-based install of MoveIt using apt-get
 
 ARG ROS_DISTRO=rolling
-FROM ros:${ROS_DISTRO}-ros-base-focal
+FROM ros:${ROS_DISTRO}-ros-base
 MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de
 
 # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size
diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index f4da39a5d6..04671ccf1e 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -16,12 +16,12 @@ jobs:
       fail-fast: false
       matrix:
         env:
-          - IMAGE: galactic-ci
+          - IMAGE: rolling-ci
             CCOV: true
-            ROS_DISTRO: galactic
-          - IMAGE: galactic-ci-testing
+            ROS_DISTRO: rolling
+          - IMAGE: rolling-ci-testing
             IKFAST_TEST: true
-            ROS_DISTRO: galactic
+            ROS_DISTRO: rolling
             CLANG_TIDY: pedantic
     env:
       CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy"
diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml
index ddfa93f93a..542457d8ee 100644
--- a/.github/workflows/format.yaml
+++ b/.github/workflows/format.yaml
@@ -17,8 +17,8 @@ jobs:
     steps:
     - uses: actions/checkout@v2
     - uses: actions/setup-python@v2
-    - name: Install clang-format-10
-      run: sudo apt-get install clang-format-10
+    - name: Install clang-format-12
+      run: sudo apt-get install clang-format-12
     - uses: pre-commit/action@v2.0.3
       id: precommit
     - name: Upload pre-commit changes
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 962dc42249..3dd06105d2 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -42,7 +42,7 @@ repos:
       - id: clang-format
         name: clang-format
         description: Format files with ClangFormat.
-        entry: clang-format-10
+        entry: clang-format-12
         language: system
         files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
         args: ['-fallback-style=none', '-i']
diff --git a/moveit2.repos b/moveit2.repos
index 71ee4bf3e0..c74e6089ed 100644
--- a/moveit2.repos
+++ b/moveit2.repos
@@ -2,7 +2,7 @@ repositories:
   geometric_shapes:
     type: git
     url: https://github.com/ros-planning/geometric_shapes
-    version: 2.1.2
+    version: ros2
   moveit_msgs:
     type: git
     url: https://github.com/ros-planning/moveit_msgs
@@ -19,3 +19,7 @@ repositories:
     type: git
     url: https://github.com/ros-controls/ros2_control.git
     version: 2.1.0
+  ompl:
+    type: git
+    url: https://github.com/ompl/ompl.git
+    version: main
diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml
index dff7eb2fc3..1e4063900f 100644
--- a/moveit_kinematics/package.xml
+++ b/moveit_kinematics/package.xml
@@ -35,7 +35,7 @@
 
   <!-- some requirements of ikfast scripts -->
   <exec_depend>urdfdom</exec_depend> <!-- provides check_urdf -->
-  <exec_depend>python-lxml</exec_depend>
+  <exec_depend>python3-lxml</exec_depend>
 
   <test_depend>moveit_ros_planning</test_depend>
   <test_depend>moveit_resources_fanuc_description</test_depend>
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp
index 2987dfabbd..7e32289394 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp
@@ -124,7 +124,7 @@ class PlanningContextTest : public ::testing::Test
     planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(robot_model_));
     moveit::core::RobotState current_state(robot_model_);
     current_state.setToDefaultValues();
-    current_state.setJointGroupPositions(planning_group_, { 0, 1.57, 1.57, 0, 0.2, 0 });
+    current_state.setJointGroupPositions(planning_group_, std::vector<double>{ 0, 1.57, 1.57, 0, 0.2, 0 });
     scene->setCurrentState(current_state);
     planning_context_->setPlanningScene(scene);  // TODO Check what happens if this is missing
   }
@@ -148,8 +148,9 @@ class PlanningContextTest : public ::testing::Test
     // state state in joint space, used as initial positions, since IK does not
     // work at zero positions
     rstate.setJointGroupPositions(this->planning_group_,
-                                  { 4.430233957464225e-12, 0.007881892504574495, -1.8157263253868452,
-                                    1.1801525390026025e-11, 1.8236082178909834, 8.591793942969161e-12 });
+                                  std::vector<double>{ 4.430233957464225e-12, 0.007881892504574495, -1.8157263253868452,
+                                                       1.1801525390026025e-11, 1.8236082178909834,
+                                                       8.591793942969161e-12 });
     Eigen::Isometry3d start_pose(Eigen::Isometry3d::Identity());
     start_pose.translation() = Eigen::Vector3d(0.3, 0, 0.65);
     rstate.setFromIK(this->robot_model_->getJointModelGroup(this->planning_group_), start_pose);
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp
index 06eabc333a..385e802b1f 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp
@@ -142,7 +142,7 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test
     req_.max_acceleration_scaling_factor = 1.0;
     moveit::core::RobotState rstate(robot_model_);
     rstate.setToDefaultValues();
-    rstate.setJointGroupPositions(planning_group_, { 0, M_PI / 2, 0, M_PI / 2, 0, 0 });
+    rstate.setJointGroupPositions(planning_group_, std::vector<double>{ 0, M_PI / 2, 0, M_PI / 2, 0, 0 });
     rstate.setVariableVelocities(std::vector<double>(rstate.getVariableCount(), 0.0));
     moveit::core::robotStateToRobotStateMsg(rstate, req_.start_state, false);
     moveit_msgs::msg::Constraints goal_constraint;
diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp
index 7e32d25d42..d31bab144b 100644
--- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp
+++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp
@@ -147,9 +147,9 @@ bool executeAttachObject(const ManipulationPlanSharedDataConstPtr& shared_plan_d
     ok = ps->processAttachedCollisionObjectMsg(msg);
   }
   motion_plan->planning_scene_monitor_->triggerSceneUpdateEvent(
-      (planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType)(
-          planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY +
-          planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE));
+      (planning_scene_monitor::PlanningSceneMonitor::
+           SceneUpdateType)(planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY +
+                            planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE));
   return ok;
 }
 
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp b/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp
index 341aa796d8..6977a0d3e2 100644
--- a/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp
+++ b/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp
@@ -40,6 +40,7 @@
 #include <rcl_interfaces/msg/integer_range.hpp>
 #include <rcl_interfaces/msg/parameter_descriptor.hpp>
 #include <rcl_interfaces/msg/parameter_type.hpp>
+#include <limits>
 #include <string>
 
 namespace moveit_servo
diff --git a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp
index 3680e01d00..ad5073cd23 100644
--- a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp
+++ b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp
@@ -113,7 +113,7 @@ class ServoFixture : public ::testing::Test
     // Otherwise the Servo is still running when another test starts...
     if (!client_servo_stop_)
     {
-      client_servo_stop_->async_send_request(std::make_shared<std_srvs::srv::Trigger::Request>());
+      stop();
     }
     executor_->cancel();
     if (executor_thread_.joinable())
diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
index 46a837d9df..32e48a589f 100644
--- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
+++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
@@ -648,7 +648,7 @@ class LockedPlanningSceneRO
     return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
   }
 
-  operator const planning_scene::PlanningSceneConstPtr &() const
+  operator const planning_scene::PlanningSceneConstPtr&() const
   {
     return static_cast<const PlanningSceneMonitor*>(planning_scene_monitor_.get())->getPlanningScene();
   }
@@ -729,7 +729,7 @@ class LockedPlanningSceneRW : public LockedPlanningSceneRO
   {
   }
 
-  operator const planning_scene::PlanningScenePtr &()
+  operator const planning_scene::PlanningScenePtr&()
   {
     return planning_scene_monitor_->getPlanningScene();
   }
diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml
index 2de19a78d4..07a82d9784 100644
--- a/moveit_ros/visualization/package.xml
+++ b/moveit_ros/visualization/package.xml
@@ -25,7 +25,6 @@
 
   <build_depend>class_loader</build_depend>
   <build_depend>eigen</build_depend>
-  <build_depend>libogre-dev</build_depend>
   <build_depend>libqt5-opengl-dev</build_depend>
   <build_depend>qtbase5-dev</build_depend>
 
diff --git a/moveit_ros/warehouse/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/warehouse/src/broadcast.cpp
index 5a340ae438..eafc03c17a 100644
--- a/moveit_ros/warehouse/warehouse/src/broadcast.cpp
+++ b/moveit_ros/warehouse/warehouse/src/broadcast.cpp
@@ -68,6 +68,7 @@ int main(int argc, char** argv)
   // time to wait in between publishing messages
   double delay = 0.001;
 
+  // clang-format off
   boost::program_options::options_description desc;
   desc.add_options()("help", "Show help message")("host", boost::program_options::value<std::string>(),
                                                   "Host for the "
@@ -80,7 +81,7 @@ int main(int argc, char** argv)
       "state", boost::program_options::value<std::string>(),
       "Name of the robot state to publish.")("delay", boost::program_options::value<double>()->default_value(delay),
                                              "Time to wait in between publishing messages (s)");
-
+  // clang-format on
   boost::program_options::variables_map vm;
   boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
   boost::program_options::notify(vm);
diff --git a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp
index c1d56be830..0ca7394679 100644
--- a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp
+++ b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp
@@ -216,12 +216,14 @@ int main(int argc, char** argv)
   node_options.automatically_declare_parameters_from_overrides(true);
   rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("import_from_text_to_warehouse", node_options);
 
+  // clang-format off
   boost::program_options::options_description desc;
   desc.add_options()("help", "Show help message")("queries", boost::program_options::value<std::string>(),
                                                   "Name of file containing motion planning queries.")(
       "scene", boost::program_options::value<std::string>(), "Name of file containing motion planning scene.")(
       "host", boost::program_options::value<std::string>(),
       "Host for the DB.")("port", boost::program_options::value<std::size_t>(), "Port for the DB.");
+  // clang-format on
 
   boost::program_options::variables_map vm;
   boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml
index 6f74c84ad9..fb1192ed95 100644
--- a/moveit_setup_assistant/package.xml
+++ b/moveit_setup_assistant/package.xml
@@ -20,7 +20,6 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <build_depend>moveit_common</build_depend>
-  <build_depend>libogre-dev</build_depend>
   <build_depend>ompl</build_depend>
 
   <depend>ament_index_cpp</depend>
diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp
index 5264d7872e..35533fa6d9 100644
--- a/moveit_setup_assistant/src/collisions_updater.cpp
+++ b/moveit_setup_assistant/src/collisions_updater.cpp
@@ -127,6 +127,7 @@ int main(int argc, char* argv[])
 
   uint32_t never_trials = 0;
 
+  // clang-format off
   po::options_description desc("Allowed options");
   desc.add_options()("help", "show help")("config-pkg", po::value(&config_pkg_path), "path to MoveIt config package")(
       "urdf", po::value(&urdf_path), "path to URDF ( or xacro)")("srdf", po::value(&srdf_path),
@@ -141,6 +142,7 @@ int main(int argc, char* argv[])
                                                               "number of trials for searching never colliding pairs")(
       "min-collision-fraction", po::value(&min_collision_fraction),
       "fraction of small sample size to determine links that are always colliding");
+  // clang-format on
 
   po::positional_options_description pos_desc;
   pos_desc.add("xacro-args", -1);
diff --git a/moveit_setup_assistant/src/setup_assistant_main.cpp b/moveit_setup_assistant/src/setup_assistant_main.cpp
index 7865772c6c..2a22178710 100644
--- a/moveit_setup_assistant/src/setup_assistant_main.cpp
+++ b/moveit_setup_assistant/src/setup_assistant_main.cpp
@@ -59,10 +59,12 @@ int main(int argc, char** argv)
   namespace po = boost::program_options;
 
   // Declare the supported options
+  // clang-format off
   po::options_description desc("Allowed options");
   desc.add_options()("help,h", "Show help message")("debug,g", "Run in debug/test mode")(
       "urdf_path,u", po::value<std::string>(), "Optional, path to URDF file in ROS package")(
       "config_pkg,c", po::value<std::string>(), "Optional, pass in existing config package to load");
+  // clang-format on
 
   // Process options
   po::variables_map vm;
diff --git a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp
index e21eb7eeb1..b6f2cd8fed 100644
--- a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp
+++ b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp
@@ -52,8 +52,8 @@ const boost::unordered_map<DisabledReason, std::string> REASONS_TO_STRING = boos
     DEFAULT, "Default")(ADJACENT, "Adjacent")(ALWAYS, "Always")(USER, "User")(NOT_DISABLED, "Not Disabled");
 
 const boost::unordered_map<std::string, DisabledReason> REASONS_FROM_STRING =
-    boost::assign::map_list_of("Never", NEVER)("Default", DEFAULT)("Adjacent", ADJACENT)("Always", ALWAYS)(
-        "User", USER)("Not Disabled", NOT_DISABLED);
+    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");
diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
index 491b5a6d7b..5542eebfeb 100644
--- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp
+++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp
@@ -1207,7 +1207,7 @@ class SortableDisabledCollision
     : dc_(dc), key_(dc.link1_ < dc.link2_ ? (dc.link1_ + "|" + dc.link2_) : (dc.link2_ + "|" + dc.link1_))
   {
   }
-  operator const srdf::Model::DisabledCollision &() const
+  operator const srdf::Model::DisabledCollision&() const
   {
     return dc_;
   }
@@ -1287,7 +1287,7 @@ template <typename T>
 bool parse(const YAML::Node& node, const std::string& key, T& storage, const T& default_value = T())
 {
   const YAML::Node& n = node[key];
-  bool valid = n;
+  bool valid = n.IsDefined();
   storage = valid ? n.as<T>() : default_value;
   return valid;
 }

From 2c75c9f818e8f30fc094a3a38b429e08e2159b07 Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatan@picknik.ai>
Date: Thu, 10 Mar 2022 13:48:02 +0000
Subject: [PATCH 074/114] Temporarily add galactic CI (#1107)

* Add galactic CI
* Comment out rolling
* panda_ros_controllers -> panda_ros2_controllers
* Ignore flake8 tests
---
 .github/workflows/ci.yaml                            | 12 ++++++++----
 .../launch/pose_tracking_example.launch.py           |  2 +-
 .../moveit_servo/launch/servo_example.launch.py      |  2 +-
 .../test/launch/servo_launch_test_common.py          |  2 +-
 .../test/launch/test_servo_pose_tracking.test.py     |  2 +-
 moveit_ros/planning/CMakeLists.txt                   |  1 +
 .../test/launch/move_group_launch_test_common.py     |  2 +-
 7 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index 04671ccf1e..253a367145 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -16,12 +16,16 @@ jobs:
       fail-fast: false
       matrix:
         env:
-          - IMAGE: rolling-ci
+          # - IMAGE: rolling-ci
+          #   ROS_DISTRO: rolling
+          # - IMAGE: rolling-ci-testing
+          #   ROS_DISTRO: rolling
+          - IMAGE: galactic-ci
             CCOV: true
-            ROS_DISTRO: rolling
-          - IMAGE: rolling-ci-testing
+            ROS_DISTRO: galactic
+          - IMAGE: galactic-ci-testing
+            ROS_DISTRO: galactic
             IKFAST_TEST: true
-            ROS_DISTRO: rolling
             CLANG_TIDY: pedantic
     env:
       CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy"
diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py
index 735608c89f..c6908b3397 100644
--- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py
+++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py
@@ -69,7 +69,7 @@ def generate_launch_description():
     ros2_controllers_path = os.path.join(
         get_package_share_directory("moveit_resources_panda_moveit_config"),
         "config",
-        "panda_ros_controllers.yaml",
+        "panda_ros2_controllers.yaml",
     )
     ros2_control_node = Node(
         package="controller_manager",
diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/servo_example.launch.py
index 640412e0ce..6cebcab884 100644
--- a/moveit_ros/moveit_servo/launch/servo_example.launch.py
+++ b/moveit_ros/moveit_servo/launch/servo_example.launch.py
@@ -70,7 +70,7 @@ def generate_launch_description():
     ros2_controllers_path = os.path.join(
         get_package_share_directory("moveit_resources_panda_moveit_config"),
         "config",
-        "panda_ros_controllers.yaml",
+        "panda_ros2_controllers.yaml",
     )
     ros2_control_node = Node(
         package="controller_manager",
diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py
index a4a819ecdc..0fba099b66 100644
--- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py
+++ b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py
@@ -47,7 +47,7 @@ def generate_servo_test_description(
     ros2_controllers_path = os.path.join(
         get_package_share_directory("moveit_resources_panda_moveit_config"),
         "config",
-        "panda_ros_controllers.yaml",
+        "panda_ros2_controllers.yaml",
     )
     ros2_control_node = Node(
         package="controller_manager",
diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py
index 62c09b163c..8b689aa48c 100644
--- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py
+++ b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py
@@ -34,7 +34,7 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType):
     ros2_controllers_path = os.path.join(
         get_package_share_directory("moveit_resources_panda_moveit_config"),
         "config",
-        "panda_ros_controllers.yaml",
+        "panda_ros2_controllers.yaml",
     )
     ros2_control_node = Node(
         package="controller_manager",
diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt
index aa51d0cde9..95e55bd783 100644
--- a/moveit_ros/planning/CMakeLists.txt
+++ b/moveit_ros/planning/CMakeLists.txt
@@ -110,6 +110,7 @@ if(BUILD_TESTING)
   set(ament_cmake_cppcheck_FOUND TRUE)
   set(ament_cmake_cpplint_FOUND TRUE)
   set(ament_cmake_uncrustify_FOUND TRUE)
+  set(ament_cmake_flake8_FOUND TRUE)
 
   # Run all lint tests in package.xml except those listed above
   ament_lint_auto_find_test_dependencies()
diff --git a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py
index 2c08c686c2..712022babf 100644
--- a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py
+++ b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py
@@ -55,7 +55,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp
     ros2_controllers_path = os.path.join(
         get_package_share_directory("moveit_resources_panda_moveit_config"),
         "config",
-        "panda_ros_controllers.yaml",
+        "panda_ros2_controllers.yaml",
     )
     ros2_control_node = Node(
         package="controller_manager",

From 6a2a2c24abf3816a90b990ce9cbb31915dd29cdf Mon Sep 17 00:00:00 2001
From: "David V. Lu!!" <davidvlu@gmail.com>
Date: Thu, 10 Mar 2022 16:08:42 -0500
Subject: [PATCH 075/114] [moveit_configs_utils] Minor fixes (#1103)

---
 .../moveit_configs_utils/moveit_configs_builder.py        | 2 +-
 moveit_configs_utils/package.xml                          | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
index d5304a79de..96a2040618 100644
--- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
+++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
@@ -34,7 +34,7 @@
     # Or to get all the parameters as a dictionary
     moveit_configs.to_dict()
 
-Each function in MoveItConfigsBuilder have a file_path as an argument which is used to override the default
+Each function in MoveItConfigsBuilder has a file_path as an argument which is used to override the default
 path for the file
 
 Example:
diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml
index b1b7075397..c26caf88a9 100644
--- a/moveit_configs_utils/package.xml
+++ b/moveit_configs_utils/package.xml
@@ -4,16 +4,16 @@
   <name>moveit_configs_utils</name>
   <version>2.3.0</version>
   <description>Python library for loading moveit config parameters in launch files</description>
-  <author email="jafar@picknik.ai">Jafar Abdi</author>
   <maintainer email="moveit_releasers@googlegroups.com">MoveIt Release Team</maintainer>
   <license>BSD</license>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
+  <author email="jafar@picknik.ai">Jafar Abdi</author>
 
   <exec_depend>ament_index_python</exec_depend>
   <exec_depend>launch_param_builder</exec_depend>
 
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
   <export>
     <build_type>ament_python</build_type>
   </export>

From 44ff32b6bc7fccf0dbf1476a63227a7fafc89e74 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Thu, 10 Mar 2022 22:11:12 -0600
Subject: [PATCH 076/114] Make lockSceneRead() and lockSceneWrite() protected
 member functions (#1100)

* No lock in planning_component.cpp

* Make lockSceneRead(), lockSceneWrite() protected

* Add a migration note
---
 MIGRATION.md                                  |  5 ++++
 .../moveit_cpp/src/planning_component.cpp     | 10 ++++----
 .../planning_scene_monitor.h                  | 25 +++++++++++--------
 3 files changed, 24 insertions(+), 16 deletions(-)

diff --git a/MIGRATION.md b/MIGRATION.md
index c18792e3aa..c80780cf70 100644
--- a/MIGRATION.md
+++ b/MIGRATION.md
@@ -3,6 +3,11 @@
 API changes in MoveIt releases
 
 ## ROS Rolling
+- `lockSceneRead()` and `lockSceneWrite()` are now protected member functions, for internal use only. To lock the planning scene, use LockedPlanningSceneRO or LockedPlanningSceneRW:
+```
+      planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor);
+      moveit::core::RobotModelConstPtr model = ls->getRobotModel();
+```
 - ServoServer was renamed to ServoNode
 - `CollisionObject` messages are now defined with a `Pose`, and shapes and subframes are defined relative to the object's pose. This makes it easier to place objects with subframes and multiple shapes in the scene. This causes several changes:
     - `getFrameTransform()` now returns this pose instead of the first shape's pose.
diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp
index d66bb15c6e..2c5bba8200 100644
--- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp
+++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp
@@ -127,11 +127,11 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor =
       moveit_cpp_->getPlanningSceneMonitorNonConst();
   planning_scene_monitor->updateFrameTransforms();
-  planning_scene_monitor->lockSceneRead();  // LOCK planning scene
-  planning_scene::PlanningScenePtr planning_scene =
-      planning_scene::PlanningScene::clone(planning_scene_monitor->getPlanningScene());
-  planning_scene_monitor->unlockSceneRead();  // UNLOCK planning scene
-  planning_scene_monitor.reset();             // release this pointer
+  const planning_scene::PlanningScenePtr planning_scene = [planning_scene_monitor] {
+    planning_scene_monitor::LockedPlanningSceneRO ls(planning_scene_monitor);
+    return planning_scene::PlanningScene::clone(ls);
+  }();
+  planning_scene_monitor.reset();  // release this pointer
 
   // Init MotionPlanRequest
   ::planning_interface::MotionPlanRequest req;
diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
index 32e48a589f..8a67220ef0 100644
--- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
+++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
@@ -404,6 +404,17 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost:
    */
   bool waitForCurrentRobotState(const rclcpp::Time& t, double wait_time = 1.);
 
+  void clearOctomap();
+
+  // Called to update the planning scene with a new message.
+  bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene);
+
+protected:
+  /** @brief Initialize the planning scene monitor
+   *  @param scene The scene instance to fill with data (an instance is allocated if the one passed in is not allocated)
+   */
+  void initialize(const planning_scene::PlanningScenePtr& scene);
+
   /** \brief Lock the scene for reading (multiple threads can lock for reading at the same time) */
   void lockSceneRead();
 
@@ -418,17 +429,6 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost:
    */
   void unlockSceneWrite();
 
-  void clearOctomap();
-
-  // Called to update the planning scene with a new message.
-  bool newPlanningSceneMessage(const moveit_msgs::msg::PlanningScene& scene);
-
-protected:
-  /** @brief Initialize the planning scene monitor
-   *  @param scene The scene instance to fill with data (an instance is allocated if the one passed in is not allocated)
-   */
-  void initialize(const planning_scene::PlanningScenePtr& scene);
-
   /** @brief Configure the collision matrix for a particular scene */
   void configureCollisionMatrix(const planning_scene::PlanningScenePtr& scene);
 
@@ -606,6 +606,9 @@ class MOVEIT_PLANNING_SCENE_MONITOR_EXPORT PlanningSceneMonitor : private boost:
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handler_;
 
   bool use_sim_time_;
+
+  friend class LockedPlanningSceneRO;
+  friend class LockedPlanningSceneRW;
 };
 
 /** \brief This is a convenience class for obtaining access to an

From 875276e858c0cb6da32550b0d07a3639650c47aa Mon Sep 17 00:00:00 2001
From: Henning Kayser <henningkayser@picknik.ai>
Date: Fri, 11 Mar 2022 16:29:10 +0000
Subject: [PATCH 077/114] Fix missing boost::ref -> std::ref

---
 .../planning_request_adapter/src/planning_request_adapter.cpp | 4 ++--
 .../src/move_group_sequence_action.cpp                        | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
index 4952050839..4260de0773 100644
--- a/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
+++ b/moveit_core/planning_request_adapter/src/planning_request_adapter.cpp
@@ -151,10 +151,10 @@ bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::Planner
     // so that in the end we have a nested sequence of function pointers that call the adapters in the correct order.
     PlanningRequestAdapter::PlannerFn fn =
         std::bind(&callAdapter1, adapters_.back().get(), planner, std::placeholders::_1, std::placeholders::_2,
-                  std::placeholders::_3, boost::ref(added_path_index_each.back()));
+                  std::placeholders::_3, std::ref(added_path_index_each.back()));
     for (int i = adapters_.size() - 2; i >= 0; --i)
       fn = std::bind(&callAdapter2, adapters_[i].get(), fn, std::placeholders::_1, std::placeholders::_2,
-                     std::placeholders::_3, boost::ref(added_path_index_each[i]));
+                     std::placeholders::_3, std::ref(added_path_index_each[i]));
     bool result = fn(planning_scene, req, res);
     added_path_index.clear();
 
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 c709dc07c9..fa8ed5d482 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
@@ -163,7 +163,7 @@ void MoveGroupSequenceAction::executeSequenceCallbackPlanAndExecute(
   opt.replan_delay_ = goal->planning_options.replan_delay;
   opt.before_execution_callback_ = std::bind(&MoveGroupSequenceAction::startMoveExecutionCallback, this);
 
-  opt.plan_callback_ = std::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, boost::cref(goal->request),
+  opt.plan_callback_ = std::bind(&MoveGroupSequenceAction::planUsingSequenceManager, this, std::cref(goal->request),
                                  std::placeholders::_1);
 
   if (goal->planning_options.look_around && context_->plan_with_sensing_)

From a7b757d275f2949532119dd899668d748e55a86f Mon Sep 17 00:00:00 2001
From: Henning Kayser <henningkayser@picknik.ai>
Date: Fri, 11 Mar 2022 22:26:56 +0100
Subject: [PATCH 078/114] Fix copyright notice in README script (#1115)

---
 moveit/scripts/create_readme_table.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/moveit/scripts/create_readme_table.py b/moveit/scripts/create_readme_table.py
index b1fc2954c9..6dd624dcb1 100644
--- a/moveit/scripts/create_readme_table.py
+++ b/moveit/scripts/create_readme_table.py
@@ -1,7 +1,5 @@
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
-# Usage: python moveit/moveit/scripts/create_readme_table.py > /tmp/table.md
-# First update supported_distro_ubuntu_dict below!
 
 # Copyright 2021 PickNik Inc.
 #
@@ -31,6 +29,9 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
+# Usage: python moveit/moveit/scripts/create_readme_table.py > /tmp/table.md
+# First update supported_distro_ubuntu_dict below!
+
 
 from __future__ import print_function
 

From d5b2d266e68eb0ef9ed7f9404043c0c861a70d6d Mon Sep 17 00:00:00 2001
From: Stephanie Eng <stephanie-eng@users.noreply.github.com>
Date: Mon, 14 Mar 2022 11:28:24 -0400
Subject: [PATCH 079/114] Off by one in getAverageSegmentDuration (#1079)

* Off by one in getAverageSegmentDuration

* Case for one waypoint

Co-authored-by: AndyZe <andyz@utexas.edu>

* Warn if too few waypoints to get duration

Co-authored-by: AndyZe <andyz@utexas.edu>

* Discount first duration_from_previous from average duration if it is 0

* Restore empty duration from previous check as per Andy's suggestion

* Changed warning message for case with 1 segment with 0 duration to be distinct from empty durations

Co-authored-by: AndyZe <andyz@utexas.edu>
Co-authored-by: Henning Kayser <henningkayser@picknik.ai>
Co-authored-by: AndyZe <zelenak@picknik.ai>
---
 .../robot_trajectory/src/robot_trajectory.cpp     | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp
index bfe2dae702..794ea9d78a 100644
--- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp
+++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp
@@ -92,7 +92,22 @@ double RobotTrajectory::getDuration() const
 double RobotTrajectory::getAverageSegmentDuration() const
 {
   if (duration_from_previous_.empty())
+  {
+    RCLCPP_WARN(rclcpp::get_logger("RobotTrajectory"), "Too few waypoints to calculate a duration. Returning 0.");
     return 0.0;
+  }
+
+  // If the initial segment has a duration of 0, exclude it from the average calculation
+  if (duration_from_previous_[0] == 0)
+  {
+    if (duration_from_previous_.size() <= 1)
+    {
+      RCLCPP_WARN(rclcpp::get_logger("RobotTrajectory"), "First and only waypoint has a duration of 0.");
+      return 0.0;
+    }
+    else
+      return getDuration() / static_cast<double>(duration_from_previous_.size() - 1);
+  }
   else
     return getDuration() / static_cast<double>(duration_from_previous_.size());
 }

From 54d2983b5c1ab94d0d72ff075effb427249d7c95 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Tue, 15 Mar 2022 09:18:25 -0500
Subject: [PATCH 080/114] Remove include of OgrePrerequisites header (#1099)

* Remove OgrePrerequisites include

* octomap_render.h includes itself

* Include OgrePrerequisites.h
---
 .../moveit/rviz_plugin_render_tools/octomap_render.h       | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h
index 11ff67c831..e7b7efbe45 100644
--- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h
+++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/octomap_render.h
@@ -37,12 +37,11 @@
 #pragma once
 
 #include <memory>
-#include <vector>
 #include <octomap/octomap.h>
-#include <rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp>
+#include <OgrePrerequisites.h>
 #include <rviz_common/properties/color_property.hpp>
-#include <moveit/rviz_plugin_render_tools/octomap_render.h>
-#include <OGRE/OgrePrerequisites.h>
+#include <rviz_default_plugins/displays/pointcloud/point_cloud_helpers.hpp>
+#include <vector>
 
 namespace octomap
 {

From 81ec447873f43268117389ccc3e911d96feff447 Mon Sep 17 00:00:00 2001
From: "David V. Lu!!" <davidvlu@gmail.com>
Date: Wed, 16 Mar 2022 15:25:13 -0400
Subject: [PATCH 081/114] New Launch Files using moveit_configs_utils (#1113)

* Add Package Path
* Launch Utils
* Some Basic Launch Files
* Remove circular dependencies
---
 .../moveit_configs_utils/launch_utils.py      |  46 ++++++
 .../moveit_configs_utils/launches.py          | 134 ++++++++++++++++++
 .../moveit_configs_builder.py                 |  13 ++
 moveit_configs_utils/package.xml              |   3 +
 4 files changed, 196 insertions(+)
 create mode 100644 moveit_configs_utils/moveit_configs_utils/launch_utils.py
 create mode 100644 moveit_configs_utils/moveit_configs_utils/launches.py

diff --git a/moveit_configs_utils/moveit_configs_utils/launch_utils.py b/moveit_configs_utils/moveit_configs_utils/launch_utils.py
new file mode 100644
index 0000000000..0d44927787
--- /dev/null
+++ b/moveit_configs_utils/moveit_configs_utils/launch_utils.py
@@ -0,0 +1,46 @@
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+
+class DeclareBooleanLaunchArg(DeclareLaunchArgument):
+    """This launch action declares a launch argument with true and false as the only valid values"""
+
+    def __init__(self, name, *, default_value=False, description=None, **kwargs):
+        super().__init__(
+            name,
+            default_value=str(default_value),
+            description=description,
+            choices=["true", "false", "True", "False"],
+            **kwargs,
+        )
+
+
+def add_debuggable_node(
+    ld, package, executable, condition_name="debug", commands_file=None, **kwargs
+):
+    """Adds two versions of a Node to the launch description, one with gdb debugging, controlled by a launch config"""
+    standard_node = Node(
+        package=package,
+        executable=executable,
+        condition=UnlessCondition(LaunchConfiguration(condition_name)),
+        **kwargs,
+    )
+    ld.add_action(standard_node)
+
+    if commands_file:
+        dash_x_arg = f"-x {commands_file} "
+    else:
+        dash_x_arg = ""
+    prefix = [f"gdb {dash_x_arg}--ex run --args"]
+
+    debug_node = Node(
+        package=package,
+        executable=executable,
+        condition=IfCondition(LaunchConfiguration(condition_name)),
+        prefix=prefix,
+        **kwargs,
+    )
+    ld.add_action(debug_node)
diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py
new file mode 100644
index 0000000000..fd7854bde8
--- /dev/null
+++ b/moveit_configs_utils/moveit_configs_utils/launches.py
@@ -0,0 +1,134 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+from moveit_configs_utils.launch_utils import (
+    add_debuggable_node,
+    DeclareBooleanLaunchArg,
+)
+
+
+def generate_rsp_launch(moveit_config):
+    """Launch file for robot state publisher (rsp)"""
+
+    ld = LaunchDescription()
+    ld.add_action(DeclareLaunchArgument("publish_frequency", default_value="15.0"))
+
+    # Given the published joint states, publish tf for the robot links and the robot description
+    rsp_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        respawn=True,
+        output="screen",
+        parameters=[
+            moveit_config.robot_description,
+            {
+                "publish_frequency": LaunchConfiguration("publish_frequency"),
+            },
+        ],
+    )
+    ld.add_action(rsp_node)
+
+    return ld
+
+
+def generate_rviz_launch(moveit_config):
+    """Launch file for rviz"""
+    ld = LaunchDescription()
+
+    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
+    ld.add_action(
+        DeclareLaunchArgument(
+            "rviz_config",
+            default_value=str(moveit_config.package_path / "launch/moveit.rviz"),
+        )
+    )
+
+    rviz_parameters = [
+        moveit_config.planning_pipelines,
+        moveit_config.robot_description_kinematics,
+    ]
+
+    add_debuggable_node(
+        ld,
+        package="rviz2",
+        executable="rviz2",
+        output="log",
+        respawn=True,
+        arguments=["-d", LaunchConfiguration("rviz_config")],
+        parameters=rviz_parameters,
+    )
+
+    return ld
+
+
+def generate_assistant_launch(moveit_config):
+    """Launch file for MoveIt Setup Assistant"""
+    ld = LaunchDescription()
+
+    ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False))
+    add_debuggable_node(
+        ld,
+        package="moveit_setup_assistant",
+        executable="moveit_setup_assistant",
+        arguments=[["--config_pkg=", str(moveit_config.package_path)]],
+    )
+
+    return ld
+
+
+def generate_warehouse_launch(moveit_config):
+    """Launch file for warehouse database"""
+    ld = LaunchDescription()
+    ld.add_action(
+        DeclareLaunchArgument(
+            "moveit_warehouse_database_path",
+            default_value=str(
+                moveit_config.package_path / "default_warehouse_mongo_db"
+            ),
+        )
+    )
+    ld.add_action(DeclareBooleanLaunchArg("reset", default_value=False))
+
+    # The default DB port for moveit (not default MongoDB port to avoid potential conflicts)
+    ld.add_action(DeclareLaunchArgument("moveit_warehouse_port", default_value="33829"))
+
+    # The default DB host for moveit
+    ld.add_action(
+        DeclareLaunchArgument("moveit_warehouse_host", default_value="localhost")
+    )
+
+    # Load warehouse parameters
+    db_parameters = [
+        {
+            "overwrite": False,
+            "database_path": LaunchConfiguration("moveit_warehouse_database_path"),
+            "warehouse_port": LaunchConfiguration("moveit_warehouse_port"),
+            "warehouse_host": LaunchConfiguration("moveit_warehouse_host"),
+            "warehouse_exec": "mongod",
+            "warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection",
+        },
+    ]
+    # Run the DB server
+    db_node = Node(
+        package="warehouse_ros_mongo",
+        executable="mongo_wrapper_ros.py",
+        # TODO(dlu): Figure out if this needs to be run in a specific directory
+        # (ROS 1 version set cwd="ROS_HOME")
+        parameters=db_parameters,
+    )
+    ld.add_action(db_node)
+
+    # If we want to reset the database, run this node
+    reset_node = Node(
+        package="moveit_ros_warehouse",
+        executable="moveit_init_demo_warehouse",
+        output="screen",
+        condition=IfCondition(LaunchConfiguration("reset")),
+    )
+    ld.add_action(reset_node)
+
+    return ld
diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
index 96a2040618..a605bb35e2 100644
--- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
+++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
@@ -21,6 +21,7 @@
 Example:
     moveit_configs = MoveItConfigsBuilder("robot_name").to_moveit_configs()
     ...
+    moveit_configs.package_path
     moveit_configs.robot_description
     moveit_configs.robot_description_semantic
     moveit_configs.robot_description_kinematics
@@ -61,6 +62,7 @@ class MoveItConfigs(object):
     """Class containing MoveIt related parameters."""
 
     __slots__ = [
+        "__package_path",
         "__robot_description",
         "__robot_description_semantic",
         "__robot_description_kinematics",
@@ -74,6 +76,8 @@ class MoveItConfigs(object):
     ]
 
     def __init__(self):
+        # A pathlib Path to the moveit config package
+        self.package_path = None
         # A dictionary that has the contents of the URDF file.
         self.robot_description = {}
         # A dictionary that has the contents of the SRDF file.
@@ -95,6 +99,14 @@ def __init__(self):
         # A dictionary containing the cartesian limits for the Pilz planner.
         self.cartesian_limits = {}
 
+    @property
+    def package_path(self):
+        return self.__package_path
+
+    @package_path.setter
+    def package_path(self, value):
+        self.__package_path = value
+
     @property
     def robot_description(self):
         return self.__robot_description
@@ -205,6 +217,7 @@ class MoveItConfigsBuilder(ParameterBuilder):
     # Look-up for robot_name_moveit_config package
     def __init__(self, robot_name: str, robot_description="robot_description"):
         super().__init__(robot_name + "_moveit_config")
+        self.__moveit_configs.package_path = self._package_path
         self.__robot_name = robot_name
         setup_assistant_file = self._package_path / ".setup_assistant"
         if not setup_assistant_file.exists():
diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml
index c26caf88a9..b2c8782bc9 100644
--- a/moveit_configs_utils/package.xml
+++ b/moveit_configs_utils/package.xml
@@ -7,9 +7,12 @@
   <maintainer email="moveit_releasers@googlegroups.com">MoveIt Release Team</maintainer>
   <license>BSD</license>
   <author email="jafar@picknik.ai">Jafar Abdi</author>
+  <author email="davidvlu@gmail.com">David V. Lu!!</author>
 
   <exec_depend>ament_index_python</exec_depend>
   <exec_depend>launch_param_builder</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

From 3b6b0d1568fbbf233995d7ff3b3511a8b959015d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= <gael.ecorchard@cvut.cz>
Date: Fri, 18 Mar 2022 15:52:29 +0100
Subject: [PATCH 082/114] [moveit_cpp] Fix double param declaration (#1097)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>
---
 .../joint_limits/joint_limits_rosparam.hpp    | 50 ++++++++++++-------
 1 file changed, 32 insertions(+), 18 deletions(-)

diff --git a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp
index 672ba60cba..b860fa49cc 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp
+++ b/moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp
@@ -27,32 +27,46 @@
 #include "joint_limits/joint_limits.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+namespace
+{
+/** declare a parameter if not already declared. */
+template <typename T>
+void declare_parameter(const rclcpp::Node::SharedPtr& node, const std::string& name, T default_value)
+{
+  if (not node->has_parameter(name))
+  {
+    node->declare_parameter<T>(name, default_value);
+  }
+}
+}  // namespace
+
 namespace joint_limits
 {
 inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node::SharedPtr& node,
                                const std::string& param_ns)
 {
   const std::string param_base_name = (param_ns.empty() ? "" : param_ns + ".") + "joint_limits." + joint_name;
+
   try
   {
-    node->declare_parameter<bool>(param_base_name + ".has_position_limits", false);
-    node->declare_parameter<double>(param_base_name + ".min_position", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<double>(param_base_name + ".max_position", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<bool>(param_base_name + ".has_velocity_limits", false);
-    node->declare_parameter<double>(param_base_name + ".min_velocity", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<double>(param_base_name + ".max_velocity", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<bool>(param_base_name + ".has_acceleration_limits", false);
-    node->declare_parameter<double>(param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<bool>(param_base_name + ".has_jerk_limits", false);
-    node->declare_parameter<double>(param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<bool>(param_base_name + ".has_effort_limits", false);
-    node->declare_parameter<double>(param_base_name + ".max_effort", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<bool>(param_base_name + ".angle_wraparound", false);
-    node->declare_parameter<bool>(param_base_name + ".has_soft_limits", false);
-    node->declare_parameter<double>(param_base_name + ".k_position", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<double>(param_base_name + ".k_velocity", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<double>(param_base_name + ".soft_lower_limit", std::numeric_limits<double>::quiet_NaN());
-    node->declare_parameter<double>(param_base_name + ".soft_upper_limit", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".has_position_limits", false);
+    declare_parameter(node, param_base_name + ".min_position", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".max_position", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".has_velocity_limits", false);
+    declare_parameter(node, param_base_name + ".min_velocity", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".max_velocity", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".has_acceleration_limits", false);
+    declare_parameter(node, param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".has_jerk_limits", false);
+    declare_parameter(node, param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".has_effort_limits", false);
+    declare_parameter(node, param_base_name + ".max_effort", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".angle_wraparound", false);
+    declare_parameter(node, param_base_name + ".has_soft_limits", false);
+    declare_parameter(node, param_base_name + ".k_position", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".k_velocity", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".soft_lower_limit", std::numeric_limits<double>::quiet_NaN());
+    declare_parameter(node, param_base_name + ".soft_upper_limit", std::numeric_limits<double>::quiet_NaN());
   }
   catch (const std::exception& ex)
   {

From d59a46372c5a3c088a61732f5c8f152f67778c34 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= <gael.ecorchard@cvut.cz>
Date: Fri, 18 Mar 2022 15:55:11 +0100
Subject: [PATCH 083/114] [moveit_cpp] Fix config of multiple pipelines (#1096)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>
---
 moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp
index 7d9254dc22..e6ba420d70 100644
--- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp
+++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp
@@ -165,7 +165,6 @@ bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& options)
   {
     for (const auto& group_name : group_names)
     {
-      groups_pipelines_map_[group_name] = {};
       const auto& pipeline = pipeline_entry.second;
       for (const auto& planner_configuration : pipeline->getPlannerManager()->getPlannerConfigurations())
       {

From 7db7d6ce59c254e5c6a2681d5eb991882d8c1b63 Mon Sep 17 00:00:00 2001
From: "David V. Lu!!" <davidvlu@gmail.com>
Date: Mon, 21 Mar 2022 17:23:10 -0400
Subject: [PATCH 084/114] Remove launch_param_builder (#1133)

---
 moveit2.repos | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/moveit2.repos b/moveit2.repos
index c74e6089ed..ecd6a89e75 100644
--- a/moveit2.repos
+++ b/moveit2.repos
@@ -11,10 +11,6 @@ repositories:
     type: git
     url: https://github.com/ros-planning/moveit_resources
     version: ros2
-  launch_param_builder:
-    type: git
-    url: https://github.com/PickNikRobotics/launch_param_builder
-    version: main
   ros2_control:
     type: git
     url: https://github.com/ros-controls/ros2_control.git

From 1b080062d79a28427c82d9720f85795ee5b007bd Mon Sep 17 00:00:00 2001
From: Henning Kayser <henningkayser@picknik.ai>
Date: Tue, 22 Mar 2022 16:27:05 +0100
Subject: [PATCH 085/114] Simply MoveItCpp::getPlanningPipelineNames() (#1114)

---
 .../planning/moveit_cpp/src/moveit_cpp.cpp    | 24 ++++---------------
 1 file changed, 4 insertions(+), 20 deletions(-)

diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp
index e6ba420d70..609eddcaee 100644
--- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp
+++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp
@@ -217,30 +217,14 @@ const std::map<std::string, planning_pipeline::PlanningPipelinePtr>& MoveItCpp::
 
 std::set<std::string> MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const
 {
-  std::set<std::string> result_names;
-  if (!group_name.empty() && groups_pipelines_map_.count(group_name) == 0)
+  if (group_name.empty() || groups_pipelines_map_.count(group_name) == 0)
   {
     RCLCPP_ERROR(LOGGER, "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.",
                  group_name.c_str());
-    return result_names;  // empty
+    return {};  // empty
   }
-  for (const auto& pipeline_entry : planning_pipelines_)
-  {
-    const std::string& pipeline_name = pipeline_entry.first;
-    // If group_name is defined and valid, skip pipelines that don't belong to the planning group
-    if (!group_name.empty())
-    {
-      const auto& group_pipelines = groups_pipelines_map_.at(group_name);
-      if (group_pipelines.find(pipeline_name) == group_pipelines.end())
-        continue;
-    }
-    result_names.insert(pipeline_name);
-  }
-  // No valid planning pipelines
-  if (result_names.empty())
-    RCLCPP_ERROR(LOGGER, "No planning pipelines loaded for group '%s'. Check planning pipeline and controller setup.",
-                 group_name.c_str());
-  return result_names;
+
+  return groups_pipelines_map_.at(group_name);
 }
 
 const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const

From 6fa02b74fc985bb63c65a03e47f1d84b16d7ebe3 Mon Sep 17 00:00:00 2001
From: "David V. Lu!!" <davidvlu@gmail.com>
Date: Tue, 22 Mar 2022 13:45:00 -0400
Subject: [PATCH 086/114] RDFLoader Broken with Xacro Files (#1132)

* A broken RDFLoader test

* Bugfix: Add space between executable and path (if no arguments)
---
 moveit_ros/planning/rdf_loader/src/rdf_loader.cpp        | 2 +-
 .../planning/rdf_loader/test/data/robin.srdf.xacro       | 9 +++++++++
 .../planning/rdf_loader/test/test_rdf_integration.cpp    | 9 +++++++++
 3 files changed, 19 insertions(+), 1 deletion(-)
 create mode 100644 moveit_ros/planning/rdf_loader/test/data/robin.srdf.xacro

diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp
index 67b3116e1e..93e0529909 100644
--- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp
+++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp
@@ -164,7 +164,7 @@ bool RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& pa
     return false;
   }
 
-  std::string cmd = "ros2 run xacro xacro";
+  std::string cmd = "ros2 run xacro xacro ";
   for (const std::string& xacro_arg : xacro_args)
     cmd += xacro_arg + " ";
   cmd += path;
diff --git a/moveit_ros/planning/rdf_loader/test/data/robin.srdf.xacro b/moveit_ros/planning/rdf_loader/test/data/robin.srdf.xacro
new file mode 100644
index 0000000000..6ebcdbc339
--- /dev/null
+++ b/moveit_ros/planning/rdf_loader/test/data/robin.srdf.xacro
@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="robin" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="srdf_macro">
+    <group name="group0">
+      <chain base_link="base_link" tip_link="flipper"/>
+    </group>
+  </xacro:macro>
+  <xacro:srdf_macro />
+</robot>
diff --git a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp
index b1f9ba4bc5..8e665c1cdb 100644
--- a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp
+++ b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp
@@ -90,6 +90,15 @@ TEST(RDFIntegration, executor)
   EXPECT_EQ("gonzo", loader.getSRDF()->getName());
 }
 
+TEST(RDFIntegration, xacro_test)
+{
+  std::string output;
+  std::vector<std::string> xacro_args;
+  ASSERT_TRUE(rdf_loader::RDFLoader::loadPkgFileToString(output, "moveit_ros_planning",
+                                                         "rdf_loader/test/data/robin.srdf.xacro", xacro_args));
+  EXPECT_GT(output.size(), 0u);
+}
+
 int main(int argc, char** argv)
 {
   rclcpp::init(argc, argv);

From a772d8eaf9bcab8ed5c92cc19a162293eb7ddc9d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sencer=20Yaz=C4=B1c=C4=B1?= <senceryazici@gmail.com>
Date: Thu, 24 Mar 2022 13:11:47 +0300
Subject: [PATCH 087/114] Remove new operators (#1135)

replace new operator with make_shared
---
 .../collision_detector_allocator.h            |  8 +-
 .../test_collision_common_panda.h             |  2 +-
 .../test_collision_common_pr2.h               |  6 +-
 .../collision_detection/src/collision_env.cpp |  2 +-
 .../collision_detection/test/test_world.cpp   | 18 ++---
 .../test/test_world_diff.cpp                  | 18 ++---
 .../bullet_cast_bvh_manager.cpp               |  2 +-
 .../bullet_discrete_bvh_manager.cpp           |  2 +-
 .../src/collision_env_bullet.cpp              | 12 +--
 ...t_bullet_continuous_collision_checking.cpp | 22 ++---
 .../collision_common.h                        |  9 ++-
 .../src/collision_common.cpp                  |  2 +-
 .../src/collision_env_fcl.cpp                 |  8 +-
 .../src/collision_common_distance_field.cpp   | 12 +--
 .../src/collision_env_distance_field.cpp      | 15 ++--
 .../src/collision_env_hybrid.cpp              |  7 +-
 .../test/test_collision_distance_field.cpp    |  2 +-
 .../src/constraint_sampler_manager.cpp        | 16 ++--
 .../src/default_constraint_samplers.cpp       |  8 +-
 .../test/test_constraint_samplers.cpp         |  9 ++-
 .../test/test_distance_field.cpp              |  4 +-
 .../src/kinematic_constraint.cpp              |  2 +-
 .../planning_scene/src/planning_scene.cpp     | 12 +--
 .../test/test_planning_scene.cpp              | 14 ++--
 .../robot_state/test/robot_state_test.cpp     |  2 +-
 .../test/test_cartesian_interpolator.cpp      |  4 +-
 .../test/test_kinematic_complex.cpp           | 12 +--
 .../robot_trajectory/src/robot_trajectory.cpp |  4 +-
 .../utils/src/robot_model_test_utils.cpp      | 28 +++----
 .../src/detail/constraints_library.cpp        | 11 ++-
 .../src/model_based_planning_context.cpp      | 13 ++-
 .../ompl_interface/src/ompl_interface.cpp     |  4 +-
 .../src/planning_context_manager.cpp          |  6 +-
 .../src/path_circle_generator.cpp             |  6 +-
 .../src/plan_components_builder.cpp           |  4 +-
 .../src/trajectory_generator.cpp              |  6 +-
 .../src/trajectory_generator_lin.cpp          |  3 +-
 ..._pilz_industrial_motion_planner_direct.cpp |  3 +-
 .../src/unittest_planning_context.cpp         |  2 +-
 .../unittest_trajectory_generator_common.cpp  |  3 +-
 .../src/environment_chain3d.cpp               | 10 +--
 .../sbpl_interface/src/sbpl_interface.cpp     |  4 +-
 .../prbt_manipulator_ikfast_moveit_plugin.cpp |  3 +-
 .../trajopt_interface/problem_description.h   |  9 +--
 .../trajopt/src/problem_description.cpp       | 80 +++++++++----------
 .../trajopt/src/trajopt_interface.cpp         | 12 +--
 .../trajopt/src/trajopt_planner_manager.cpp   |  3 +-
 .../trajopt/src/trajopt_planning_context.cpp  |  5 +-
 .../trajopt/test/trajectory_test.cpp          |  8 +-
 .../src/moveit_controller_manager_example.cpp |  2 +-
 .../src/approach_and_translate_stage.cpp      | 14 ++--
 .../manipulation/pick_place/src/pick.cpp      |  8 +-
 .../manipulation/pick_place/src/place.cpp     |  8 +-
 .../pick_place/src/plan_stage.cpp             |  4 +-
 .../src/reachable_valid_pose_filter.cpp       |  2 +-
 .../mesh_filter/src/mesh_filter_base.cpp      | 22 ++---
 .../src/constraint_sampler_manager_loader.cpp |  2 +-
 ...re_collision_speed_checking_fcl_bullet.cpp |  2 +-
 .../src/display_random_state.cpp              |  2 +-
 .../src/publish_scene_from_text.cpp           |  3 +-
 .../src/visualize_robot_collision_volume.cpp  |  2 +-
 .../src/fix_start_state_collision.cpp         |  2 +-
 .../test/test_execution_manager.cpp           |  2 +-
 .../test/test_moveit_controller_manager.h     |  2 +-
 .../src/locked_robot_state.cpp                |  4 +-
 .../src/robot_interaction.cpp                 |  2 +-
 .../test/locked_robot_state_test.cpp          |  4 +-
 .../src/motion_planning_display.cpp           |  4 +-
 .../src/motion_planning_frame_objects.cpp     |  2 +-
 .../src/motion_planning_frame_planning.cpp    |  4 +-
 .../src/trajectory_visualization.cpp          |  2 +-
 71 files changed, 281 insertions(+), 275 deletions(-)

diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h
index 05df2799a0..d63bb5aaff 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h
@@ -79,23 +79,23 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator
 
   CollisionEnvPtr allocateEnv(const WorldPtr& world, const moveit::core::RobotModelConstPtr& robot_model) const override
   {
-    return CollisionEnvPtr(new CollisionEnvType(robot_model, world));
+    return std::make_shared<CollisionEnvType>(robot_model, world);
   }
 
   CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const override
   {
-    return CollisionEnvPtr(new CollisionEnvType(dynamic_cast<const CollisionEnvType&>(*orig), world));
+    return std::make_shared<CollisionEnvType>(dynamic_cast<const CollisionEnvType&>(*orig), world);
   }
 
   CollisionEnvPtr allocateEnv(const moveit::core::RobotModelConstPtr& robot_model) const override
   {
-    return CollisionEnvPtr(new CollisionEnvType(robot_model));
+    return std::make_shared<CollisionEnvType>(robot_model);
   }
 
   /** Create an allocator for collision detectors. */
   static CollisionDetectorAllocatorPtr create()
   {
-    return CollisionDetectorAllocatorPtr(new CollisionDetectorAllocatorType());
+    return std::make_shared<CollisionDetectorAllocatorType>();
   }
 };
 }  // namespace collision_detection
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
index ca840ac763..016d5162da 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h
@@ -280,7 +280,7 @@ TYPED_TEST_P(DistanceCheckPandaTest, DistanceSingle)
   {
     collision_detection::DistanceResult res;
 
-    shapes::ShapeConstPtr shape(new shapes::Cylinder(rng.uniform01(), rng.uniform01()));
+    shapes::ShapeConstPtr shape = std::make_shared<const shapes::Cylinder>(rng.uniform01(), rng.uniform01());
     Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
     pose.translation() =
         Eigen::Vector3d(rng.uniformReal(0.1, 2.0), rng.uniformReal(0.1, 2.0), rng.uniformReal(1.2, 1.7));
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h
index 7643dfab07..e59dbf2aa1 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h
@@ -472,7 +472,7 @@ TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed)
   for (unsigned int i = 0; i < 10000; ++i)
   {
     poses.push_back(Eigen::Isometry3d::Identity());
-    shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01)));
+    shapes.push_back(std::make_shared<shapes::Box>(.01, .01, .01));
   }
   auto start = std::chrono::system_clock::now();
   this->cenv_->getWorld()->addToObject("map", shapes, poses);
@@ -526,7 +526,7 @@ TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize)
     this->cenv_->getWorld()->removeObject("shape");
     shapes.clear();
     poses.clear();
-    shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)));
+    shapes.push_back(std::make_shared<const shapes::Box>(1 + i * .0001, 1 + i * .0001, 1 + i * .0001));
     poses.push_back(Eigen::Isometry3d::Identity());
     this->cenv_->getWorld()->addToObject("shape", shapes, poses);
     collision_detection::CollisionRequest req;
@@ -548,7 +548,7 @@ TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize)
     this->cenv_->getWorld()->removeObject("shape");
     shapes.clear();
     poses.clear();
-    shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)));
+    shapes.push_back(std::make_shared<shapes::Box>(1 + i * .0001, 1 + i * .0001, 1 + i * .0001));
     poses.push_back(Eigen::Isometry3d::Identity());
     this->cenv_->getWorld()->addToObject("shape", shapes, poses);
     collision_detection::CollisionRequest req;
diff --git a/moveit_core/collision_detection/src/collision_env.cpp b/moveit_core/collision_detection/src/collision_env.cpp
index bba3cc2e4d..7e547cb3ab 100644
--- a/moveit_core/collision_detection/src/collision_env.cpp
+++ b/moveit_core/collision_detection/src/collision_env.cpp
@@ -74,7 +74,7 @@ static inline bool validatePadding(double padding)
 namespace collision_detection
 {
 CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, double padding, double scale)
-  : robot_model_(model), world_(new World()), world_const_(world_)
+  : robot_model_(model), world_(std::make_shared<World>()), world_const_(world_)
 {
   if (!validateScale(scale))
     scale = 1.0;
diff --git a/moveit_core/collision_detection/test/test_world.cpp b/moveit_core/collision_detection/test/test_world.cpp
index 498f3bc668..1c4b553972 100644
--- a/moveit_core/collision_detection/test/test_world.cpp
+++ b/moveit_core/collision_detection/test/test_world.cpp
@@ -44,9 +44,9 @@ TEST(World, AddRemoveShape)
   collision_detection::World world;
 
   // Create some shapes
-  shapes::ShapePtr ball(new shapes::Sphere(1.0));
-  shapes::ShapePtr box(new shapes::Box(1, 2, 3));
-  shapes::ShapePtr cyl(new shapes::Cylinder(4, 5));
+  shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);
+  shapes::ShapePtr box = std::make_shared<shapes::Box>(1, 2, 3);
+  shapes::ShapePtr cyl = std::make_shared<shapes::Cylinder>(4, 5);
 
   EXPECT_EQ(1, ball.use_count());
 
@@ -239,9 +239,9 @@ TEST(World, TrackChanges)
   observer_ta = world.addObserver(std::bind(TrackChangesNotify, &ta, std::placeholders::_1, std::placeholders::_2));
 
   // Create some shapes
-  shapes::ShapePtr ball(new shapes::Sphere(1.0));
-  shapes::ShapePtr box(new shapes::Box(1, 2, 3));
-  shapes::ShapePtr cyl(new shapes::Cylinder(4, 5));
+  shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);
+  shapes::ShapePtr box = std::make_shared<shapes::Box>(1, 2, 3);
+  shapes::ShapePtr cyl = std::make_shared<shapes::Cylinder>(4, 5);
 
   world.addToObject("obj1", ball, Eigen::Isometry3d::Identity());
 
@@ -374,9 +374,9 @@ TEST(World, ObjectPoseAndSubframes)
   observer_ta = world.addObserver(std::bind(TrackChangesNotify, &ta, std::placeholders::_1, std::placeholders::_2));
 
   // Create shapes
-  shapes::ShapePtr ball(new shapes::Sphere(1.0));
-  shapes::ShapePtr box(new shapes::Box(1, 1, 1));
-  shapes::ShapePtr cyl(new shapes::Cylinder(0.5, 3));  // radius, length
+  shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);
+  shapes::ShapePtr box = std::make_shared<shapes::Box>(1, 1, 1);
+  shapes::ShapePtr cyl = std::make_shared<shapes::Cylinder>(0.5, 3);  // radius, length
 
   // Confirm that setting object pose creates an object
   world.setObjectPose("mix1", Eigen::Isometry3d::Identity());
diff --git a/moveit_core/collision_detection/test/test_world_diff.cpp b/moveit_core/collision_detection/test/test_world_diff.cpp
index f5dd079b8d..e073cb17c1 100644
--- a/moveit_core/collision_detection/test/test_world_diff.cpp
+++ b/moveit_core/collision_detection/test/test_world_diff.cpp
@@ -40,7 +40,7 @@
 
 TEST(WorldDiff, TrackChanges)
 {
-  collision_detection::WorldPtr world(new collision_detection::World);
+  collision_detection::WorldPtr world = std::make_shared<collision_detection::World>();
   collision_detection::WorldDiff diff1(world);
   collision_detection::WorldDiff diff2;
   collision_detection::WorldDiff::const_iterator it;
@@ -49,9 +49,9 @@ TEST(WorldDiff, TrackChanges)
   EXPECT_EQ(diff2.getChanges().size(), 0u);
 
   // Create some shapes
-  shapes::ShapePtr ball(new shapes::Sphere(1.0));
-  shapes::ShapePtr box(new shapes::Box(1, 2, 3));
-  shapes::ShapePtr cyl(new shapes::Cylinder(4, 5));
+  shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);
+  shapes::ShapePtr box = std::make_shared<shapes::Box>(1, 2, 3);
+  shapes::ShapePtr cyl = std::make_shared<shapes::Cylinder>(4, 5);
 
   world->addToObject("obj1", ball, Eigen::Isometry3d::Identity());
 
@@ -190,16 +190,16 @@ TEST(WorldDiff, TrackChanges)
 
 TEST(WorldDiff, SetWorld)
 {
-  collision_detection::WorldPtr world1(new collision_detection::World);
-  collision_detection::WorldPtr world2(new collision_detection::World);
+  collision_detection::WorldPtr world1 = std::make_shared<collision_detection::World>();
+  collision_detection::WorldPtr world2 = std::make_shared<collision_detection::World>();
   collision_detection::WorldDiff diff1(world1);
   collision_detection::WorldDiff diff1b(world1);
   collision_detection::WorldDiff diff2(world2);
   collision_detection::WorldDiff::const_iterator it;
 
-  shapes::ShapePtr ball(new shapes::Sphere(1.0));
-  shapes::ShapePtr box(new shapes::Box(1, 2, 3));
-  shapes::ShapePtr cyl(new shapes::Cylinder(4, 5));
+  shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);
+  shapes::ShapePtr box = std::make_shared<shapes::Box>(1, 2, 3);
+  shapes::ShapePtr cyl = std::make_shared<shapes::Cylinder>(4, 5);
 
   world1->addToObject("objA1", ball, Eigen::Isometry3d::Identity());
 
diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp
index d54a29b2aa..712c03f7b8 100644
--- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp
+++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_cast_bvh_manager.cpp
@@ -41,7 +41,7 @@ namespace collision_detection_bullet
 {
 BulletCastBVHManagerPtr BulletCastBVHManager::clone() const
 {
-  BulletCastBVHManagerPtr manager(new BulletCastBVHManager());
+  BulletCastBVHManagerPtr manager = std::make_shared<BulletCastBVHManager>();
 
   for (const std::pair<const std::string, collision_detection_bullet::CollisionObjectWrapperPtr>& cow : link2cow_)
   {
diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp
index a34890f030..8ea5ce55dd 100644
--- a/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp
+++ b/moveit_core/collision_detection_bullet/src/bullet_integration/bullet_discrete_bvh_manager.cpp
@@ -39,7 +39,7 @@ namespace collision_detection_bullet
 {
 BulletDiscreteBVHManagerPtr BulletDiscreteBVHManager::clone() const
 {
-  BulletDiscreteBVHManagerPtr manager(new BulletDiscreteBVHManager());
+  auto manager = std::make_shared<BulletDiscreteBVHManager>();
 
   for (const std::pair<const std::string, CollisionObjectWrapperPtr>& cow : link2cow_)
   {
diff --git a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp
index 9681c1d735..eab7da4187 100644
--- a/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp
+++ b/moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp
@@ -340,9 +340,10 @@ void CollisionEnvBullet::addAttachedOjects(const moveit::core::RobotState& state
 
     try
     {
-      collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper(
-          body->getName(), collision_detection::BodyType::ROBOT_ATTACHED, body->getShapes(), attached_body_transform,
-          collision_object_types, body->getTouchLinks()));
+      collision_detection_bullet::CollisionObjectWrapperPtr cow =
+          std::make_shared<collision_detection_bullet::CollisionObjectWrapper>(
+              body->getName(), collision_detection::BodyType::ROBOT_ATTACHED, body->getShapes(),
+              attached_body_transform, collision_object_types, body->getTouchLinks());
       cows.push_back(cow);
     }
     catch (std::exception&)
@@ -427,8 +428,9 @@ void CollisionEnvBullet::addLinkAsCollisionObject(const urdf::LinkSharedPtr& lin
 
     try
     {
-      collision_detection_bullet::CollisionObjectWrapperPtr cow(new collision_detection_bullet::CollisionObjectWrapper(
-          link->name, collision_detection::BodyType::ROBOT_LINK, shapes, shape_poses, collision_object_types, true));
+      collision_detection_bullet::CollisionObjectWrapperPtr cow =
+          std::make_shared<collision_detection_bullet::CollisionObjectWrapper>(
+              link->name, collision_detection::BodyType::ROBOT_LINK, shapes, shape_poses, collision_object_types, true);
       manager_->addCollisionObject(cow);
       manager_CCD_->addCollisionObject(cow->clone());
       active_.push_back(cow->getName());
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 0c2059c306..ca71889128 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
@@ -116,7 +116,7 @@ void addCollisionObjects(cb::BulletCastBVHManager& checker)
   ////////////////////////////
   // Add static box to checker
   ////////////////////////////
-  shapes::ShapePtr static_box(new shapes::Box(1, 1, 1));
+  shapes::ShapePtr static_box = std::make_shared<shapes::Box>(1, 1, 1);
   Eigen::Isometry3d static_box_pose;
   static_box_pose.setIdentity();
 
@@ -127,14 +127,14 @@ void addCollisionObjects(cb::BulletCastBVHManager& checker)
   obj1_poses.push_back(static_box_pose);
   obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE);
 
-  cb::CollisionObjectWrapperPtr cow_1(new cb::CollisionObjectWrapper(
-      "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types));
+  cb::CollisionObjectWrapperPtr cow_1 = std::make_shared<cb::CollisionObjectWrapper>(
+      "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types);
   checker.addCollisionObject(cow_1);
 
   ////////////////////////////
   // Add moving box to checker
   ////////////////////////////
-  shapes::ShapePtr moving_box(new shapes::Box(0.2, 0.2, 0.2));
+  shapes::ShapePtr moving_box = std::make_shared<shapes::Box>(0.2, 0.2, 0.2);
   Eigen::Isometry3d moving_box_pose;
 
   moving_box_pose.setIdentity();
@@ -147,8 +147,8 @@ void addCollisionObjects(cb::BulletCastBVHManager& checker)
   obj2_poses.push_back(moving_box_pose);
   obj2_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE);
 
-  cb::CollisionObjectWrapperPtr cow_2(new cb::CollisionObjectWrapper(
-      "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types));
+  cb::CollisionObjectWrapperPtr cow_2 = std::make_shared<cb::CollisionObjectWrapper>(
+      "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types);
   checker.addCollisionObject(cow_2);
 }
 
@@ -157,7 +157,7 @@ void addCollisionObjectsMesh(cb::BulletCastBVHManager& checker)
   ////////////////////////////
   // Add static box to checker
   ////////////////////////////
-  shapes::ShapePtr static_box(new shapes::Box(0.3, 0.3, 0.3));
+  shapes::ShapePtr static_box = std::make_shared<shapes::Box>(0.3, 0.3, 0.3);
   Eigen::Isometry3d static_box_pose;
   static_box_pose.setIdentity();
 
@@ -168,8 +168,8 @@ void addCollisionObjectsMesh(cb::BulletCastBVHManager& checker)
   obj1_poses.push_back(static_box_pose);
   obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE);
 
-  cb::CollisionObjectWrapperPtr cow_1(new cb::CollisionObjectWrapper(
-      "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types));
+  cb::CollisionObjectWrapperPtr cow_1 = std::make_shared<cb::CollisionObjectWrapper>(
+      "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types);
   checker.addCollisionObject(cow_1);
   ////////////////////////////
   // Add moving mesh to checker
@@ -191,8 +191,8 @@ void addCollisionObjectsMesh(cb::BulletCastBVHManager& checker)
   obj2_types.push_back(cb::CollisionObjectType::CONVEX_HULL);
   obj2_poses.push_back(s_pose);
 
-  cb::CollisionObjectWrapperPtr cow_2(new cb::CollisionObjectWrapper(
-      "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types));
+  cb::CollisionObjectWrapperPtr cow_2 = std::make_shared<cb::CollisionObjectWrapper>(
+      "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types);
   checker.addCollisionObject(cow_2);
 }
 
diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h
index ee5a00beab..e5c5beac41 100644
--- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h
+++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h
@@ -206,21 +206,24 @@ struct FCLGeometry
 
   /** \brief Constructor for a robot link. */
   FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::LinkModel* link, int shape_index)
-    : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(link, shape_index))
+    : collision_geometry_(collision_geometry)
+    , collision_geometry_data_(std::make_shared<CollisionGeometryData>(link, shape_index))
   {
     collision_geometry_->setUserData(collision_geometry_data_.get());
   }
 
   /** \brief Constructor for an attached body. */
   FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::AttachedBody* ab, int shape_index)
-    : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(ab, shape_index))
+    : collision_geometry_(collision_geometry)
+    , collision_geometry_data_(std::make_shared<CollisionGeometryData>(ab, shape_index))
   {
     collision_geometry_->setUserData(collision_geometry_data_.get());
   }
 
   /** \brief Constructor for a world object. */
   FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const World::Object* obj, int shape_index)
-    : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(obj, shape_index))
+    : collision_geometry_(collision_geometry)
+    , collision_geometry_data_(std::make_shared<CollisionGeometryData>(obj, shape_index))
   {
     collision_geometry_->setUserData(collision_geometry_data_.get());
   }
diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp
index da4c5d4549..d2bbe354ff 100644
--- a/moveit_core/collision_detection_fcl/src/collision_common.cpp
+++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp
@@ -857,7 +857,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape,
   if (cg_g)
   {
     cg_g->computeLocalAABB();
-    FCLGeometryConstPtr res(new FCLGeometry(cg_g, data, shape_index));
+    FCLGeometryConstPtr res = std::make_shared<const FCLGeometry>(cg_g, data, shape_index);
     cache.map_[wptr] = res;
     cache.bumpUseCount();
     return res;
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 62ba918c80..71f229a805 100644
--- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
+++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
@@ -97,8 +97,8 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model,
         // Every time this object is created, g->computeLocalAABB() is called  which is
         // very expensive and should only be calculated once. To update the AABB, use the
         // collObj->setTransform and then call collObj->computeAABB() to transform the AABB.
-        robot_fcl_objs_[index] =
-            FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(link_geometry->collision_geometry_));
+        robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(
+            std::make_shared<const fcl::CollisionObjectd>(link_geometry->collision_geometry_));
       }
       else
         RCLCPP_ERROR(LOGGER, "Unable to construct collision geometry for link '%s'", link->getName().c_str());
@@ -134,7 +134,7 @@ CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model,
         // Every time this object is created, g->computeLocalAABB() is called  which is
         // very expensive and should only be calculated once. To update the AABB, use the
         // collObj->setTransform and then call collObj->computeAABB() to transform the AABB.
-        robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_));
+        robot_fcl_objs_[index] = std::make_shared<const fcl::CollisionObjectd>(g->collision_geometry_);
       }
       else
         RCLCPP_ERROR(LOGGER, "Unable to construct collision geometry for link '%s'", link->getName().c_str());
@@ -452,7 +452,7 @@ void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector<std::string>& li
         {
           index = lmodel->getFirstCollisionBodyTransformIndex() + j;
           robot_geoms_[index] = g;
-          robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_));
+          robot_fcl_objs_[index] = std::make_shared<const fcl::CollisionObjectd>(g->collision_geometry_);
         }
       }
     }
diff --git a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp
index 9fcd49e826..655b5ae7fb 100644
--- a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp
+++ b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp
@@ -82,7 +82,7 @@ BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeCons
     }
   }
 
-  BodyDecompositionConstPtr bdcp(new BodyDecomposition(shape, resolution));
+  BodyDecompositionConstPtr bdcp = std::make_shared<const BodyDecomposition>(shape, resolution);
   {
     boost::mutex::scoped_lock slock(cache.lock_);
     cache.map_[wptr] = bdcp;
@@ -95,7 +95,7 @@ BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeCons
 PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const collision_detection::World::Object& obj,
                                                                           double resolution)
 {
-  PosedBodyPointDecompositionVectorPtr ret(new PosedBodyPointDecompositionVector());
+  PosedBodyPointDecompositionVectorPtr ret = std::make_shared<PosedBodyPointDecompositionVector>();
   for (unsigned int i = 0; i < obj.shapes_.size(); ++i)
   {
     PosedBodyPointDecompositionPtr pbd(
@@ -109,7 +109,7 @@ PosedBodyPointDecompositionVectorPtr getCollisionObjectPointDecomposition(const
 PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const moveit::core::AttachedBody* att,
                                                                          double resolution)
 {
-  PosedBodySphereDecompositionVectorPtr ret(new PosedBodySphereDecompositionVector());
+  PosedBodySphereDecompositionVectorPtr ret = std::make_shared<PosedBodySphereDecompositionVector>();
   for (unsigned int i = 0; i < att->getShapes().size(); ++i)
   {
     PosedBodySphereDecompositionPtr pbd(
@@ -123,11 +123,11 @@ PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const m
 PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const moveit::core::AttachedBody* att,
                                                                        double resolution)
 {
-  PosedBodyPointDecompositionVectorPtr ret(new PosedBodyPointDecompositionVector());
+  PosedBodyPointDecompositionVectorPtr ret = std::make_shared<PosedBodyPointDecompositionVector>();
   for (unsigned int i = 0; i < att->getShapes().size(); ++i)
   {
-    PosedBodyPointDecompositionPtr pbd(
-        new PosedBodyPointDecomposition(getBodyDecompositionCacheEntry(att->getShapes()[i], resolution)));
+    PosedBodyPointDecompositionPtr pbd =
+        std::make_shared<PosedBodyPointDecomposition>(getBodyDecompositionCacheEntry(att->getShapes()[i], resolution));
     ret->addToVector(pbd);
     ret->updatePose(ret->getSize() - 1, att->getGlobalCollisionBodyTransforms()[i]);
   }
diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
index 5970c5eb03..a5b2bd6af8 100644
--- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
+++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
@@ -710,7 +710,7 @@ DistanceFieldCacheEntryPtr CollisionEnvDistanceField::generateDistanceFieldCache
     const std::string& group_name, const moveit::core::RobotState& state,
     const collision_detection::AllowedCollisionMatrix* acm, bool generate_distance_field) const
 {
-  DistanceFieldCacheEntryPtr dfce(new DistanceFieldCacheEntry());
+  DistanceFieldCacheEntryPtr dfce = std::make_shared<DistanceFieldCacheEntry>();
 
   if (robot_model_->getJointModelGroup(group_name) == nullptr)
   {
@@ -969,9 +969,9 @@ void CollisionEnvDistanceField::addLinkBodyDecompositions(double resolution)
     }
 
     RCLCPP_DEBUG(LOGGER, "Generating model for %s", link_model->getName().c_str());
-    BodyDecompositionConstPtr bd(new BodyDecomposition(link_model->getShapes(),
-                                                       link_model->getCollisionOriginTransforms(), resolution,
-                                                       getLinkPadding(link_model->getName())));
+    BodyDecompositionConstPtr bd =
+        std::make_shared<const BodyDecomposition>(link_model->getShapes(), link_model->getCollisionOriginTransforms(),
+                                                  resolution, getLinkPadding(link_model->getName()));
     link_body_decomposition_vector_.push_back(bd);
     link_body_decomposition_index_map_[link_model->getName()] = link_body_decomposition_vector_.size() - 1;
   }
@@ -1060,8 +1060,9 @@ void CollisionEnvDistanceField::addLinkBodyDecompositions(
       continue;
     }
 
-    BodyDecompositionPtr bd(new BodyDecomposition(link_model->getShapes(), link_model->getCollisionOriginTransforms(),
-                                                  resolution, getLinkPadding(link_model->getName())));
+    BodyDecompositionPtr bd =
+        std::make_shared<BodyDecomposition>(link_model->getShapes(), link_model->getCollisionOriginTransforms(),
+                                            resolution, getLinkPadding(link_model->getName()));
 
     RCLCPP_DEBUG(LOGGER, "Generated model for %s", link_model->getName().c_str());
 
@@ -1780,7 +1781,7 @@ void CollisionEnvDistanceField::updateDistanceObject(const std::string& id, Dist
 CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr
 CollisionEnvDistanceField::generateDistanceFieldCacheEntryWorld()
 {
-  DistanceFieldCacheEntryWorldPtr dfce(new DistanceFieldCacheEntryWorld());
+  DistanceFieldCacheEntryWorldPtr dfce = std::make_shared<DistanceFieldCacheEntryWorld>();
   dfce->distance_field_ = std::make_shared<distance_field::PropagationDistanceField>(
       size_.x(), size_.y(), size_.z(), resolution_, origin_.x() - 0.5 * size_.x(), origin_.y() - 0.5 * size_.y(),
       origin_.z() - 0.5 * size_.z(), max_propogation_distance_, use_signed_distance_field_);
diff --git a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp
index f1abf3fb4c..7f1686022b 100644
--- a/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp
+++ b/moveit_core/collision_distance_field/src/collision_env_hybrid.cpp
@@ -47,7 +47,7 @@ CollisionEnvHybrid::CollisionEnvHybrid(
     double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution,
     double collision_tolerance, double max_propogation_distance, double padding, double scale)
   : CollisionEnvFCL(robot_model)
-  , cenv_distance_(new collision_detection::CollisionEnvDistanceField(
+  , cenv_distance_(std::make_shared<collision_detection::CollisionEnvDistanceField>(
         robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, origin, use_signed_distance_field,
         resolution, collision_tolerance, max_propogation_distance, padding, scale))
 {
@@ -59,7 +59,7 @@ CollisionEnvHybrid::CollisionEnvHybrid(
     double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution,
     double collision_tolerance, double max_propogation_distance, double padding, double scale)
   : CollisionEnvFCL(robot_model, world, padding, scale)
-  , cenv_distance_(new collision_detection::CollisionEnvDistanceField(
+  , cenv_distance_(std::make_shared<collision_detection::CollisionEnvDistanceField>(
         robot_model, getWorld(), link_body_decompositions, size_x, size_y, size_z, origin, use_signed_distance_field,
         resolution, collision_tolerance, max_propogation_distance, padding, scale))
 {
@@ -67,7 +67,8 @@ CollisionEnvHybrid::CollisionEnvHybrid(
 
 CollisionEnvHybrid::CollisionEnvHybrid(const CollisionEnvHybrid& other, const WorldPtr& world)
   : CollisionEnvFCL(other, world)
-  , cenv_distance_(new collision_detection::CollisionEnvDistanceField(*other.getCollisionWorldDistanceField(), world))
+  , cenv_distance_(std::make_shared<collision_detection::CollisionEnvDistanceField>(
+        *other.getCollisionWorldDistanceField(), world))
 {
 }
 
diff --git a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp
index fe36904ab1..b4a3b96afc 100644
--- a/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp
+++ b/moveit_core/collision_distance_field/test/test_collision_distance_field.cpp
@@ -292,7 +292,7 @@ TEST_F(DistanceFieldCollisionDetectionTester, AttachedBodyTester)
   const auto identity = Eigen::Isometry3d::Identity();
   std::vector<shapes::ShapeConstPtr> shapes;
   EigenSTL::vector_Isometry3d poses;
-  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.25, .25, .25)));
+  shapes.push_back(std::make_shared<const shapes::Box>(.25, .25, .25));
   poses.push_back(identity);
   std::set<std::string> touch_links;
   trajectory_msgs::msg::JointTrajectory empty_state;
diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp
index d69403a6fc..fdf8b850e9 100644
--- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp
+++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp
@@ -107,7 +107,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni
     // if we have constrained every joint, then we just use a sampler using these constraints
     if (full_coverage)
     {
-      JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName()));
+      JointConstraintSamplerPtr sampler = std::make_shared<JointConstraintSampler>(scene, jmg->getName());
       if (sampler->configure(jc))
       {
         RCLCPP_DEBUG(LOGGER, "Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
@@ -119,7 +119,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni
         // sampler has been specified.
         if (!jc.empty())
     {
-      JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName()));
+      JointConstraintSamplerPtr sampler = std::make_shared<JointConstraintSampler>(scene, jmg->getName());
       if (sampler->configure(jc))
       {
         RCLCPP_DEBUG(LOGGER,
@@ -166,7 +166,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni
           if (pc->configure(constr.position_constraints[p], scene->getTransforms()) &&
               oc->configure(constr.orientation_constraints[o], scene->getTransforms()))
           {
-            IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName()));
+            IKConstraintSamplerPtr iks = std::make_shared<IKConstraintSampler>(scene, jmg->getName());
             if (iks->configure(IKSamplingPose(pc, oc)))
             {
               bool use = true;
@@ -202,7 +202,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni
           new kinematic_constraints::PositionConstraint(scene->getRobotModel()));
       if (pc->configure(position_constraint, scene->getTransforms()))
       {
-        IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName()));
+        IKConstraintSamplerPtr iks = std::make_shared<IKConstraintSampler>(scene, jmg->getName());
         if (iks->configure(IKSamplingPose(pc)))
         {
           bool use = true;
@@ -232,7 +232,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni
           new kinematic_constraints::OrientationConstraint(scene->getRobotModel()));
       if (oc->configure(orientation_constraint, scene->getTransforms()))
       {
-        IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName()));
+        IKConstraintSamplerPtr iks = std::make_shared<IKConstraintSampler>(scene, jmg->getName());
         if (iks->configure(IKSamplingPose(oc)))
         {
           bool use = true;
@@ -258,7 +258,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni
       else
       {
         samplers.push_back(used_l.begin()->second);
-        return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
+        return std::make_shared<UnionConstraintSampler>(scene, jmg->getName(), samplers);
       }
     }
     else if (used_l.size() > 1)
@@ -284,7 +284,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni
       else
       {
         samplers.push_back(iks);
-        return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
+        return std::make_shared<UnionConstraintSampler>(scene, jmg->getName(), samplers);
       }
     }
   }
@@ -343,7 +343,7 @@ ConstraintSamplerPtr ConstraintSamplerManager::selectDefaultSampler(const planni
     {
       RCLCPP_DEBUG(LOGGER, "Constructing sampler for group '%s' as a union of %zu samplers", jmg->getName().c_str(),
                    samplers.size());
-      return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
+      return std::make_shared<UnionConstraintSampler>(scene, jmg->getName(), samplers);
     }
   }
 
diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
index 007dbd17a7..14ce4017c4 100644
--- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
+++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
@@ -197,19 +197,19 @@ IKSamplingPose::IKSamplingPose()
 }
 
 IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraint& pc)
-  : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
+  : position_constraint_(std::make_shared<kinematic_constraints::PositionConstraint>(pc))
 {
 }
 
 IKSamplingPose::IKSamplingPose(const kinematic_constraints::OrientationConstraint& oc)
-  : orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
+  : orientation_constraint_(std::make_shared<kinematic_constraints::OrientationConstraint>(oc))
 {
 }
 
 IKSamplingPose::IKSamplingPose(const kinematic_constraints::PositionConstraint& pc,
                                const kinematic_constraints::OrientationConstraint& oc)
-  : position_constraint_(new kinematic_constraints::PositionConstraint(pc))
-  , orientation_constraint_(new kinematic_constraints::OrientationConstraint(oc))
+  : position_constraint_(std::make_shared<kinematic_constraints::PositionConstraint>(pc))
+  , orientation_constraint_(std::make_shared<kinematic_constraints::OrientationConstraint>(oc))
 {
 }
 
diff --git a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp
index 15a1a0422e..fb30b530f3 100644
--- a/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp
+++ b/moveit_core/constraint_samplers/test/test_constraint_samplers.cpp
@@ -530,7 +530,8 @@ TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler)
   std::vector<kinematic_constraints::JointConstraint> js2;
   js2.push_back(jc2);
 
-  constraint_samplers::JointConstraintSamplerPtr jcsp2(new constraint_samplers::JointConstraintSampler(ps_, "arms"));
+  constraint_samplers::JointConstraintSamplerPtr jcsp2 =
+      std::make_shared<constraint_samplers::JointConstraintSampler>(ps_, "arms");
   EXPECT_TRUE(jcsp2->configure(js2));
 
   kinematic_constraints::PositionConstraint pc(robot_model_);
@@ -539,7 +540,8 @@ TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler)
   kinematic_constraints::OrientationConstraint oc(robot_model_);
   EXPECT_TRUE(oc.configure(ocm, tf));
 
-  constraint_samplers::IKConstraintSamplerPtr iksp(new constraint_samplers::IKConstraintSampler(ps_, "left_arm"));
+  constraint_samplers::IKConstraintSamplerPtr iksp =
+      std::make_shared<constraint_samplers::IKConstraintSampler>(ps_, "left_arm");
   EXPECT_TRUE(iksp->configure(constraint_samplers::IKSamplingPose(pc, oc)));
   EXPECT_TRUE(iksp->isValid());
 
@@ -583,7 +585,8 @@ TEST_F(LoadPlanningModelsPr2, UnionConstraintSampler)
   kinematic_constraints::OrientationConstraint oc2(robot_model_);
   EXPECT_TRUE(oc2.configure(ocm, tf));
 
-  constraint_samplers::IKConstraintSamplerPtr iksp2(new constraint_samplers::IKConstraintSampler(ps_, "right_arm"));
+  constraint_samplers::IKConstraintSamplerPtr iksp2 =
+      std::make_shared<constraint_samplers::IKConstraintSampler>(ps_, "right_arm");
   EXPECT_TRUE(iksp2->configure(constraint_samplers::IKSamplingPose(pc2, oc2)));
   EXPECT_TRUE(iksp2->isValid());
 
diff --git a/moveit_core/distance_field/test/test_distance_field.cpp b/moveit_core/distance_field/test/test_distance_field.cpp
index 347983c5d3..3aa26b017e 100644
--- a/moveit_core/distance_field/test/test_distance_field.cpp
+++ b/moveit_core/distance_field/test/test_distance_field.cpp
@@ -908,7 +908,7 @@ TEST(TestSignedPropagationDistanceField, TestOcTree)
   std::cout << "Occupied cells " << countOccupiedCells(df_highres) << '\n';
 
   // testing adding shape that happens to be octree
-  std::shared_ptr<octomap::OcTree> tree_shape(new octomap::OcTree(.05));
+  auto tree_shape = std::make_shared<octomap::OcTree>(.05);
   octomap::point3d tpoint1(1.0, .5, 1.0);
   octomap::point3d tpoint2(1.7, .5, .5);
   octomap::point3d tpoint3(1.8, .5, .5);
@@ -916,7 +916,7 @@ TEST(TestSignedPropagationDistanceField, TestOcTree)
   tree_shape->updateNode(tpoint2, true);
   tree_shape->updateNode(tpoint3, true);
 
-  std::shared_ptr<shapes::OcTree> shape_oc(new shapes::OcTree(tree_shape));
+  auto shape_oc = std::make_shared<shapes::OcTree>(tree_shape);
 
   PropagationDistanceField df_test_shape_1(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION, PERF_ORIGIN_X,
                                            PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false);
diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
index 28b6b66d1a..acac1617b3 100644
--- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
+++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
@@ -758,7 +758,7 @@ void OrientationConstraint::print(std::ostream& out) const
 }
 
 VisibilityConstraint::VisibilityConstraint(const moveit::core::RobotModelConstPtr& model)
-  : KinematicConstraint(model), collision_env_(new collision_detection::CollisionEnvFCL(model))
+  : KinematicConstraint(model), collision_env_(std::make_shared<collision_detection::CollisionEnvFCL>(model))
 {
   type_ = VISIBILITY_CONSTRAINT;
 }
diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
index 55c99ea0b7..bc32280d4c 100644
--- a/moveit_core/planning_scene/src/planning_scene.cpp
+++ b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -173,7 +173,7 @@ void PlanningScene::initialize()
 moveit::core::RobotModelPtr PlanningScene::createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
                                                             const srdf::ModelConstSharedPtr& srdf_model)
 {
-  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model, srdf_model));
+  auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
   if (!robot_model || !robot_model->getRootJoint())
     return moveit::core::RobotModelPtr();
 
@@ -495,7 +495,7 @@ moveit::core::RobotState& PlanningScene::getCurrentStateNonConst()
 
 moveit::core::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_msgs::msg::RobotState& update) const
 {
-  moveit::core::RobotStatePtr state(new moveit::core::RobotState(getCurrentState()));
+  auto state = std::make_shared<moveit::core::RobotState>(getCurrentState());
   moveit::core::robotStateMsgToRobotState(getTransforms(), update, *state);
   return state;
 }
@@ -1276,11 +1276,11 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::msg::Octomap& map)
   if (!map.header.frame_id.empty())
   {
     const Eigen::Isometry3d& t = getFrameTransform(map.header.frame_id);
-    world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), t);
+    world_->addToObject(OCTOMAP_NS, std::make_shared<const shapes::OcTree>(om), t);
   }
   else
   {
-    world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), Eigen::Isometry3d::Identity());
+    world_->addToObject(OCTOMAP_NS, std::make_shared<const shapes::OcTree>(om), Eigen::Isometry3d::Identity());
   }
 }
 
@@ -1316,7 +1316,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::msg::OctomapWithPose&
   Eigen::Isometry3d p;
   PlanningScene::poseMsgToEigen(map.origin, p);
   p = t * p;
-  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), p);
+  world_->addToObject(OCTOMAP_NS, std::make_shared<const shapes::OcTree>(om), p);
 }
 
 void PlanningScene::processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree, const Eigen::Isometry3d& t)
@@ -1349,7 +1349,7 @@ void PlanningScene::processOctomapPtr(const std::shared_ptr<const octomap::OcTre
   }
   // if the octree pointer changed, update the structure
   world_->removeObject(OCTOMAP_NS);
-  world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(octree)), t);
+  world_->addToObject(OCTOMAP_NS, std::make_shared<const shapes::OcTree>(octree), t);
 }
 
 bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject& object)
diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp
index aaca8c337a..b41b0ec5cc 100644
--- a/moveit_core/planning_scene/test/test_planning_scene.cpp
+++ b/moveit_core/planning_scene/test/test_planning_scene.cpp
@@ -51,7 +51,7 @@
 TEST(PlanningScene, LoadRestore)
 {
   urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
-  srdf::ModelSharedPtr srdf_model(new srdf::Model());
+  srdf::ModelSharedPtr srdf_model = std::make_shared<srdf::Model>();
   planning_scene::PlanningScene ps(urdf_model, srdf_model);
   moveit_msgs::msg::PlanningScene ps_msg;
   ps.getPlanningSceneMsg(ps_msg);
@@ -65,14 +65,14 @@ TEST(PlanningScene, LoadRestore)
 TEST(PlanningScene, LoadRestoreDiff)
 {
   urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
-  srdf::ModelSharedPtr srdf_model(new srdf::Model());
+  srdf::ModelSharedPtr srdf_model = std::make_shared<srdf::Model>();
   auto ps = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);
 
   collision_detection::World& world = *ps->getWorldNonConst();
 
   /* add one object to ps's world */
   Eigen::Isometry3d id = Eigen::Isometry3d::Identity();
-  world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
+  world.addToObject("sphere", std::make_shared<const shapes::Sphere>(0.4), id);
 
   /* ps can be written to and set from message */
   moveit_msgs::msg::PlanningScene ps_msg;
@@ -90,7 +90,7 @@ TEST(PlanningScene, LoadRestoreDiff)
   EXPECT_TRUE(next->getWorld()->hasObject("sphere"));
 
   /* object in overlay is only added in overlay */
-  next->getWorldNonConst()->addToObject("sphere_in_next_only", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id);
+  next->getWorldNonConst()->addToObject("sphere_in_next_only", std::make_shared<const shapes::Sphere>(0.5), id);
   EXPECT_EQ(next->getWorld()->size(), 2u);
   EXPECT_EQ(ps->getWorld()->size(), 1u);
 
@@ -125,13 +125,13 @@ TEST(PlanningScene, LoadRestoreDiff)
 TEST(PlanningScene, MakeAttachedDiff)
 {
   urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
-  srdf::ModelSharedPtr srdf_model(new srdf::Model());
+  srdf::ModelSharedPtr srdf_model = std::make_shared<srdf::Model>();
   auto ps = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);
 
   /* add a single object to ps's world */
   collision_detection::World& world = *ps->getWorldNonConst();
   Eigen::Isometry3d id = Eigen::Isometry3d::Identity();
-  world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
+  world.addToObject("sphere", std::make_shared<const shapes::Sphere>(0.4), id);
 
   /* attach object in diff */
   planning_scene::PlanningScenePtr attached_object_diff_scene = ps->diff();
@@ -277,7 +277,7 @@ TEST_P(CollisionDetectorTests, ClearDiff)
   SCOPED_TRACE(plugin_name);
 
   urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
-  srdf::ModelSharedPtr srdf_model(new srdf::Model());
+  srdf::ModelSharedPtr srdf_model = std::make_shared<srdf::Model>();
   // create parent scene
   planning_scene::PlanningScenePtr parent = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);
 
diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp
index 1421b98cdf..5330d64f46 100644
--- a/moveit_core/robot_state/test/robot_state_test.cpp
+++ b/moveit_core/robot_state/test/robot_state_test.cpp
@@ -374,7 +374,7 @@ class OneRobot : public testing::Test
         "</robot>";
 
     urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
-    srdf::ModelSharedPtr srdf_model(new srdf::Model());
+    auto srdf_model = std::make_shared<srdf::Model>();
     srdf_model->initString(*urdf_model, SMODEL2);
     robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
   }
diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp
index 0c0c2de208..88dbf676e1 100644
--- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp
+++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp
@@ -229,7 +229,7 @@ class OneRobot : public testing::Test
         "</robot>";
 
     urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
-    srdf::ModelSharedPtr srdf_model(new srdf::Model());
+    srdf::ModelSharedPtr srdf_model = std::make_shared<srdf::Model>();
     srdf_model->initString(*urdf_model, SMODEL2);
     robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
   }
@@ -247,7 +247,7 @@ std::size_t generateTestTraj(std::vector<std::shared_ptr<moveit::core::RobotStat
 {
   traj.clear();
 
-  std::shared_ptr<moveit::core::RobotState> robot_state(new moveit::core::RobotState(robot_model_));
+  auto robot_state = std::make_shared<moveit::core::RobotState>(robot_model_);
   robot_state->setToDefaultValues();
   double ja, jc;
 
diff --git a/moveit_core/robot_state/test/test_kinematic_complex.cpp b/moveit_core/robot_state/test/test_kinematic_complex.cpp
index fb7a9f8c51..1b221aed0d 100644
--- a/moveit_core/robot_state/test/test_kinematic_complex.cpp
+++ b/moveit_core/robot_state/test/test_kinematic_complex.cpp
@@ -77,7 +77,7 @@ TEST_F(LoadPlanningModelsPr2, InitOK)
 
 TEST_F(LoadPlanningModelsPr2, ModelInit)
 {
-  srdf::ModelSharedPtr srdf_model(new srdf::Model());
+  auto srdf_model = std::make_shared<srdf::Model>();
 
   // with no world multidof we should get a fixed joint
   moveit::core::RobotModel robot_model0(urdf_model_, srdf_model);
@@ -121,7 +121,7 @@ TEST_F(LoadPlanningModelsPr2, GroupInit)
                                      "</group>"
                                      "</robot>";
 
-  srdf::ModelSharedPtr srdf_model(new srdf::Model());
+  auto srdf_model = std::make_shared<srdf::Model>();
   srdf_model->initString(*urdf_model_, SMODEL1);
   moveit::core::RobotModel robot_model1(urdf_model_, srdf_model);
 
@@ -150,7 +150,7 @@ TEST_F(LoadPlanningModelsPr2, GroupInit)
                                      "</robot>";
   srdf_model->initString(*urdf_model_, SMODEL2);
 
-  moveit::core::RobotModelPtr robot_model2(new moveit::core::RobotModel(urdf_model_, srdf_model));
+  auto robot_model2 = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_model);
 
   left_arm_base_tip_group = robot_model2->getJointModelGroup("left_arm_base_tip");
   ASSERT_TRUE(left_arm_base_tip_group != nullptr);
@@ -219,13 +219,13 @@ TEST_F(LoadPlanningModelsPr2, SubgroupInit)
 
 TEST_F(LoadPlanningModelsPr2, AssociatedFixedLinks)
 {
-  moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdf_model_, srdf_model_));
+  auto model = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_model_);
   EXPECT_TRUE(model->getLinkModel("r_gripper_palm_link")->getAssociatedFixedTransforms().size() > 1);
 }
 
 TEST_F(LoadPlanningModelsPr2, FullTest)
 {
-  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model_, srdf_model_));
+  auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_model_);
 
   moveit::core::RobotState ks(robot_model);
   ks.setToDefaultValues();
@@ -268,7 +268,7 @@ TEST_F(LoadPlanningModelsPr2, FullTest)
 
 TEST_F(LoadPlanningModelsPr2, ObjectPoseAndSubframes)
 {
-  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model_, srdf_model_));
+  auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_model_);
 
   moveit::core::RobotState ks(robot_model);
   ks.setToDefaultValues();
diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp
index 794ea9d78a..bbb3bcd3a3 100644
--- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp
+++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp
@@ -387,7 +387,7 @@ RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::Robo
   for (std::size_t i = 0; i < state_count; ++i)
   {
     this_time_stamp = rclcpp::Time(trajectory.header.stamp) + trajectory.points[i].time_from_start;
-    moveit::core::RobotStatePtr st(new moveit::core::RobotState(copy));
+    auto st = std::make_shared<moveit::core::RobotState>(copy);
     st->setVariablePositions(trajectory.joint_names, trajectory.points[i].positions);
     if (!trajectory.points[i].velocities.empty())
       st->setVariableVelocities(trajectory.joint_names, trajectory.points[i].velocities);
@@ -418,7 +418,7 @@ RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::Robo
 
   for (std::size_t i = 0; i < state_count; ++i)
   {
-    moveit::core::RobotStatePtr st(new moveit::core::RobotState(copy));
+    auto st = std::make_shared<moveit::core::RobotState>(copy);
     if (trajectory.joint_trajectory.points.size() > i)
     {
       st->setVariablePositions(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].positions);
diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp
index 9feacc7d37..5bfbd3b7e5 100644
--- a/moveit_core/utils/src/robot_model_test_utils.cpp
+++ b/moveit_core/utils/src/robot_model_test_utils.cpp
@@ -54,7 +54,7 @@ moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name)
 {
   urdf::ModelInterfaceSharedPtr urdf = loadModelInterface(robot_name);
   srdf::ModelSharedPtr srdf = loadSRDFModel(robot_name);
-  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf, srdf));
+  auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf, srdf);
   return robot_model;
 }
 
@@ -83,7 +83,7 @@ urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name)
 srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name)
 {
   urdf::ModelInterfaceSharedPtr urdf_model = loadModelInterface(robot_name);
-  srdf::ModelSharedPtr srdf_model(new srdf::Model());
+  auto srdf_model = std::make_shared<srdf::Model>();
   std::string srdf_path;
   if (robot_name == "pr2")
   {
@@ -102,12 +102,12 @@ srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name)
 }
 
 RobotModelBuilder::RobotModelBuilder(const std::string& name, const std::string& base_link_name)
-  : urdf_model_(new urdf::ModelInterface()), srdf_writer_(new srdf::SRDFWriter())
+  : urdf_model_(std::make_shared<urdf::ModelInterface>()), srdf_writer_(std::make_shared<srdf::SRDFWriter>())
 {
   urdf_model_->clear();
   urdf_model_->name_ = name;
 
-  urdf::LinkSharedPtr base_link(new urdf::Link);
+  auto base_link = std::make_shared<urdf::Link>();
   base_link->name = base_link_name;
   urdf_model_->links_.insert(std::make_pair(base_link_name, base_link));
 
@@ -151,10 +151,10 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string&
       is_valid_ = false;
       return;
     }
-    urdf::LinkSharedPtr link(new urdf::Link);
+    auto link = std::make_shared<urdf::Link>();
     link->name = link_names[i];
     urdf_model_->links_.insert(std::make_pair(link_names[i], link));
-    urdf::JointSharedPtr joint(new urdf::Joint);
+    auto joint = std::make_shared<urdf::Joint>();
     joint->name = link_names[i - 1] + "-" + link_names[i] + "-joint";
     // Default to Identity transform for origins.
     joint->parent_to_joint_origin_transform.clear();
@@ -190,7 +190,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string&
     joint->axis = joint_axis;
     if (joint->type == urdf::Joint::REVOLUTE || joint->type == urdf::Joint::PRISMATIC)
     {
-      urdf::JointLimitsSharedPtr limits(new urdf::JointLimits);
+      auto limits = std::make_shared<urdf::JointLimits>();
       limits->lower = -boost::math::constants::pi<double>();
       limits->upper = boost::math::constants::pi<double>();
 
@@ -210,7 +210,7 @@ void RobotModelBuilder::addInertial(const std::string& link_name, double mass, g
     return;
   }
 
-  urdf::InertialSharedPtr inertial(new urdf::Inertial);
+  auto inertial = std::make_shared<urdf::Inertial>();
   inertial->origin.position = urdf::Vector3(origin.position.x, origin.position.y, origin.position.z);
   inertial->origin.rotation =
       urdf::Rotation(origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
@@ -230,8 +230,8 @@ void RobotModelBuilder::addInertial(const std::string& link_name, double mass, g
 void RobotModelBuilder::addVisualBox(const std::string& link_name, const std::vector<double>& size,
                                      geometry_msgs::msg::Pose origin)
 {
-  urdf::VisualSharedPtr vis(new urdf::Visual);
-  urdf::BoxSharedPtr geometry(new urdf::Box);
+  auto vis = std::make_shared<urdf::Visual>();
+  auto geometry = std::make_shared<urdf::Box>();
   geometry->dim = urdf::Vector3(size[0], size[1], size[2]);
   vis->geometry = geometry;
   addLinkVisual(link_name, vis, origin);
@@ -246,8 +246,8 @@ void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std:
     is_valid_ = false;
     return;
   }
-  urdf::CollisionSharedPtr coll(new urdf::Collision);
-  urdf::BoxSharedPtr geometry(new urdf::Box);
+  auto coll = std::make_shared<urdf::Collision>();
+  auto geometry = std::make_shared<urdf::Box>();
   geometry->dim = urdf::Vector3(dims[0], dims[1], dims[2]);
   coll->geometry = geometry;
   addLinkCollision(link_name, coll, origin);
@@ -256,8 +256,8 @@ void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std:
 void RobotModelBuilder::addCollisionMesh(const std::string& link_name, const std::string& filename,
                                          geometry_msgs::msg::Pose origin)
 {
-  urdf::CollisionSharedPtr coll(new urdf::Collision);
-  urdf::MeshSharedPtr geometry(new urdf::Mesh);
+  auto coll = std::make_shared<urdf::Collision>();
+  auto geometry = std::make_shared<urdf::Mesh>();
   geometry->filename = filename;
   coll->geometry = geometry;
   addLinkCollision(link_name, coll, origin);
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 77ed723b60..c19130877d 100644
--- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp
+++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp
@@ -219,7 +219,7 @@ allocConstraintApproximationStateSampler(const ob::StateSpace* space, const std:
   if (sig != expected_signature)
     return ompl::base::StateSamplerPtr();
   else
-    return ompl::base::StateSamplerPtr(new ConstraintApproximationStateSampler(space, state_storage, milestones));
+    return std::make_shared<ConstraintApproximationStateSampler>(space, state_storage, milestones);
 }
 }  // namespace ompl_interface
 
@@ -343,9 +343,8 @@ void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std:
     hexToMsg(serialization, msg);
     auto* cass = new ConstraintApproximationStateStorage(context_->getOMPLSimpleSetup()->getStateSpace());
     cass->load((std::string{ path }.append("/").append(filename)).c_str());
-    ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions,
-                                                               msg, filename, ompl::base::StateStoragePtr(cass),
-                                                               milestones));
+    auto cap = std::make_shared<ConstraintApproximation>(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());
     constraint_approximations_[cap->getName()] = cap;
@@ -461,11 +460,11 @@ ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs
   RCLCPP_INFO(LOGGER, "Spent %lf seconds constructing the database", (clock.now() - start).seconds());
   if (state_storage)
   {
-    ConstraintApproximationPtr constraint_approx(new ConstraintApproximation(
+    auto constraint_approx = std::make_shared<ConstraintApproximation>(
         group, options.state_space_parameterization, options.explicit_motions, constr_hard,
         group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) +
             ".ompldb",
-        state_storage, res.milestones));
+        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());
     constraint_approximations_[constraint_approx->getName()] = constraint_approx;
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 14b4375264..9a31b45d19 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
@@ -119,8 +119,7 @@ void ompl_interface::ModelBasedPlanningContext::configure(const rclcpp::Node::Sh
     ompl::base::ScopedState<> ompl_start_state(spec_.constrained_state_space_);
     spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
     ompl_simple_setup_->setStartState(ompl_start_state);
-    ompl_simple_setup_->setStateValidityChecker(
-        ob::StateValidityCheckerPtr(std::make_shared<ConstrainedPlanningStateValidityChecker>(this)));
+    ompl_simple_setup_->setStateValidityChecker(std::make_shared<ConstrainedPlanningStateValidityChecker>(this));
   }
   else
   {
@@ -166,7 +165,7 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str
   {
     std::string link_name = peval.substr(5, peval.length() - 6);
     if (getRobotModel()->hasLinkModel(link_name))
-      return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorLinkPose(this, link_name));
+      return std::make_shared<ProjectionEvaluatorLinkPose>(this, link_name);
     else
       RCLCPP_ERROR(LOGGER,
                    "Attempted to set projection evaluator with respect to position of link '%s', "
@@ -213,7 +212,7 @@ ompl_interface::ModelBasedPlanningContext::getProjectionEvaluator(const std::str
     }
     else
     {
-      return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorJointValue(this, j));
+      return std::make_shared<ProjectionEvaluatorJointValue>(this, j);
     }
   }
   else
@@ -266,7 +265,7 @@ ompl_interface::ModelBasedPlanningContext::allocPathConstrainedSampler(const omp
     if (constraint_sampler)
     {
       RCLCPP_INFO(LOGGER, "%s: Allocating specialized state sampler for state space", name_.c_str());
-      return ob::StateSamplerPtr(new ConstrainedSampler(this, constraint_sampler));
+      return std::make_shared<ConstrainedSampler>(this, constraint_sampler);
     }
   }
   RCLCPP_DEBUG(LOGGER, "%s: Allocating default state sampler for state space", name_.c_str());
@@ -510,14 +509,14 @@ ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal()
 
     if (constraint_sampler)
     {
-      ob::GoalPtr goal = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraint, constraint_sampler));
+      ob::GoalPtr goal = std::make_shared<ConstrainedGoalSampler>(this, goal_constraint, constraint_sampler);
       goals.push_back(goal);
     }
   }
 
   if (!goals.empty())
   {
-    return goals.size() == 1 ? goals[0] : ompl::base::GoalPtr(new GoalSampleableRegionMux(goals));
+    return goals.size() == 1 ? goals[0] : std::make_shared<GoalSampleableRegionMux>(goals);
   }
   else
   {
diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
index 5edc97861d..9677fae93a 100644
--- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
+++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
@@ -51,7 +51,7 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model
   : node_(node)
   , parameter_namespace_(parameter_namespace)
   , robot_model_(robot_model)
-  , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager())
+  , constraint_sampler_manager_(std::make_shared<constraint_samplers::ConstraintSamplerManager>())
   , context_manager_(robot_model, constraint_sampler_manager_)
   , use_constraints_approximations_(true)
   , simplify_solutions_(true)
@@ -67,7 +67,7 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model
   : node_(node)
   , parameter_namespace_(parameter_namespace)
   , robot_model_(robot_model)
-  , constraint_sampler_manager_(new constraint_samplers::ConstraintSamplerManager())
+  , constraint_sampler_manager_(std::make_shared<constraint_samplers::ConstraintSamplerManager>())
   , context_manager_(robot_model, constraint_sampler_manager_)
   , use_constraints_approximations_(true)
   , simplify_solutions_(true)
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 c84074be5b..1a8caeef7a 100644
--- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp
+++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp
@@ -308,9 +308,9 @@ void PlanningContextManager::registerDefaultPlanners()
 
 void PlanningContextManager::registerDefaultStateSpaces()
 {
-  registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new JointModelStateSpaceFactory()));
-  registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new PoseModelStateSpaceFactory()));
-  registerStateSpaceFactory(ModelBasedStateSpaceFactoryPtr(new ConstrainedPlanningStateSpaceFactory()));
+  registerStateSpaceFactory(std::make_shared<JointModelStateSpaceFactory>());
+  registerStateSpaceFactory(std::make_shared<PoseModelStateSpaceFactory>());
+  registerStateSpaceFactory(std::make_shared<ConstrainedPlanningStateSpaceFactory>());
 }
 
 ConfiguredPlannerSelector PlanningContextManager::getPlannerSelector() const
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp
index 503a1a3c89..682c7b6bec 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/path_circle_generator.cpp
@@ -123,9 +123,9 @@ std::unique_ptr<KDL::Path> PathCircleGenerator::circleFromInterim(const KDL::Fra
   KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis();
   try
   {
-    return std::unique_ptr<KDL::Path>(new KDL::Path_Circle(start_pose, center_point, kdl_aux_point, goal_pose.M, alpha,
-                                                           rot_interpo, eqradius,
-                                                           true /* take ownership of RotationalInterpolation */));
+    return std::unique_ptr<KDL::Path>(
+        std::make_unique<KDL::Path_Circle>(start_pose, center_point, kdl_aux_point, goal_pose.M, alpha, rot_interpo,
+                                           eqradius, true /* take ownership of RotationalInterpolation */));
   }
   catch (KDL::Error_MotionPlanning&)  // LCOV_EXCL_START // The cases where
                                       // KDL would throw are already handled
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp
index fe078b2ef4..2e07bf8ce3 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp
@@ -111,7 +111,7 @@ void PlanComponentsBuilder::append(const planning_scene::PlanningSceneConstPtr&
   {
     traj_tail_ = other;
     // Reserve space in container for new trajectory
-    traj_cont_.emplace_back(new robot_trajectory::RobotTrajectory(model_, other->getGroupName()));
+    traj_cont_.emplace_back(std::make_shared<robot_trajectory::RobotTrajectory>(model_, other->getGroupName()));
     return;
   }
 
@@ -121,7 +121,7 @@ void PlanComponentsBuilder::append(const planning_scene::PlanningSceneConstPtr&
     appendWithStrictTimeIncrease(*(traj_cont_.back()), *traj_tail_);
     traj_tail_ = other;
     // Create new container element
-    traj_cont_.emplace_back(new robot_trajectory::RobotTrajectory(model_, other->getGroupName()));
+    traj_cont_.emplace_back(std::make_shared<robot_trajectory::RobotTrajectory>(model_, other->getGroupName()));
     return;
   }
 
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 7d96cd3f2d..484d98604a 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
@@ -267,7 +267,7 @@ void TrajectoryGenerator::setSuccessResponse(const moveit::core::RobotState& sta
                                              const rclcpp::Time& planning_start,
                                              planning_interface::MotionPlanResponse& res) const
 {
-  robot_trajectory::RobotTrajectoryPtr rt(new robot_trajectory::RobotTrajectory(robot_model_, group_name));
+  auto rt = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, group_name);
   rt->setRobotTrajectoryMsg(start_state, joint_trajectory);
 
   res.trajectory_ = rt;
@@ -290,9 +290,9 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_sca
                                                   const double& max_acceleration_scaling_factor,
                                                   const std::unique_ptr<KDL::Path>& path) const
 {
-  std::unique_ptr<KDL::VelocityProfile> vp_trans(new KDL::VelocityProfile_Trap(
+  std::unique_ptr<KDL::VelocityProfile> vp_trans = std::make_unique<KDL::VelocityProfile_Trap>(
       max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity(),
-      max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalAcceleration()));
+      max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalAcceleration());
 
   if (path->PathLength() > std::numeric_limits<double>::epsilon())  // avoid division by zero
   {
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 c5de781973..f6af37f2d8 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
@@ -199,7 +199,8 @@ std::unique_ptr<KDL::Path> TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affin
                     planner_limits_.getCartesianLimits().getMaxRotationalVelocity();
   KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis();
 
-  return std::unique_ptr<KDL::Path>(new KDL::Path_Line(kdl_start_pose, kdl_goal_pose, rot_interpo, eqradius, true));
+  return std::unique_ptr<KDL::Path>(
+      std::make_unique<KDL::Path_Line>(kdl_start_pose, kdl_goal_pose, rot_interpo, eqradius, true));
 }
 
 }  // namespace pilz_industrial_motion_planner
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp
index 83eda91c35..1214b00646 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner_direct.cpp
@@ -103,7 +103,8 @@ TEST(CommandPlannerTestDirect, FailOnLoadContext)
   };
 
   /// Registered a found loader
-  pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader(new TestPlanningContextLoader());
+  pilz_industrial_motion_planner::PlanningContextLoaderPtr planning_context_loader =
+      std::make_shared<TestPlanningContextLoader>();
   planner.registerContextLoader(planning_context_loader);
 
   moveit_msgs::msg::MotionPlanRequest req;
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp
index 7e32289394..7e9b7c0d7c 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp
@@ -121,7 +121,7 @@ class PlanningContextTest : public ::testing::Test
         new typename T::Type_("TestPlanningContext", planning_group_, robot_model_, limits));
 
     // Define and set the current scene
-    planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(robot_model_));
+    auto scene = std::make_shared<planning_scene::PlanningScene>(robot_model_);
     moveit::core::RobotState current_state(robot_model_);
     current_state.setToDefaultValues();
     current_state.setJointGroupPositions(planning_group_, std::vector<double>{ 0, 1.57, 1.57, 0, 0.2, 0 });
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp
index 385e802b1f..b4fe4c7600 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp
@@ -131,8 +131,7 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test
     planner_limits.setCartesianLimits(cart_limits);
 
     // create planner instance
-    trajectory_generator_ =
-        std::unique_ptr<typename T::Type_>(new typename T::Type_(robot_model_, planner_limits, planning_group_));
+    trajectory_generator_ = std::make_unique<typename T::Type_>(robot_model_, planner_limits, planning_group_);
     ASSERT_NE(nullptr, trajectory_generator_) << "failed to create trajectory generator";
 
     // create a valid motion plan request with goal in joint space as basis for
diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp
index c7753b3c0c..c162f6578a 100644
--- a/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp
+++ b/moveit_planners/sbpl/core/sbpl_interface/src/environment_chain3d.cpp
@@ -647,15 +647,15 @@ void EnvironmentChain3D::setMotionPrimitives(const std::string& group_name)
   for (unsigned int i = 0; i < jmg->getActiveDOFNames().size(); ++i)
   {
     const planning_models::RobotModel::JointModel* joint = jmg->getJointModel(jmg->getActiveDOFNames()[i]);
-    boost::shared_ptr<JointMotionWrapper> jmw(new JointMotionWrapper(joint));
+    auto jmw = boost::make_shared<JointMotionWrapper>(joint);
     joint_motion_wrappers_.push_back(jmw);
     // TODO - figure out which DOFs have something to do with end effector position
     if (!planning_parameters_.use_bfs_ || i < 4)
     {
-      boost::shared_ptr<SingleJointMotionPrimitive> sing_pos(
-          new SingleJointMotionPrimitive(jmw, i, planning_parameters_.joint_motion_primitive_distance_));
-      boost::shared_ptr<SingleJointMotionPrimitive> sing_neg(
-          new SingleJointMotionPrimitive(jmw, i, -planning_parameters_.joint_motion_primitive_distance_));
+      auto sing_pos =
+          boost::make_shared<SingleJointMotionPrimitive>(jmw, i, planning_parameters_.joint_motion_primitive_distance_);
+      auto sing_neg = boost::make_shared<SingleJointMotionPrimitive>(
+          jmw, i, -planning_parameters_.joint_motion_primitive_distance_);
       possible_actions_.push_back(sing_pos);
       possible_actions_.push_back(sing_neg);
     }
diff --git a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp
index afafeba5d8..f60464db37 100644
--- a/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp
+++ b/moveit_planners/sbpl/core/sbpl_interface/src/sbpl_interface.cpp
@@ -49,7 +49,7 @@ bool SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_
                                              start_state);
 
   ros::WallTime wt = ros::WallTime::now();
-  boost::shared_ptr<EnvironmentChain3D> env_chain(new EnvironmentChain3D(planning_scene));
+  auto env_chain = boost::make_shared<EnvironmentChain3D>(planning_scene);
   if (!env_chain->setupForMotionPlan(planning_scene, req, res, params))
   {
     // std::cerr << "Env chain setup failing" << '\n';
@@ -60,7 +60,7 @@ bool SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_
   boost::this_thread::interruption_point();
 
   // DummyEnvironment* dummy_env = new DummyEnvironment();
-  boost::shared_ptr<ARAPlanner> planner(new ARAPlanner(env_chain.get(), true));
+  auto planner = boost::make_shared<ARAPlanner>(env_chain.get(), true);
   planner->set_initialsolution_eps(100.0);
   planner->set_search_mode(true);
   planner->force_planning_from_scratch();
diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp
index d9d6f8cb8e..7479184c92 100644
--- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp
+++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp
@@ -384,8 +384,7 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase
 bool IKFastKinematicsPlugin::computeRelativeTransform(const std::string& from, const std::string& to,
                                                       Eigen::Isometry3d& transform, bool& differs_from_identity)
 {
-  RobotStatePtr robot_state;
-  robot_state.reset(new RobotState(robot_model_));
+  RobotStatePtr robot_state = std::make_shared<RobotState>(robot_model_);
   robot_state->setToDefaultValues();
 
   auto* from_link = robot_model_->getLinkModel(from);
diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h
index 6ad995e4ff..3a84346eee 100644
--- a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h
+++ b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h
@@ -286,8 +286,7 @@ struct CartPoseTermInfo : public TermInfo
 
   static TermInfoPtr create()
   {
-    TermInfoPtr out(new CartPoseTermInfo());
-    return out;
+    return std::make_shared<CartPoseTermInfo>();
   }
 };
 
@@ -326,8 +325,7 @@ struct JointPoseTermInfo : public TermInfo
 
   static TermInfoPtr create()
   {
-    TermInfoPtr out(new JointPoseTermInfo());
-    return out;
+    return std::make_shared<JointPoseTermInfo>();
   }
 };
 
@@ -356,8 +354,7 @@ struct JointVelTermInfo : public TermInfo
 
   static TermInfoPtr create()
   {
-    TermInfoPtr out(new JointVelTermInfo());
-    return out;
+    return std::make_shared<JointVelTermInfo>();
   }
 };
 
diff --git a/moveit_planners/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/src/problem_description.cpp
index 6fc2df1860..7b492c2f0c 100644
--- a/moveit_planners/trajopt/src/problem_description.cpp
+++ b/moveit_planners/trajopt/src/problem_description.cpp
@@ -177,7 +177,7 @@ TrajOptProblemPtr ConstructProblem(const ProblemInfo& pci)
   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");
 
-  TrajOptProblemPtr prob(new TrajOptProblem(pci));
+  auto prob = std::make_shared<TrajOptProblem>(pci);
 
   // Generate initial trajectory and check its size
   moveit::core::RobotModelConstPtr robot_model = pci.planning_scene->getRobotModel();
@@ -291,15 +291,15 @@ void CartPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob)
   }
   else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME))
   {
-    sco::VectorOfVectorPtr f(new CartPoseErrCalculator(input_pose, prob.GetPlanningScene(), link, tcp));
-    prob.addCost(sco::CostPtr(new sco::CostFromErrFunc(f, prob.GetVarRow(timestep, 0, n_dof),
-                                                       concatVector(rot_coeffs, pos_coeffs), sco::ABS, name)));
+    sco::VectorOfVectorPtr f = std::make_shared<CartPoseErrCalculator>(input_pose, prob.GetPlanningScene(), link, tcp);
+    prob.addCost(std::make_shared<sco::CostFromErrFunc>(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::VectorOfVectorPtr f(new CartPoseErrCalculator(input_pose, prob.GetPlanningScene(), link, tcp));
-    prob.addConstraint(sco::ConstraintPtr(new sco::ConstraintFromErrFunc(
-        f, prob.GetVarRow(timestep, 0, n_dof), concatVector(rot_coeffs, pos_coeffs), sco::EQ, name)));
+    sco::VectorOfVectorPtr f = std::make_shared<CartPoseErrCalculator>(input_pose, prob.GetPlanningScene(), link, tcp);
+    prob.addConstraint(std::make_shared<sco::ConstraintFromErrFunc>(
+        f, prob.GetVarRow(timestep, 0, n_dof), concatVector(rot_coeffs, pos_coeffs), sco::EQ, name));
   }
   else
   {
@@ -361,15 +361,15 @@ void JointPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob)
     // 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(sco::CostPtr(new trajopt::JointPosEqCost(joint_vars, util::toVectorXd(coeffs),
-                                                            util::toVectorXd(targets), first_step, last_step)));
+      prob.addCost(std::make_shared<trajopt::JointPosEqCost>(joint_vars, util::toVectorXd(coeffs),
+                                                             util::toVectorXd(targets), first_step, last_step));
       prob.getCosts().back()->setName(name);
     }
     else
     {
-      prob.addCost(sco::CostPtr(new trajopt::JointPosIneqCost(joint_vars, util::toVectorXd(coeffs),
-                                                              util::toVectorXd(targets), util::toVectorXd(upper_tols),
-                                                              util::toVectorXd(lower_tols), first_step, last_step)));
+      prob.addCost(std::make_shared<trajopt::JointPosIneqCost>(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);
     }
   }
@@ -378,15 +378,15 @@ void JointPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob)
     // 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(sco::ConstraintPtr(new trajopt::JointPosEqConstraint(
-          joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step)));
+      prob.addConstraint(std::make_shared<trajopt::JointPosEqConstraint>(
+          joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step));
       prob.getConstraints().back()->setName(name);
     }
     else
     {
-      prob.addConstraint(sco::ConstraintPtr(new trajopt::JointPosIneqConstraint(
+      prob.addConstraint(std::make_shared<trajopt::JointPosIneqConstraint>(
           joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols),
-          util::toVectorXd(lower_tols), first_step, last_step)));
+          util::toVectorXd(lower_tols), first_step, last_step));
       prob.getConstraints().back()->setName(name);
     }
   }
@@ -458,19 +458,19 @@ void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob)
       if (is_upper_zeros && is_lower_zeros)
       {
         trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]);
-        prob.addCost(sco::CostPtr(new sco::CostFromErrFunc(
-            sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])),
-            sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec),
-            util::toVectorXd(single_jnt_coeffs), sco::SQUARED, name + "_j" + std::to_string(j))));
+        prob.addCost(std::make_shared<sco::CostFromErrFunc>(
+            std::make_shared<JointVelErrCalculator>(targets[j], upper_tols[j], lower_tols[j]),
+            std::make_shared<JointVelJacobianCalculator>(), 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(sco::CostPtr(new sco::CostFromErrFunc(
-            sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])),
-            sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec),
-            util::toVectorXd(single_jnt_coeffs), sco::HINGE, name + "_j" + std::to_string(j))));
+        prob.addCost(std::make_shared<sco::CostFromErrFunc>(
+            std::make_shared<JointVelErrCalculator>(targets[j], upper_tols[j], lower_tols[j]),
+            std::make_shared<JointVelJacobianCalculator>(), concatVector(joint_vars_vec, time_vars_vec),
+            util::toVectorXd(single_jnt_coeffs), sco::HINGE, name + "_j" + std::to_string(j)));
       }
     }
   }
@@ -489,18 +489,18 @@ void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob)
       if (is_upper_zeros && is_lower_zeros)
       {
         trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]);
-        prob.addConstraint(sco::ConstraintPtr(new sco::ConstraintFromErrFunc(
-            sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])),
-            sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec),
-            util::toVectorXd(single_jnt_coeffs), sco::EQ, name + "_j" + std::to_string(j))));
+        prob.addConstraint(std::make_shared<sco::ConstraintFromErrFunc>(
+            std::make_shared<JointVelErrCalculator>(targets[j], upper_tols[j], lower_tols[j]),
+            std::make_shared<JointVelJacobianCalculator>(), 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(sco::ConstraintPtr(new sco::ConstraintFromErrFunc(
-            sco::VectorOfVectorPtr(new JointVelErrCalculator(targets[j], upper_tols[j], lower_tols[j])),
-            sco::MatrixOfVectorPtr(new JointVelJacobianCalculator()), concatVector(joint_vars_vec, time_vars_vec),
+        prob.addConstraint(std::make_shared<sco::ConstraintFromErrFunc>(
+            std::make_shared<JointVelErrCalculator>(targets[j], upper_tols[j], lower_tols[j]),
+            std::make_shared<JointVelJacobianCalculator>(), concatVector(joint_vars_vec, time_vars_vec),
             util::toVectorXd(single_jnt_coeffs), sco::INEQ, name + "_j" + std::to_string(j))));
       }
     }
@@ -510,15 +510,15 @@ void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob)
     // 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(sco::CostPtr(new trajopt::JointVelEqCost(joint_vars, util::toVectorXd(coeffs),
-                                                            util::toVectorXd(targets), first_step, last_step)));
+      prob.addCost(std::make_shared<trajopt::JointVelEqCost>(joint_vars, util::toVectorXd(coeffs),
+                                                             util::toVectorXd(targets), first_step, last_step));
       prob.getCosts().back()->setName(name);
     }
     else
     {
-      prob.addCost(sco::CostPtr(new trajopt::JointVelIneqCost(joint_vars, util::toVectorXd(coeffs),
-                                                              util::toVectorXd(targets), util::toVectorXd(upper_tols),
-                                                              util::toVectorXd(lower_tols), first_step, last_step)));
+      prob.addCost(std::make_shared<trajopt::JointVelIneqCost>(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);
     }
   }
@@ -527,15 +527,15 @@ void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob)
     // 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(sco::ConstraintPtr(new trajopt::JointVelEqConstraint(
-          joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step)));
+      prob.addConstraint(std::make_shared<trajopt::JointVelEqConstraint>(
+          joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step));
       prob.getConstraints().back()->setName(name);
     }
     else
     {
-      prob.addConstraint(sco::ConstraintPtr(new trajopt::JointVelIneqConstraint(
+      prob.addConstraint(std::make_shared<trajopt::JointVelIneqConstraint>(
           joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols),
-          util::toVectorXd(lower_tols), first_step, last_step)));
+          util::toVectorXd(lower_tols), first_step, last_step));
       prob.getConstraints().back()->setName(name);
     }
   }
diff --git a/moveit_planners/trajopt/src/trajopt_interface.cpp b/moveit_planners/trajopt/src/trajopt_interface.cpp
index bf9da1494d..cf78e2667f 100644
--- a/moveit_planners/trajopt/src/trajopt_interface.cpp
+++ b/moveit_planners/trajopt/src/trajopt_interface.cpp
@@ -60,7 +60,7 @@ namespace trajopt_interface
 {
 TrajOptInterface::TrajOptInterface(const ros::NodeHandle& nh) : nh_(nh), name_("TrajOptInterface")
 {
-  trajopt_problem_ = TrajOptProblemPtr(new TrajOptProblem);
+  trajopt_problem_ = std::make_shared<TrajOptProblem>();
   setDefaultTrajOPtParams();
 
   // TODO: callbacks should be defined by the user
@@ -87,7 +87,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
   bool robot_model_ok = static_cast<bool>(robot_model);
   if (!robot_model_ok)
     ROS_ERROR_STREAM_NAMED(name_, "robot model is not loaded properly");
-  moveit::core::RobotStatePtr current_state(new moveit::core::RobotState(robot_model));
+  auto current_state = std::make_shared<moveit::core::RobotState>(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)
@@ -151,7 +151,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
   ROS_INFO(" ======================================= Cartesian Constraints");
   if (!req.goal_constraints[0].position_constraints.empty() && !req.goal_constraints[0].orientation_constraints.empty())
   {
-    CartPoseTermInfoPtr cart_goal_pos(new CartPoseTermInfo);
+    CartPoseTermInfoPtr cart_goal_pos = std::make_shared<CartPoseTermInfo>();
 
     // TODO: Feed cart_goal_pos with request information and the needed param to the setup.yaml file
     // TODO: Multiple Cartesian constraints
@@ -177,7 +177,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
   ROS_INFO(" ======================================= Constraints from request goal_constraints");
   for (auto goal_cnt : req.goal_constraints)
   {
-    JointPoseTermInfoPtr joint_pos_term(new JointPoseTermInfo);
+    JointPoseTermInfoPtr joint_pos_term = std::make_shared<JointPoseTermInfo>();
     // When using MotionPlanning Display in RViz, the created request has no name for the constriant
     setJointPoseTermInfoParams(joint_pos_term, (goal_cnt.name != "") ? goal_cnt.name : "goal_tmp");
 
@@ -192,7 +192,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
 
   ROS_INFO(" ======================================= Constraints from request start_state");
   // add the start pos from request as a constraint
-  JointPoseTermInfoPtr joint_start_pos(new JointPoseTermInfo);
+  auto joint_start_pos = std::make_shared<JointPoseTermInfo>();
 
   joint_start_pos->targets = start_joint_values;
   setJointPoseTermInfoParams(joint_start_pos, "start_pos");
@@ -200,7 +200,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
 
   ROS_INFO(" ======================================= Velocity Constraints, hard-coded");
   // TODO: should be defined by user, its parameters should be added to setup.yaml
-  JointVelTermInfoPtr joint_vel(new JointVelTermInfo);
+  auto joint_vel = std::make_shared<JointVelTermInfo>();
 
   joint_vel->coeffs = std::vector<double>(dof, 5.0);
   joint_vel->targets = std::vector<double>(dof, 0.0);
diff --git a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp b/moveit_planners/trajopt/src/trajopt_planner_manager.cpp
index 1ed418f5ca..c8f591e102 100644
--- a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp
+++ b/moveit_planners/trajopt/src/trajopt_planner_manager.cpp
@@ -65,8 +65,7 @@ class TrajOptPlannerManager : public planning_interface::PlannerManager
     {
       ROS_INFO(" ======================================= group name: %s, robot model: %s", gpName.c_str(),
                model->getName().c_str());
-      planning_contexts_[gpName] =
-          TrajOptPlanningContextPtr(new TrajOptPlanningContext("trajopt_planning_context", gpName, model));
+      planning_contexts_[gpName] = std::make_shared<TrajOptPlanningContext>("trajopt_planning_context", gpName, model);
     }
 
     return true;
diff --git a/moveit_planners/trajopt/src/trajopt_planning_context.cpp b/moveit_planners/trajopt/src/trajopt_planning_context.cpp
index af6e3ab0f9..24590366b1 100644
--- a/moveit_planners/trajopt/src/trajopt_planning_context.cpp
+++ b/moveit_planners/trajopt/src/trajopt_planning_context.cpp
@@ -17,7 +17,7 @@ TrajOptPlanningContext::TrajOptPlanningContext(const std::string& context_name,
   : planning_interface::PlanningContext(context_name, group_name), robot_model_(model)
 {
   ROS_INFO(" ======================================= TrajOptPlanningContext is constructed");
-  trajopt_interface_ = TrajOptInterfacePtr(new TrajOptInterface());
+  trajopt_interface_ = std::make_shared<TrajOptInterface>();
 }
 
 bool TrajOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res)
@@ -28,8 +28,7 @@ bool TrajOptPlanningContext::solve(planning_interface::MotionPlanDetailedRespons
   if (trajopt_solved)
   {
     res.trajectory_.resize(1);
-    res.trajectory_[0] =
-        robot_trajectory::RobotTrajectoryPtr(new robot_trajectory::RobotTrajectory(robot_model_, getGroupName()));
+    res.trajectory_[0] = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, getGroupName());
 
     moveit::core::RobotState start_state(robot_model_);
     moveit::core::robotStateMsgToRobotState(res_msg.trajectory_start, start_state);
diff --git a/moveit_planners/trajopt/test/trajectory_test.cpp b/moveit_planners/trajopt/test/trajectory_test.cpp
index 7d3e6aaed0..4f571ddb9d 100644
--- a/moveit_planners/trajopt/test/trajectory_test.cpp
+++ b/moveit_planners/trajopt/test/trajectory_test.cpp
@@ -91,7 +91,7 @@ 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
-  moveit::core::RobotStatePtr current_state(new moveit::core::RobotState(robot_model_));
+  auto current_state = std::make_shared<moveit::core::RobotState>(robot_model_);
   current_state->setToDefaultValues();
 
   const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLANNING_GROUP);
@@ -99,7 +99,7 @@ TEST_F(TrajectoryTest, goalTolerance)
   const std::vector<std::string>& joint_names = joint_model_group->getActiveJointModelNames();
   EXPECT_EQ(joint_names.size(), 7);
 
-  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model_));
+  auto planning_scene = std::make_shared<planning_scene::PlanningScene>(robot_model_);
 
   // Create response and request
   planning_interface::MotionPlanRequest req;
@@ -108,7 +108,7 @@ TEST_F(TrajectoryTest, goalTolerance)
   // Set start state
   // ======================================================================================
   std::vector<double> start_joint_values = { 0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 };
-  moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(robot_model_));
+  auto start_state = std::make_shared<moveit::core::RobotState>(robot_model_);
   start_state->setJointGroupPositions(joint_model_group, start_joint_values);
   start_state->update();
 
@@ -119,7 +119,7 @@ TEST_F(TrajectoryTest, goalTolerance)
 
   // Set the goal state and joints tolerance
   // ========================================================================================
-  moveit::core::RobotStatePtr goal_state(new moveit::core::RobotState(robot_model_));
+  auto goal_state = std::make_shared<moveit::core::RobotState>(robot_model_);
   std::vector<double> 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();
diff --git a/moveit_plugins/moveit_controller_manager_example/src/moveit_controller_manager_example.cpp b/moveit_plugins/moveit_controller_manager_example/src/moveit_controller_manager_example.cpp
index 5f319c1ca2..70efcb7469 100644
--- a/moveit_plugins/moveit_controller_manager_example/src/moveit_controller_manager_example.cpp
+++ b/moveit_plugins/moveit_controller_manager_example/src/moveit_controller_manager_example.cpp
@@ -84,7 +84,7 @@ class MoveItControllerManagerExample : public moveit_controller_manager::MoveItC
 
   moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name) override
   {
-    return moveit_controller_manager::MoveItControllerHandlePtr(new ExampleControllerHandle(name));
+    return std::make_shared<ExampleControllerHandle>(name);
   }
 
   /*
diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp
index a28e83bc8b..d2954826ec 100644
--- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp
+++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp
@@ -98,7 +98,7 @@ bool samplePossibleGoalStates(const ManipulationPlanPtr& plan, const moveit::cor
                               double min_distance, unsigned int attempts)
 {
   // initialize with scene state
-  moveit::core::RobotStatePtr token_state(new moveit::core::RobotState(reference_state));
+  auto token_state = std::make_shared<moveit::core::RobotState>(reference_state);
   for (unsigned int j = 0; j < attempts; ++j)
   {
     double min_d = std::numeric_limits<double>::infinity();
@@ -164,7 +164,7 @@ void addGripperTrajectory(const ManipulationPlanPtr& plan,
     moveit::core::RobotStatePtr ee_closed_state(
         new moveit::core::RobotState(plan->trajectories_.back().trajectory_->getLastWayPoint()));
 
-    robot_trajectory::RobotTrajectoryPtr ee_closed_traj(new robot_trajectory::RobotTrajectory(
+    auto ee_closed_traj = std::make_shared<robot_trajectory::RobotTrajectory>(
         ee_closed_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
     ee_closed_traj->setRobotTrajectoryMsg(*ee_closed_state, plan->retreat_posture_);
     // If user has defined a time for it's gripper movement time, don't add the
@@ -243,7 +243,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const
       if (plan->shared_data_->minimize_object_distance_)
       {
         static const double MAX_CLOSE_UP_DIST = 1.0;
-        moveit::core::RobotStatePtr close_up_state(new moveit::core::RobotState(*plan->possible_goal_states_[i]));
+        auto close_up_state = std::make_shared<moveit::core::RobotState>(*plan->possible_goal_states_[i]);
         std::vector<moveit::core::RobotStatePtr> close_up_states;
         double d_close_up = moveit::core::CartesianInterpolator::computeCartesianPath(
             close_up_state.get(), plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_,
@@ -255,7 +255,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const
       }
 
       // try to compute a straight line path that arrives at the goal using the specified approach direction
-      moveit::core::RobotStatePtr first_approach_state(new moveit::core::RobotState(*plan->possible_goal_states_[i]));
+      auto first_approach_state = std::make_shared<moveit::core::RobotState>(*plan->possible_goal_states_[i]);
 
       std::vector<moveit::core::RobotStatePtr> approach_states;
       double d_approach = moveit::core::CartesianInterpolator::computeCartesianPath(
@@ -300,13 +300,13 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const
           {
             // Create approach trajectory
             std::reverse(approach_states.begin(), approach_states.end());
-            robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
+            auto approach_traj = std::make_shared<robot_trajectory::RobotTrajectory>(
                 planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
             for (const moveit::core::RobotStatePtr& approach_state : approach_states)
               approach_traj->addSuffixWayPoint(approach_state, 0.0);
 
             // Create retreat trajectory
-            robot_trajectory::RobotTrajectoryPtr retreat_traj(new robot_trajectory::RobotTrajectory(
+            auto retreat_traj = std::make_shared<robot_trajectory::RobotTrajectory>(
                 planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
             for (const moveit::core::RobotStatePtr& retreat_state : retreat_states)
               retreat_traj->addSuffixWayPoint(retreat_state, 0.0);
@@ -339,7 +339,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const
 
           // Create approach trajectory
           std::reverse(approach_states.begin(), approach_states.end());
-          robot_trajectory::RobotTrajectoryPtr approach_traj(new robot_trajectory::RobotTrajectory(
+          auto approach_traj = std::make_shared<robot_trajectory::RobotTrajectory>(
               planning_scene_->getRobotModel(), plan->shared_data_->planning_group_->getName()));
           for (const moveit::core::RobotStatePtr& approach_state : approach_states)
             approach_traj->addSuffixWayPoint(approach_state, 0.0);
diff --git a/moveit_ros/manipulation/pick_place/src/pick.cpp b/moveit_ros/manipulation/pick_place/src/pick.cpp
index 2d83fbff40..ec1067dc31 100644
--- a/moveit_ros/manipulation/pick_place/src/pick.cpp
+++ b/moveit_ros/manipulation/pick_place/src/pick.cpp
@@ -123,7 +123,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
   ros::WallTime start_time = ros::WallTime::now();
 
   // construct common data for possible manipulation plans
-  ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
+  auto plan_data = std::make_shared<ManipulationPlanSharedData>();
   ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
   plan_data->planning_group_ = planning_scene->getRobotModel()->getJointModelGroup(planning_group);
   plan_data->end_effector_group_ = eef;
@@ -162,8 +162,8 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
   pipeline_.reset();
   ManipulationStagePtr stage1(
       new ReachableAndValidPoseFilter(planning_scene, approach_grasp_acm, pick_place_->getConstraintsSamplerManager()));
-  ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_grasp_acm));
-  ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
+  ManipulationStagePtr stage2 = std::make_shared<ApproachAndTranslateStage>(planning_scene, approach_grasp_acm);
+  ManipulationStagePtr stage3 = std::make_shared<PlanStage>(planning_scene, pick_place_->getPlanningPipeline());
   pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
 
   initialize();
@@ -180,7 +180,7 @@ bool PickPlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene,
   // feed the available grasps to the stages we set up
   for (std::size_t i = 0; i < goal.possible_grasps.size(); ++i)
   {
-    ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
+    auto p = std::make_shared<ManipulationPlan>(const_plan_data);
     const moveit_msgs::msg::Grasp& g = goal.possible_grasps[grasp_order[i]];
     p->approach_ = g.pre_grasp_approach;
     p->retreat_ = g.post_grasp_retreat;
diff --git a/moveit_ros/manipulation/pick_place/src/place.cpp b/moveit_ros/manipulation/pick_place/src/place.cpp
index 0852bf2aab..a383cb1ed5 100644
--- a/moveit_ros/manipulation/pick_place/src/place.cpp
+++ b/moveit_ros/manipulation/pick_place/src/place.cpp
@@ -272,7 +272,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
   ros::WallTime start_time = ros::WallTime::now();
 
   // construct common data for possible manipulation plans
-  ManipulationPlanSharedDataPtr plan_data(new ManipulationPlanSharedData());
+  auto plan_data = std::make_shared<ManipulationPlanSharedData>();
   ManipulationPlanSharedDataConstPtr const_plan_data = plan_data;
   plan_data->planning_group_ = jmg;
   plan_data->end_effector_group_ = eef;
@@ -315,8 +315,8 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
 
   ManipulationStagePtr stage1(
       new ReachableAndValidPoseFilter(planning_scene, approach_place_acm, pick_place_->getConstraintsSamplerManager()));
-  ManipulationStagePtr stage2(new ApproachAndTranslateStage(planning_scene, approach_place_acm));
-  ManipulationStagePtr stage3(new PlanStage(planning_scene, pick_place_->getPlanningPipeline()));
+  ManipulationStagePtr stage2 = std::make_shared<ApproachAndTranslateStage>(planning_scene, approach_place_acm);
+  ManipulationStagePtr stage3 = std::make_shared<PlanStage>(planning_scene, pick_place_->getPlanningPipeline());
   pipeline_.addStage(stage1).addStage(stage2).addStage(stage3);
 
   initialize();
@@ -334,7 +334,7 @@ bool PlacePlan::plan(const planning_scene::PlanningSceneConstPtr& planning_scene
   // add possible place locations
   for (std::size_t i = 0; i < goal.place_locations.size(); ++i)
   {
-    ManipulationPlanPtr p(new ManipulationPlan(const_plan_data));
+    auto p = std::make_shared<ManipulationPlan>(const_plan_data);
     const moveit_msgs::action::PlaceLocation& pl = goal.place_locations[place_locations_order[i]];
 
     if (goal.place_eef)
diff --git a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp
index daa1f8e51f..27c639c436 100644
--- a/moveit_ros/manipulation/pick_place/src/plan_stage.cpp
+++ b/moveit_ros/manipulation/pick_place/src/plan_stage.cpp
@@ -78,8 +78,8 @@ bool PlanStage::evaluate(const ManipulationPlanPtr& plan) const
       // We have a valid motion plan, now apply pre-approach end effector posture (open gripper) if applicable
       if (!plan->approach_posture_.joint_names.empty())
       {
-        moveit::core::RobotStatePtr pre_approach_state(new moveit::core::RobotState(res.trajectory_->getLastWayPoint()));
-        robot_trajectory::RobotTrajectoryPtr pre_approach_traj(new robot_trajectory::RobotTrajectory(
+        auto pre_approach_state = std::make_shared<moveit::core::RobotState>(res.trajectory_->getLastWayPoint()));
+        auto pre_approach_traj = std::make_shared<robot_trajectory::RobotTrajectory>(
             pre_approach_state->getRobotModel(), plan->shared_data_->end_effector_group_->getName()));
         pre_approach_traj->setRobotTrajectoryMsg(*pre_approach_state, plan->approach_posture_);
 
diff --git a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp
index 2f9e5dbacd..39e9d3e488 100644
--- a/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp
+++ b/moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp
@@ -111,7 +111,7 @@ bool pick_place::ReachableAndValidPoseFilter::isEndEffectorFree(const Manipulati
 bool pick_place::ReachableAndValidPoseFilter::evaluate(const ManipulationPlanPtr& plan) const
 {
   // initialize with scene state
-  moveit::core::RobotStatePtr token_state(new moveit::core::RobotState(planning_scene_->getCurrentState()));
+  auto token_state = std::make_shared<moveit::core::RobotState>(planning_scene_->getCurrentState());
   if (isEndEffectorFree(plan, *token_state))
   {
     // update the goal pose message if anything has changed; this is because the name of the frame in the input goal
diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp
index d29e7dde20..513e307524 100644
--- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp
+++ b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp
@@ -171,7 +171,7 @@ mesh_filter::MeshHandle mesh_filter::MeshFilterBase::addMesh(const shapes::Mesh&
 {
   std::unique_lock<std::mutex> _(meshes_mutex_);
 
-  JobPtr job(new FilterJob<void>(std::bind(&MeshFilterBase::addMeshHelper, this, next_handle_, &mesh)));
+  JobPtr job = std::make_shared<FilterJob<void>>(std::bind(&MeshFilterBase::addMeshHelper, this, next_handle_, &mesh));
   addJob(job);
   job->wait();
   mesh_filter::MeshHandle ret = next_handle_;
@@ -217,16 +217,17 @@ void mesh_filter::MeshFilterBase::setShadowThreshold(float threshold)
 
 void mesh_filter::MeshFilterBase::getModelLabels(LabelType* labels) const
 {
-  JobPtr job(new FilterJob<void>(std::bind(&GLRenderer::getColorBuffer, mesh_renderer_.get(), (unsigned char*)labels)));
+  JobPtr job = std::make_shared<FilterJob<void>>(
+      std::bind(&GLRenderer::getColorBuffer, mesh_renderer_.get(), (unsigned char*)labels));
   addJob(job);
   job->wait();
 }
 
 void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const
 {
-  JobPtr job1(new FilterJob<void>(std::bind(&GLRenderer::getDepthBuffer, mesh_renderer_.get(), depth)));
-  JobPtr job2(new FilterJob<void>(
-      std::bind(&SensorModel::Parameters::transformModelDepthToMetricDepth, sensor_parameters_.get(), depth)));
+  JobPtr job1 = std::make_shared<FilterJob<void>>(std::bind(&GLRenderer::getDepthBuffer, mesh_renderer_.get(), depth));
+  JobPtr job2 = std::make_shared<FilterJob<void>>(
+      std::bind(&SensorModel::Parameters::transformModelDepthToMetricDepth, sensor_parameters_.get(), depth));
   {
     std::unique_lock<std::mutex> lock(jobs_mutex_);
     jobs_queue_.push(job1);
@@ -239,9 +240,9 @@ void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const
 
 void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const
 {
-  JobPtr job1(new FilterJob<void>(std::bind(&GLRenderer::getDepthBuffer, depth_filter_.get(), depth)));
-  JobPtr job2(new FilterJob<void>(
-      std::bind(&SensorModel::Parameters::transformFilteredDepthToMetricDepth, sensor_parameters_.get(), depth)));
+  JobPtr job1 = std::make_shared<FilterJob<void>>(std::bind(&GLRenderer::getDepthBuffer, depth_filter_.get(), depth));
+  JobPtr job2 = std::make_shared<FilterJob<void>>(
+      std::bind(&SensorModel::Parameters::transformFilteredDepthToMetricDepth, sensor_parameters_.get(), depth));
   {
     std::unique_lock<std::mutex> lock(jobs_mutex_);
     jobs_queue_.push(job1);
@@ -254,7 +255,8 @@ void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const
 
 void mesh_filter::MeshFilterBase::getFilteredLabels(LabelType* labels) const
 {
-  JobPtr job(new FilterJob<void>(std::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels)));
+  JobPtr job = std::make_shared<FilterJob<void>>(
+      std::bind(&GLRenderer::getColorBuffer, depth_filter_.get(), (unsigned char*)labels));
   addJob(job);
   job->wait();
 }
@@ -294,7 +296,7 @@ void mesh_filter::MeshFilterBase::filter(const void* sensor_data, GLushort type,
     throw std::runtime_error(msg.str());
   }
 
-  JobPtr job(new FilterJob<void>(std::bind(&MeshFilterBase::doFilter, this, sensor_data, type)));
+  JobPtr job = std::make_shared<FilterJob<void>>(std::bind(&MeshFilterBase::doFilter, this, sensor_data, type));
   addJob(job);
   if (wait)
     job->wait();
diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp
index 650805e979..84556b1280 100644
--- a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp
+++ b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp
@@ -91,7 +91,7 @@ class ConstraintSamplerManagerLoader::Helper
 ConstraintSamplerManagerLoader::ConstraintSamplerManagerLoader(
     const rclcpp::Node::SharedPtr& node, const constraint_samplers::ConstraintSamplerManagerPtr& csm)
   : constraint_sampler_manager_(csm ? csm : std::make_shared<constraint_samplers::ConstraintSamplerManager>())
-  , impl_(new Helper(node, constraint_sampler_manager_))
+  , impl_(std::make_shared<Helper>(node, constraint_sampler_manager_))
 {
 }
 }  // namespace constraint_sampler_manager_loader
diff --git a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp
index 7dfd0d5505..1cb46d46b0 100644
--- a/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp
+++ b/moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp
@@ -305,7 +305,7 @@ int main(int argc, char** argv)
 
   robot_model = moveit::core::loadTestingRobotModel("panda");
 
-  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));
+  auto scene = std::make_shared<planning_scene::PlanningScene>(robot_model);
   planning_scene_monitor::PlanningSceneMonitor psm(planning_scene, ROBOT_DESCRIPTION);
   psm.startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE);
   psm.startSceneMonitor();
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 475074074c..4f92339f26 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
@@ -67,7 +67,7 @@ int main(int argc, char** argv)
 
   robot_model_loader::RobotModelLoader::Options opt;
   opt.robot_description_ = "robot_description";
-  robot_model_loader::RobotModelLoaderPtr rml(new robot_model_loader::RobotModelLoader(node, opt));
+  auto rml = std::make_shared<robot_model_loader::RobotModelLoader>(node, opt);
   planning_scene_monitor::PlanningSceneMonitor psm(node, rml);
   psm.startWorldGeometryMonitor();
   psm.startSceneMonitor();
diff --git a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp
index 40ceac4d5e..d73033fc16 100644
--- a/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp
+++ b/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp
@@ -76,7 +76,8 @@ int main(int argc, char** argv)
     robot_model_loader::RobotModelLoader::Options opt;
     opt.robot_description_ = "robot_description";
     opt.load_kinematics_solvers_ = false;
-    robot_model_loader::RobotModelLoaderPtr rml(new robot_model_loader::RobotModelLoader(node, opt));
+
+    auto rml = std::make_shared<robot_model_loader::RobotModelLoader>(node, opt);
     planning_scene::PlanningScene ps(rml->getModel());
 
     std::ifstream f(argv[filename_index]);
diff --git a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp
index f894c625e0..fe4bca6537 100644
--- a/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp
+++ b/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp
@@ -107,7 +107,7 @@ int main(int argc, char** argv)
       t.translation() = Eigen::Vector3d(rng.uniformReal(aabb[0], aabb[1]), rng.uniformReal(aabb[2], aabb[3]),
                                         rng.uniformReal(aabb[4], aabb[5]));
       scene->getWorldNonConst()->clearObjects();
-      scene->getWorldNonConst()->addToObject("test", shapes::ShapeConstPtr(new shapes::Sphere(radius)), t);
+      scene->getWorldNonConst()->addToObject("test", std::make_shared<shapes::Sphere>(radius), t);
       collision_detection::CollisionResult res;
       scene->checkCollision(req, res);
       if (res.collision)
diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp
index 92e2abc57c..ad3b4242b3 100644
--- a/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp
+++ b/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_collision.cpp
@@ -96,7 +96,7 @@ class FixStartStateCollision : public planning_request_adapter::PlanningRequestA
       else
         RCLCPP_INFO(LOGGER, "Start state appears to be in collision with respect to group %s", creq.group_name.c_str());
 
-      moveit::core::RobotStatePtr prefix_state(new moveit::core::RobotState(start_state));
+      auto prefix_state = std::make_shared<moveit::core::RobotState>(start_state);
       random_numbers::RandomNumberGenerator& rng = prefix_state->getRandomNumberGenerator();
 
       const std::vector<const moveit::core::JointModel*>& jmodels =
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 1d4a48c173..a7c8e7e50c 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
@@ -45,7 +45,7 @@ int main(int argc, char** argv)
   rclcpp::init(argc, argv);
   auto node = rclcpp::Node::make_shared("test_trajectory_execution_manager");
 
-  robot_model_loader::RobotModelLoaderPtr rml(new robot_model_loader::RobotModelLoader(node));
+  auto rml = std::make_shared<robot_model_loader::RobotModelLoader>(node);
   planning_scene_monitor::PlanningSceneMonitor psm(node, rml);
   trajectory_execution_manager::TrajectoryExecutionManager tem(node, rml->getModel(), psm.getStateMonitor(), true);
 
diff --git a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h
index 7ff17e7802..a157678483 100644
--- a/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h
+++ b/moveit_ros/planning/trajectory_execution_manager/test/test_moveit_controller_manager.h
@@ -115,7 +115,7 @@ class TestMoveItControllerManager : public moveit_controller_manager::MoveItCont
 
   moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name) override
   {
-    return moveit_controller_manager::MoveItControllerHandlePtr(new TestMoveItControllerHandle(name));
+    return std::make_shared<TestMoveItControllerHandle>(name);
   }
 
   void getControllersList(std::vector<std::string>& names) override
diff --git a/moveit_ros/robot_interaction/src/locked_robot_state.cpp b/moveit_ros/robot_interaction/src/locked_robot_state.cpp
index 27ed768cb8..262711e45c 100644
--- a/moveit_ros/robot_interaction/src/locked_robot_state.cpp
+++ b/moveit_ros/robot_interaction/src/locked_robot_state.cpp
@@ -39,13 +39,13 @@
 #include <moveit/robot_interaction/locked_robot_state.h>
 
 robot_interaction::LockedRobotState::LockedRobotState(const moveit::core::RobotState& state)
-  : state_(new moveit::core::RobotState(state))
+  : state_(std::make_shared<moveit::core::RobotState>(state))
 {
   state_->update();
 }
 
 robot_interaction::LockedRobotState::LockedRobotState(const moveit::core::RobotModelPtr& model)
-  : state_(new moveit::core::RobotState(model))
+  : state_(std::make_shared<moveit::core::RobotState>(model))
 {
   state_->setToDefaultValues();
   state_->update();
diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp
index 825a4e75e6..a610bc7f67 100644
--- a/moveit_ros/robot_interaction/src/robot_interaction.cpp
+++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp
@@ -73,7 +73,7 @@ const std::string RobotInteraction::INTERACTIVE_MARKER_TOPIC = "robot_interactio
 
 RobotInteraction::RobotInteraction(const moveit::core::RobotModelConstPtr& robot_model,
                                    const rclcpp::Node::SharedPtr& node, const std::string& ns)
-  : robot_model_(robot_model), kinematic_options_map_(new KinematicOptionsMap)
+  : robot_model_(robot_model), kinematic_options_map_(std::make_shared<KinematicOptionsMap>())
 {
   topic_ = ns.empty() ? INTERACTIVE_MARKER_TOPIC : ns + "/" + INTERACTIVE_MARKER_TOPIC;
   node_ = node;
diff --git a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp
index 4f3d9fb7b0..000190ebfd 100644
--- a/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp
+++ b/moveit_ros/robot_interaction/test/locked_robot_state_test.cpp
@@ -222,7 +222,7 @@ static moveit::core::RobotModelPtr getModel()
   if (!model)
   {
     urdf::ModelInterfaceSharedPtr urdf(urdf::parseURDF(URDF_STR));
-    srdf::ModelSharedPtr srdf(new srdf::Model());
+    auto srdf = std::make_shared<srdf::Model>();
     srdf->initString(*urdf, SRDF_STR);
     model = std::make_shared<moveit::core::RobotModel>(urdf, srdf);
   }
@@ -240,7 +240,7 @@ TEST(LockedRobotState, load)
   state2.setToDefaultValues();
   robot_interaction::LockedRobotState ls2(state2);
 
-  robot_interaction::LockedRobotStatePtr ls4(new robot_interaction::LockedRobotState(model));
+  auto ls4 = std::make_shared<robot_interaction::LockedRobotState>(model);
 }
 
 // sanity test the URDF and enum
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
index 67cbab8643..768b6309ae 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
@@ -86,8 +86,8 @@ MotionPlanningDisplay::MotionPlanningDisplay()
   , text_to_display_(nullptr)
   , frame_(nullptr)
   , frame_dock_(nullptr)
-  , menu_handler_start_(new interactive_markers::MenuHandler)
-  , menu_handler_goal_(new interactive_markers::MenuHandler)
+  , menu_handler_start_(std::make_shared<interactive_markers::MenuHandler>())
+  , menu_handler_goal_(std::make_shared<interactive_markers::MenuHandler>())
   , int_marker_display_(nullptr)
 {
   // Category Groups
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp
index c3a9d3fa92..842384d109 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp
@@ -751,7 +751,7 @@ void MotionPlanningFrame::computeLoadQueryButtonClicked()
                                                   mp->start_state, *start_state);
           planning_display_->setQueryStartState(*start_state);
 
-          moveit::core::RobotStatePtr goal_state(new moveit::core::RobotState(*planning_display_->getQueryGoalState()));
+          auto goal_state = std::make_shared<moveit::core::RobotState>(*planning_display_->getQueryGoalState());
           for (const moveit_msgs::msg::Constraints& goal_constraint : mp->goal_constraints)
             if (!goal_constraint.joint_constraints.empty())
             {
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
index 86848be8ce..743d2b2f0b 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
@@ -608,7 +608,7 @@ void MotionPlanningFrame::remoteUpdateCustomStartStateCallback(const moveit_msgs
     const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
     if (ps)
     {
-      moveit::core::RobotStatePtr state(new moveit::core::RobotState(ps->getCurrentState()));
+      auto state = std::make_shared<moveit::core::RobotState>(ps->getCurrentState());
       moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state);
       planning_display_->setQueryStartState(*state);
     }
@@ -626,7 +626,7 @@ void MotionPlanningFrame::remoteUpdateCustomGoalStateCallback(const moveit_msgs:
     const planning_scene_monitor::LockedPlanningSceneRO& ps = planning_display_->getPlanningSceneRO();
     if (ps)
     {
-      moveit::core::RobotStatePtr state(new moveit::core::RobotState(ps->getCurrentState()));
+      auto state = std::make_shared<moveit::core::RobotState>(ps->getCurrentState());
       moveit::core::robotStateMsgToRobotState(ps->getTransforms(), msg_no_attached, *state);
       planning_display_->setQueryGoalState(*state);
     }
diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp
index ebcb8fee56..11f46757b5 100644
--- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp
+++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp
@@ -537,7 +537,7 @@ void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::msg::
 
   trajectory_message_to_display_.reset();
 
-  robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(robot_model_, ""));
+  auto t = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, "");
   try
   {
     for (std::size_t i = 0; i < msg->trajectory.size(); ++i)

From b6c0e0e7251c4d04b97dea4dbf892a56190549ff Mon Sep 17 00:00:00 2001
From: Stephanie Eng <stephanie-eng@users.noreply.github.com>
Date: Tue, 29 Mar 2022 15:15:20 -0400
Subject: [PATCH 088/114] Update black version, formatting changes (#1148)

---
 .pre-commit-config.yaml                       |   2 +-
 .../src/moveit_commander/move_group.py        | 132 +++++++++---------
 .../planning_scene_interface.py               |  18 +--
 .../src/moveit_commander/robot.py             |   4 +-
 .../planning_interface/test/serialize_msg.py  |   4 +-
 5 files changed, 80 insertions(+), 80 deletions(-)

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 3dd06105d2..a5f0f0e115 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -33,7 +33,7 @@ repos:
       - id: trailing-whitespace
 
   - repo: https://github.com/psf/black
-    rev: 20.8b1
+    rev: 22.3.0
     hooks:
       - id: black
 
diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py
index f206007200..8c980bccaf 100644
--- a/moveit_commander/src/moveit_commander/move_group.py
+++ b/moveit_commander/src/moveit_commander/move_group.py
@@ -62,68 +62,68 @@ class MoveGroupCommander(object):
     def __init__(
         self, name, robot_description="robot_description", ns="", wait_for_servers=5.0
     ):
-        """ Specify the group name for which to construct this commander instance. Throws an exception if there is an initialization error. """
+        """Specify the group name for which to construct this commander instance. Throws an exception if there is an initialization error."""
         self._g = _moveit_move_group_interface.MoveGroupInterface(
             name, robot_description, ns, wait_for_servers
         )
 
     def get_name(self):
-        """ Get the name of the group this instance was initialized for """
+        """Get the name of the group this instance was initialized for"""
         return self._g.get_name()
 
     def stop(self):
-        """ Stop the current execution, if any """
+        """Stop the current execution, if any"""
         self._g.stop()
 
     def get_active_joints(self):
-        """ Get the active joints of this group """
+        """Get the active joints of this group"""
         return self._g.get_active_joints()
 
     def get_joints(self):
-        """ Get the joints of this group """
+        """Get the joints of this group"""
         return self._g.get_joints()
 
     def get_variable_count(self):
-        """ Return the number of variables used to parameterize a state in this group (larger or equal to number of DOF)"""
+        """Return the number of variables used to parameterize a state in this group (larger or equal to number of DOF)"""
         return self._g.get_variable_count()
 
     def has_end_effector_link(self):
-        """ Check if this group has a link that is considered to be an end effector """
+        """Check if this group has a link that is considered to be an end effector"""
         return len(self._g.get_end_effector_link()) > 0
 
     def get_end_effector_link(self):
-        """ Get the name of the link that is considered to be an end-effector. Return an empty string if there is no end-effector. """
+        """Get the name of the link that is considered to be an end-effector. Return an empty string if there is no end-effector."""
         return self._g.get_end_effector_link()
 
     def set_end_effector_link(self, link_name):
-        """ Set the name of the link to be considered as an end effector """
+        """Set the name of the link to be considered as an end effector"""
         if not self._g.set_end_effector_link(link_name):
             raise MoveItCommanderException("Unable to set end effector link")
 
     def get_interface_description(self):
-        """ Get the description of the planner interface (list of planner ids) """
+        """Get the description of the planner interface (list of planner ids)"""
         desc = PlannerInterfaceDescription()
         conversions.msg_from_string(desc, self._g.get_interface_description())
         return desc
 
     def get_pose_reference_frame(self):
-        """ Get the reference frame assumed for poses of end-effectors """
+        """Get the reference frame assumed for poses of end-effectors"""
         return self._g.get_pose_reference_frame()
 
     def set_pose_reference_frame(self, reference_frame):
-        """ Set the reference frame to assume for poses of end-effectors """
+        """Set the reference frame to assume for poses of end-effectors"""
         self._g.set_pose_reference_frame(reference_frame)
 
     def get_planning_frame(self):
-        """ Get the name of the frame where all planning is performed """
+        """Get the name of the frame where all planning is performed"""
         return self._g.get_planning_frame()
 
     def get_current_joint_values(self):
-        """ Get the current configuration of the group as a list (these are values published on /joint_states) """
+        """Get the current configuration of the group as a list (these are values published on /joint_states)"""
         return self._g.get_current_joint_values()
 
     def get_current_pose(self, end_effector_link=""):
-        """ Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector. """
+        """Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector."""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
             return conversions.list_to_pose_stamped(
                 self._g.get_current_pose(end_effector_link), self.get_planning_frame()
@@ -134,7 +134,7 @@ def get_current_pose(self, end_effector_link=""):
             )
 
     def get_current_rpy(self, end_effector_link=""):
-        """ Get a list of 3 elements defining the [roll, pitch, yaw] of the end-effector. Throws an exception if there is not end-effector. """
+        """Get a list of 3 elements defining the [roll, pitch, yaw] of the end-effector. Throws an exception if there is not end-effector."""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
             return self._g.get_current_rpy(end_effector_link)
         else:
@@ -180,14 +180,14 @@ def set_start_state(self, msg):
         self._g.set_start_state(conversions.msg_to_string(msg))
 
     def get_current_state_bounded(self):
-        """ Get the current state of the robot bounded."""
+        """Get the current state of the robot bounded."""
         s = RobotState()
         c_str = self._g.get_current_state_bounded()
         conversions.msg_from_string(s, c_str)
         return s
 
     def get_current_state(self):
-        """ Get the current state of the robot."""
+        """Get the current state of the robot."""
         s = RobotState()
         c_str = self._g.get_current_state()
         conversions.msg_from_string(s, c_str)
@@ -290,7 +290,7 @@ def set_joint_value_target(self, arg1, arg2=None, arg3=None):
             )
 
     def set_rpy_target(self, rpy, end_effector_link=""):
-        """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
+        """Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
             if len(rpy) == 3:
                 if not self._g.set_rpy_target(
@@ -305,7 +305,7 @@ def set_rpy_target(self, rpy, end_effector_link=""):
             )
 
     def set_orientation_target(self, q, end_effector_link=""):
-        """ Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
+        """Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
             if len(q) == 4:
                 if not self._g.set_orientation_target(
@@ -320,7 +320,7 @@ def set_orientation_target(self, q, end_effector_link=""):
             )
 
     def set_position_target(self, xyz, end_effector_link=""):
-        """ Specify a target position for the end-effector. Any orientation of the end-effector is acceptable."""
+        """Specify a target position for the end-effector. Any orientation of the end-effector is acceptable."""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
             if not self._g.set_position_target(
                 xyz[0], xyz[1], xyz[2], end_effector_link
@@ -332,7 +332,7 @@ def set_position_target(self, xyz, end_effector_link=""):
             )
 
     def set_pose_target(self, pose, end_effector_link=""):
-        """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
+        """Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
         """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
         if len(end_effector_link) > 0 or self.has_end_effector_link():
             ok = False
@@ -357,7 +357,7 @@ def set_pose_target(self, pose, end_effector_link=""):
             )
 
     def set_pose_targets(self, poses, end_effector_link=""):
-        """ Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
+        """Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw]"""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
             if not self._g.set_pose_targets(
                 [conversions.pose_to_list(p) if type(p) is Pose else p for p in poses],
@@ -368,7 +368,7 @@ def set_pose_targets(self, poses, end_effector_link=""):
             raise MoveItCommanderException("There is no end effector to set poses for")
 
     def shift_pose_target(self, axis, value, end_effector_link=""):
-        """ Get the current pose of the end effector, add value to the corresponding axis (0..5: X, Y, Z, R, P, Y) and set the new pose as the pose target """
+        """Get the current pose of the end effector, add value to the corresponding axis (0..5: X, Y, Z, R, P, Y) and set the new pose as the pose target"""
         if len(end_effector_link) > 0 or self.has_end_effector_link():
             pose = self._g.get_current_pose(end_effector_link)
             # by default we get orientation as a quaternion list
@@ -385,23 +385,23 @@ def shift_pose_target(self, axis, value, end_effector_link=""):
             raise MoveItCommanderException("There is no end effector to set poses for")
 
     def clear_pose_target(self, end_effector_link):
-        """ Clear the pose target for a particular end-effector """
+        """Clear the pose target for a particular end-effector"""
         self._g.clear_pose_target(end_effector_link)
 
     def clear_pose_targets(self):
-        """ Clear all known pose targets """
+        """Clear all known pose targets"""
         self._g.clear_pose_targets()
 
     def set_random_target(self):
-        """ Set a random joint configuration target """
+        """Set a random joint configuration target"""
         self._g.set_random_target()
 
     def get_named_targets(self):
-        """ Get a list of all the names of joint configurations."""
+        """Get a list of all the names of joint configurations."""
         return self._g.get_named_targets()
 
     def set_named_target(self, name):
-        """ Set a joint configuration by name. The name can be a name previlusy remembered with remember_joint_values() or a configuration specified in the SRDF. """
+        """Set a joint configuration by name. The name can be a name previlusy remembered with remember_joint_values() or a configuration specified in the SRDF."""
         if not self._g.set_named_target(name):
             raise MoveItCommanderException(
                 "Unable to set target %s. Is the target within bounds?" % name
@@ -412,21 +412,21 @@ def get_named_target_values(self, target):
         return self._g.get_named_target_values(target)
 
     def remember_joint_values(self, name, values=None):
-        """ Record the specified joint configuration of the group under the specified name. If no values are specified, the current state of the group is recorded. """
+        """Record the specified joint configuration of the group under the specified name. If no values are specified, the current state of the group is recorded."""
         if values is None:
             values = self.get_current_joint_values()
         self._g.remember_joint_values(name, values)
 
     def get_remembered_joint_values(self):
-        """ Get a dictionary that maps names to joint configurations for the group """
+        """Get a dictionary that maps names to joint configurations for the group"""
         return self._g.get_remembered_joint_values()
 
     def forget_joint_values(self, name):
-        """ Forget a stored joint configuration """
+        """Forget a stored joint configuration"""
         self._g.forget_joint_values(name)
 
     def get_goal_tolerance(self):
-        """ Return a tuple of goal tolerances: joint, position and orientation. """
+        """Return a tuple of goal tolerances: joint, position and orientation."""
         return (
             self.get_goal_joint_tolerance(),
             self.get_goal_position_tolerance(),
@@ -434,54 +434,54 @@ def get_goal_tolerance(self):
         )
 
     def get_goal_joint_tolerance(self):
-        """ Get the tolerance for achieving a joint goal (distance for each joint variable) """
+        """Get the tolerance for achieving a joint goal (distance for each joint variable)"""
         return self._g.get_goal_joint_tolerance()
 
     def get_goal_position_tolerance(self):
-        """ When moving to a position goal or to a pose goal, the tolerance for the goal position is specified as the radius a sphere around the target origin of the end-effector """
+        """When moving to a position goal or to a pose goal, the tolerance for the goal position is specified as the radius a sphere around the target origin of the end-effector"""
         return self._g.get_goal_position_tolerance()
 
     def get_goal_orientation_tolerance(self):
-        """ When moving to an orientation goal or to a pose goal, the tolerance for the goal orientation is specified as the distance (roll, pitch, yaw) to the target origin of the end-effector """
+        """When moving to an orientation goal or to a pose goal, the tolerance for the goal orientation is specified as the distance (roll, pitch, yaw) to the target origin of the end-effector"""
         return self._g.get_goal_orientation_tolerance()
 
     def set_goal_tolerance(self, value):
-        """ Set the joint, position and orientation goal tolerances simultaneously """
+        """Set the joint, position and orientation goal tolerances simultaneously"""
         self._g.set_goal_tolerance(value)
 
     def set_goal_joint_tolerance(self, value):
-        """ Set the tolerance for a target joint configuration """
+        """Set the tolerance for a target joint configuration"""
         self._g.set_goal_joint_tolerance(value)
 
     def set_goal_position_tolerance(self, value):
-        """ Set the tolerance for a target end-effector position """
+        """Set the tolerance for a target end-effector position"""
         self._g.set_goal_position_tolerance(value)
 
     def set_goal_orientation_tolerance(self, value):
-        """ Set the tolerance for a target end-effector orientation """
+        """Set the tolerance for a target end-effector orientation"""
         self._g.set_goal_orientation_tolerance(value)
 
     def allow_looking(self, value):
-        """ Enable/disable looking around for motion planning """
+        """Enable/disable looking around for motion planning"""
         self._g.allow_looking(value)
 
     def allow_replanning(self, value):
-        """ Enable/disable replanning """
+        """Enable/disable replanning"""
         self._g.allow_replanning(value)
 
     def get_known_constraints(self):
-        """ Get a list of names for the constraints specific for this group, as read from the warehouse """
+        """Get a list of names for the constraints specific for this group, as read from the warehouse"""
         return self._g.get_known_constraints()
 
     def get_path_constraints(self):
-        """ Get the actual path constraints in form of a moveit_msgs.msgs.Constraints """
+        """Get the actual path constraints in form of a moveit_msgs.msgs.Constraints"""
         c = Constraints()
         c_str = self._g.get_path_constraints()
         conversions.msg_from_string(c, c_str)
         return c
 
     def set_path_constraints(self, value):
-        """ Specify the path constraints to be used (as read from the database) """
+        """Specify the path constraints to be used (as read from the database)"""
         if value is None:
             self.clear_path_constraints()
         else:
@@ -493,18 +493,18 @@ def set_path_constraints(self, value):
                 )
 
     def clear_path_constraints(self):
-        """ Specify that no path constraints are to be used during motion planning """
+        """Specify that no path constraints are to be used during motion planning"""
         self._g.clear_path_constraints()
 
     def get_trajectory_constraints(self):
-        """ Get the actual trajectory constraints in form of a moveit_msgs.msgs.TrajectoryConstraints """
+        """Get the actual trajectory constraints in form of a moveit_msgs.msgs.TrajectoryConstraints"""
         c = TrajectoryConstraints()
         c_str = self._g.get_trajectory_constraints()
         conversions.msg_from_string(c, c_str)
         return c
 
     def set_trajectory_constraints(self, value):
-        """ Specify the trajectory constraints to be used (setting from database is not implemented yet)"""
+        """Specify the trajectory constraints to be used (setting from database is not implemented yet)"""
         if value is None:
             self.clear_trajectory_constraints()
         else:
@@ -518,43 +518,43 @@ def set_trajectory_constraints(self, value):
                 )
 
     def clear_trajectory_constraints(self):
-        """ Specify that no trajectory constraints are to be used during motion planning """
+        """Specify that no trajectory constraints are to be used during motion planning"""
         self._g.clear_trajectory_constraints()
 
     def set_constraints_database(self, host, port):
-        """ Specify which database to connect to for loading possible path constraints """
+        """Specify which database to connect to for loading possible path constraints"""
         self._g.set_constraints_database(host, port)
 
     def set_planning_time(self, seconds):
-        """ Specify the amount of time to be used for motion planning. """
+        """Specify the amount of time to be used for motion planning."""
         self._g.set_planning_time(seconds)
 
     def get_planning_time(self):
-        """ Specify the amount of time to be used for motion planning. """
+        """Specify the amount of time to be used for motion planning."""
         return self._g.get_planning_time()
 
     def set_planning_pipeline_id(self, planning_pipeline):
-        """ Specify which planning pipeline to use when motion planning (e.g. ompl, pilz_industrial_motion_planner) """
+        """Specify which planning pipeline to use when motion planning (e.g. ompl, pilz_industrial_motion_planner)"""
         self._g.set_planning_pipeline_id(planning_pipeline)
 
     def get_planning_pipeline_id(self):
-        """ Get the current planning_pipeline_id (e.g. ompl, pilz_industrial_motion_planner) """
+        """Get the current planning_pipeline_id (e.g. ompl, pilz_industrial_motion_planner)"""
         return self._g.get_planning_pipeline_id()
 
     def set_planner_id(self, planner_id):
-        """ Specify which planner of the currently selected pipeline to use when motion planning (e.g. RRTConnect, LIN) """
+        """Specify which planner of the currently selected pipeline to use when motion planning (e.g. RRTConnect, LIN)"""
         self._g.set_planner_id(planner_id)
 
     def get_planner_id(self):
-        """ Get the current planner_id (e.g. RRTConnect, LIN) of the currently selected pipeline """
+        """Get the current planner_id (e.g. RRTConnect, LIN) of the currently selected pipeline"""
         return self._g.get_planner_id()
 
     def set_num_planning_attempts(self, num_planning_attempts):
-        """ Set the number of times the motion plan is to be computed from scratch before the shortest solution is returned. The default value is 1. """
+        """Set the number of times the motion plan is to be computed from scratch before the shortest solution is returned. The default value is 1."""
         self._g.set_num_planning_attempts(num_planning_attempts)
 
     def set_workspace(self, ws):
-        """ Set the workspace for the robot as either [], [minX, minY, maxX, maxY] or [minX, minY, minZ, maxX, maxY, maxZ] """
+        """Set the workspace for the robot as either [], [minX, minY, maxX, maxY] or [minX, minY, minZ, maxX, maxY, maxZ]"""
         if len(ws) == 0:
             self._g.set_workspace(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
         else:
@@ -589,7 +589,7 @@ def set_max_acceleration_scaling_factor(self, value):
             )
 
     def go(self, joints=None, wait=True):
-        """ Set the target of the group and then move the group to the specified target """
+        """Set the target of the group and then move the group to the specified target"""
         if type(joints) is bool:
             wait = joints
             joints = None
@@ -639,7 +639,7 @@ def plan(self, joints=None):
         )
 
     def construct_motion_plan_request(self):
-        """ Returns a MotionPlanRequest filled with the current goals of the move_group_interface"""
+        """Returns a MotionPlanRequest filled with the current goals of the move_group_interface"""
         mpr = MotionPlanRequest()
         return mpr.deserialize(self._g.construct_motion_plan_request())
 
@@ -651,7 +651,7 @@ def compute_cartesian_path(
         avoid_collisions=True,
         path_constraints=None,
     ):
-        """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
+        """Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory."""
         if path_constraints:
             if type(path_constraints) is Constraints:
                 constraints_str = conversions.msg_to_string(path_constraints)
@@ -687,11 +687,11 @@ def execute(self, plan_msg, wait=True):
             return self._g.async_execute(conversions.msg_to_string(plan_msg))
 
     def attach_object(self, object_name, link_name="", touch_links=[]):
-        """ Given the name of an object existing in the planning scene, attach it to a link. The link used is specified by the second argument. If left unspecified, the end-effector link is used, if one is known. If there is no end-effector link, the first link in the group is used. If no link is identified, failure is reported. True is returned if an attach request was successfully sent to the move_group node. This does not verify that the attach request also was successfully applied by move_group."""
+        """Given the name of an object existing in the planning scene, attach it to a link. The link used is specified by the second argument. If left unspecified, the end-effector link is used, if one is known. If there is no end-effector link, the first link in the group is used. If no link is identified, failure is reported. True is returned if an attach request was successfully sent to the move_group node. This does not verify that the attach request also was successfully applied by move_group."""
         return self._g.attach_object(object_name, link_name, touch_links)
 
     def detach_object(self, name=""):
-        """ Given the name of a link, detach the object(s) from that link. If no such link exists, the name is interpreted as an object name. If there is no name specified, an attempt is made to detach all objects attached to any link in the group."""
+        """Given the name of a link, detach the object(s) from that link. If no such link exists, the name is interpreted as an object name. If there is no name specified, an attempt is made to detach all objects attached to any link in the group."""
         return self._g.detach_object(name)
 
     def pick(self, object_name, grasp=[], plan_only=False):
@@ -750,7 +750,7 @@ def place(self, object_name, location=None, plan_only=False):
         return result
 
     def set_support_surface_name(self, value):
-        """ Set the support surface name for a place operation """
+        """Set the support surface name for a place operation"""
         self._g.set_support_surface_name(value)
 
     def retime_trajectory(
@@ -775,14 +775,14 @@ def retime_trajectory(
         return traj_out
 
     def get_jacobian_matrix(self, joint_values, reference_point=None):
-        """ Get the jacobian matrix of the group as a list"""
+        """Get the jacobian matrix of the group as a list"""
         return self._g.get_jacobian_matrix(
             joint_values,
             [0.0, 0.0, 0.0] if reference_point is None else reference_point,
         )
 
     def enforce_bounds(self, robot_state_msg):
-        """ Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds() """
+        """Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds()"""
         s = RobotState()
         c_str = self._g.enforce_bounds(conversions.msg_to_string(robot_state_msg))
         conversions.msg_from_string(s, c_str)
diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py
index efa16d08ac..acd442e77e 100644
--- a/moveit_commander/src/moveit_commander/planning_scene_interface.py
+++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py
@@ -65,7 +65,7 @@ class PlanningSceneInterface(object):
     """
 
     def __init__(self, ns="", synchronous=False, service_timeout=5.0):
-        """ Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics. """
+        """Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics."""
         self._psi = _moveit_planning_scene_interface.PlanningSceneInterface(ns)
 
         self._pub_co = rospy.Publisher(
@@ -94,31 +94,31 @@ def __submit(self, collision_object, attach=False):
                 self._pub_co.publish(collision_object)
 
     def add_object(self, collision_object):
-        """ Add an object to the planning scene """
+        """Add an object to the planning scene"""
         self.__submit(collision_object, attach=False)
 
     def add_sphere(self, name, pose, radius=1):
-        """ Add a sphere to the planning scene """
+        """Add a sphere to the planning scene"""
         co = self.__make_sphere(name, pose, radius)
         self.__submit(co, attach=False)
 
     def add_cylinder(self, name, pose, height, radius):
-        """ Add a cylinder to the planning scene """
+        """Add a cylinder to the planning scene"""
         co = self.__make_cylinder(name, pose, height, radius)
         self.__submit(co, attach=False)
 
     def add_mesh(self, name, pose, filename, size=(1, 1, 1)):
-        """ Add a mesh to the planning scene """
+        """Add a mesh to the planning scene"""
         co = self.__make_mesh(name, pose, filename, size)
         self.__submit(co, attach=False)
 
     def add_box(self, name, pose, size=(1, 1, 1)):
-        """ Add a box to the planning scene """
+        """Add a box to the planning scene"""
         co = self.__make_box(name, pose, size)
         self.__submit(co, attach=False)
 
     def add_plane(self, name, pose, normal=(0, 0, 1), offset=0):
-        """ Add a plane to the planning scene """
+        """Add a plane to the planning scene"""
         co = CollisionObject()
         co.operation = CollisionObject.ADD
         co.id = name
@@ -131,7 +131,7 @@ def add_plane(self, name, pose, normal=(0, 0, 1), offset=0):
         self.__submit(co, attach=False)
 
     def attach_object(self, attached_collision_object):
-        """ Attach an object in the planning scene """
+        """Attach an object in the planning scene"""
         self.__submit(attached_collision_object, attach=True)
 
     def attach_mesh(
@@ -162,7 +162,7 @@ def attach_box(self, link, name, pose=None, size=(1, 1, 1), touch_links=[]):
         self.__submit(aco, attach=True)
 
     def clear(self):
-        """ Remove all objects from the planning scene """
+        """Remove all objects from the planning scene"""
         self.remove_attached_object()
         self.remove_world_object()
 
diff --git a/moveit_commander/src/moveit_commander/robot.py b/moveit_commander/src/moveit_commander/robot.py
index 55f6e7b07c..e86e44d723 100644
--- a/moveit_commander/src/moveit_commander/robot.py
+++ b/moveit_commander/src/moveit_commander/robot.py
@@ -192,7 +192,7 @@ def get_robot_markers(self, *args):
         return mrkr
 
     def get_root_link(self):
-        """Get the name of the root link of the robot model """
+        """Get the name of the root link of the robot model"""
         return self._r.get_robot_root_link()
 
     def get_active_joint_names(self, group=None):
@@ -241,7 +241,7 @@ def get_group_names(self):
         return self._r.get_group_names()
 
     def get_current_state(self):
-        """ Get a RobotState message describing the current state of the robot"""
+        """Get a RobotState message describing the current state of the robot"""
         s = RobotState()
         s.deserialize(self._r.get_current_state())
         return s
diff --git a/moveit_ros/planning_interface/test/serialize_msg.py b/moveit_ros/planning_interface/test/serialize_msg.py
index 212c0905eb..469e68d7f9 100644
--- a/moveit_ros/planning_interface/test/serialize_msg.py
+++ b/moveit_ros/planning_interface/test/serialize_msg.py
@@ -102,14 +102,14 @@ def test_rejectIntTuple(self):
 
     def test_rejectUnicode(self):
         with self.assertRaisesRegexp(Exception, "Python argument types in"):
-            self.helper.compareEmbeddedZeros(u"kdasd")
+            self.helper.compareEmbeddedZeros(u"kdasd")  # fmt: skip
 
     @unittest.skipIf(py_version_maj == 2, "does not trigger with python 2.7")
     def test_rejectUnicodeTuple(self):
         with self.assertRaisesRegexp(
             RuntimeError, "Underlying python object is not a Bytes/String instance"
         ):
-            self.helper.compareVectorTuple((u"kdasd",))
+            self.helper.compareVectorTuple((u"kdasd",))  # fmt: skip
 
 
 if __name__ == "__main__":

From 9e30bdc77d0a9386dfdc50766e782de8be155010 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Denis=20=C5=A0togl?= <denis@stogl.de>
Date: Tue, 29 Mar 2022 21:52:59 +0200
Subject: [PATCH 089/114] Set controller status before it is checked on
 trajectory execution (#1014)

---
 .../src/trajectory_execution_manager.cpp        | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
index ab2b0dc093..6443a17839 100644
--- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
+++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
@@ -569,6 +569,18 @@ void TrajectoryExecutionManager::reloadControllerInformation()
       known_controllers_[ci.name_] = ci;
     }
 
+    names.clear();
+    controller_manager_->getActiveControllers(names);
+    for (const auto& active_name : names)
+    {
+      auto found_it = std::find_if(known_controllers_.begin(), known_controllers_.end(),
+                                   [&](const auto& known_controller) { return known_controller.first == active_name; });
+      if (found_it != known_controllers_.end())
+      {
+        found_it->second.state_.active_ = true;
+      }
+    }
+
     for (std::map<std::string, ControllerInformation>::iterator it = known_controllers_.begin();
          it != known_controllers_.end(); ++it)
       for (std::map<std::string, ControllerInformation>::iterator jt = known_controllers_.begin();
@@ -1577,6 +1589,7 @@ bool TrajectoryExecutionManager::executePart(std::size_t part_index)
   }
   else
   {
+    RCLCPP_ERROR(LOGGER, "Active status of required controllers can not be assured.");
     last_execution_status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
     return false;
   }
@@ -1804,8 +1817,12 @@ bool TrajectoryExecutionManager::ensureActiveControllers(const std::vector<std::
     std::set<std::string> originally_active;
     for (std::map<std::string, ControllerInformation>::const_iterator it = known_controllers_.begin();
          it != known_controllers_.end(); ++it)
+    {
       if (it->second.state_.active_)
+      {
         originally_active.insert(it->first);
+      }
+    }
     return std::includes(originally_active.begin(), originally_active.end(), controllers.begin(), controllers.end());
   }
 }

From dec4d8f0f0b09b1f405a4df4c497a67f429b182b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= <gael.ecorchard@cvut.cz>
Date: Tue, 29 Mar 2022 23:11:12 +0200
Subject: [PATCH 090/114] Return `ExecutionStatus` from `MoveItCpp::execute()`
 (#1147)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Return an `ExecutionStatus` from `MoveItCpp::execute()`, which is
convertible to a bool in the caller code.
This change is forward compatible.

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>
---
 .../include/moveit/moveit_cpp/moveit_cpp.h          |  6 ++++--
 moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp   | 13 ++++++++-----
 2 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h
index bff2bed980..68db705ba2 100644
--- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h
+++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h
@@ -40,6 +40,7 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
+#include <moveit/controller_manager/controller_manager.h>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <moveit/planning_pipeline/planning_pipeline.h>
 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
@@ -167,8 +168,9 @@ class MoveItCpp
 
   /** \brief Execute a trajectory on the planning group specified by group_name using the trajectory execution manager.
    * If blocking is set to false, the execution is run in background and the function returns immediately. */
-  bool execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
-               bool blocking = true);
+  moveit_controller_manager::ExecutionStatus execute(const std::string& group_name,
+                                                     const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
+                                                     bool blocking = true);
 
 private:
   //  Core properties and instances
diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp
index 609eddcaee..7780373d8c 100644
--- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp
+++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp
@@ -35,6 +35,8 @@
 /* Author: Henning Kayser */
 
 #include <stdexcept>
+
+#include <moveit/controller_manager/controller_manager.h>
 #include <moveit/moveit_cpp/moveit_cpp.h>
 #if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
@@ -247,20 +249,21 @@ trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveItCpp::getTrajec
   return trajectory_execution_manager_;
 }
 
-bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
-                        bool blocking)
+moveit_controller_manager::ExecutionStatus
+MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
+                   bool blocking)
 {
   if (!robot_trajectory)
   {
     RCLCPP_ERROR(LOGGER, "Robot trajectory is undefined");
-    return false;
+    return moveit_controller_manager::ExecutionStatus::ABORTED;
   }
 
   // Check if there are controllers that can handle the execution
   if (!trajectory_execution_manager_->ensureActiveControllersForGroup(group_name))
   {
     RCLCPP_ERROR(LOGGER, "Execution failed! No active controllers configured for group '%s'", group_name.c_str());
-    return false;
+    return moveit_controller_manager::ExecutionStatus::ABORTED;
   }
 
   // Execute trajectory
@@ -273,7 +276,7 @@ bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::R
     return trajectory_execution_manager_->waitForExecution();
   }
   trajectory_execution_manager_->pushAndExecute(robot_trajectory_msg);
-  return true;
+  return moveit_controller_manager::ExecutionStatus::RUNNING;
 }
 
 const std::shared_ptr<tf2_ros::Buffer>& MoveItCpp::getTFBuffer() const

From 9bf64de4f1a982861a1077571f6264ae3559f332 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= <gael.ecorchard@cvut.cz>
Date: Thu, 31 Mar 2022 02:55:49 +0200
Subject: [PATCH 091/114] [ompl] Small code refactor (#1138)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>
---
 .../ompl_interface/src/ompl_interface.cpp     | 27 +++++++------------
 .../src/ompl_planner_manager.cpp              |  2 +-
 2 files changed, 11 insertions(+), 18 deletions(-)

diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
index 9677fae93a..f18477e3c3 100644
--- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
+++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
@@ -182,29 +182,22 @@ void OMPLInterface::loadPlannerConfigurations()
 
     // get parameters specific for the robot planning group
     std::map<std::string, std::string> specific_group_params;
-    for (const auto& k : KNOWN_GROUP_PARAMS)
+    for (const auto& [name, type] : KNOWN_GROUP_PARAMS)
     {
       std::string param_name{ group_name_param };
       param_name += ".";
-      param_name += k.first;
+      param_name += name;
       if (node_->has_parameter(param_name))
       {
         const rclcpp::Parameter parameter = node_->get_parameter(param_name);
-        if (parameter.get_type() != k.second)
+        if (parameter.get_type() != type)
         {
-          RCLCPP_ERROR_STREAM(LOGGER, "Invalid type for parameter '" << k.first << "' expected ["
-                                                                     << rclcpp::to_string(k.second) << "] got ["
+          RCLCPP_ERROR_STREAM(LOGGER, "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)
-          specific_group_params[k.first] = parameter.as_string();
-        else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
-          specific_group_params[k.first] = moveit::core::toString(parameter.as_double());
-        else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
-          specific_group_params[k.first] = std::to_string(parameter.as_int());
-        else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL)
-          specific_group_params[k.first] = std::to_string(parameter.as_bool());
+        specific_group_params[name] = parameter.value_to_string();
       }
     }
 
@@ -244,13 +237,13 @@ void OMPLInterface::loadPlannerConfigurations()
     }
   }
 
-  for (const std::pair<const std::string, planning_interface::PlannerConfigurationSettings>& config : pconfig)
+  for (const auto& [name, config_settings] : pconfig)
   {
-    RCLCPP_DEBUG(LOGGER, "Parameters for configuration '%s'", config.first.c_str());
+    RCLCPP_DEBUG(LOGGER, "Parameters for configuration '%s'", name.c_str());
 
-    for (const std::pair<const std::string, std::string>& parameters : config.second.config)
+    for (const auto& [param_name, param_value] : config_settings.config)
     {
-      RCLCPP_DEBUG(LOGGER, " - %s = %s", parameters.first.c_str(), parameters.second.c_str());
+      RCLCPP_DEBUG_STREAM(LOGGER, " - " << param_name << " = " << param_value);
     }
   }
 
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 00b45a240c..ba06cf3d43 100644
--- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp
+++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp
@@ -86,7 +86,7 @@ class OMPLPlannerManager : public planning_interface::PlannerManager
                   const std::string& parameter_namespace) override
   {
     ompl_interface_ = std::make_unique<OMPLInterface>(model, node, parameter_namespace);
-    config_settings_ = ompl_interface_->getPlannerConfigurations();
+    setPlannerConfigurations(ompl_interface_->getPlannerConfigurations());
     return true;
   }
 

From 9ae8aada1029c1f21ddcd049236f2305cef0f52d Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatan@picknik.ai>
Date: Thu, 31 Mar 2022 18:07:23 +0300
Subject: [PATCH 092/114] Enable rolling / jammy CI (again) (#1134)

* Use ros2_control binaries
* Use output screen instead of explicitly stating stderr
---
 .github/workflows/ci.yaml                            | 12 ++++--------
 moveit2.repos                                        |  4 ----
 .../prbt_moveit_config/launch/demo.launch.py         |  5 +----
 .../test/launch/hybrid_planning_common.py            |  5 +----
 .../launch/pose_tracking_example.launch.py           |  5 +----
 .../moveit_servo/launch/servo_example.launch.py      | 10 ++--------
 .../test/launch/servo_launch_test_common.py          |  5 +----
 .../test/launch/test_servo_pose_tracking.test.py     |  5 +----
 .../test/launch/move_group_launch_test_common.py     |  5 +----
 9 files changed, 12 insertions(+), 44 deletions(-)

diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
index 7bcea4da26..6f735f2826 100644
--- a/.github/workflows/ci.yaml
+++ b/.github/workflows/ci.yaml
@@ -16,15 +16,11 @@ jobs:
       fail-fast: false
       matrix:
         env:
-          # - IMAGE: rolling-ci
-          #   ROS_DISTRO: rolling
-          # - IMAGE: rolling-ci-testing
-          #   ROS_DISTRO: rolling
-          - IMAGE: galactic-ci
+          - IMAGE: rolling-ci
             CCOV: true
-            ROS_DISTRO: galactic
-          - IMAGE: galactic-ci-testing
-            ROS_DISTRO: galactic
+            ROS_DISTRO: rolling
+          - IMAGE: rolling-ci-testing
+            ROS_DISTRO: rolling
             IKFAST_TEST: true
             CLANG_TIDY: pedantic
     env:
diff --git a/moveit2.repos b/moveit2.repos
index ecd6a89e75..fb926989a2 100644
--- a/moveit2.repos
+++ b/moveit2.repos
@@ -11,10 +11,6 @@ repositories:
     type: git
     url: https://github.com/ros-planning/moveit_resources
     version: ros2
-  ros2_control:
-    type: git
-    url: https://github.com/ros-controls/ros2_control.git
-    version: 2.1.0
   ompl:
     type: git
     url: https://github.com/ompl/ompl.git
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 678fe0dd4a..824dd83460 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
@@ -187,10 +187,7 @@ def generate_launch_description():
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[robot_description, ros2_controllers_path],
-        output={
-            "stdout": "screen",
-            "stderr": "screen",
-        },
+        output="screen",
     )
 
     # Load controllers
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 be58f6038c..9322498a69 100644
--- a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py
+++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py
@@ -187,10 +187,7 @@ def generate_common_hybrid_launch_description():
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[robot_description, ros2_controllers_path],
-        output={
-            "stdout": "screen",
-            "stderr": "screen",
-        },
+        output="screen",
     )
 
     # Load controllers
diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py
index c6908b3397..69fed7eb75 100644
--- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py
+++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py
@@ -75,10 +75,7 @@ def generate_launch_description():
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[moveit_config.robot_description, ros2_controllers_path],
-        output={
-            "stdout": "screen",
-            "stderr": "screen",
-        },
+        output="screen",
     )
 
     # Load controllers
diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/servo_example.launch.py
index 6cebcab884..4bccca9ec1 100644
--- a/moveit_ros/moveit_servo/launch/servo_example.launch.py
+++ b/moveit_ros/moveit_servo/launch/servo_example.launch.py
@@ -76,10 +76,7 @@ def generate_launch_description():
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[robot_description, ros2_controllers_path],
-        output={
-            "stdout": "screen",
-            "stderr": "screen",
-        },
+        output="screen",
     )
 
     # Load controllers
@@ -147,10 +144,7 @@ def generate_launch_description():
             robot_description,
             robot_description_semantic,
         ],
-        output={
-            "stdout": "screen",
-            "stderr": "screen",
-        },
+        output="screen",
     )
 
     return LaunchDescription(
diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py
index 0fba099b66..ee5930d6fb 100644
--- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py
+++ b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py
@@ -53,10 +53,7 @@ def generate_servo_test_description(
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[moveit_config.robot_description, ros2_controllers_path],
-        output={
-            "stdout": "screen",
-            "stderr": "screen",
-        },
+        output="screen",
     )
 
     # Load controllers
diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py
index 8b689aa48c..cf151faae3 100644
--- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py
+++ b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py
@@ -40,10 +40,7 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType):
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[moveit_config.robot_description, ros2_controllers_path],
-        output={
-            "stdout": "screen",
-            "stderr": "screen",
-        },
+        output="screen",
     )
 
     # Load controllers
diff --git a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py
index 712022babf..860eeff78d 100644
--- a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py
+++ b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py
@@ -61,10 +61,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[moveit_config.robot_description, ros2_controllers_path],
-        output={
-            "stdout": "screen",
-            "stderr": "screen",
-        },
+        output="screen",
     )
 
     # Load controllers

From 867aa498ae3db9eb3dc6f922faac74e483d527fd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= <gael.ecorchard@cvut.cz>
Date: Thu, 31 Mar 2022 17:20:42 +0200
Subject: [PATCH 093/114] Use a steady clock for timeout for IK (#795)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>
---
 .../kdl_kinematics_plugin/kdl_kinematics_plugin.h      |  2 ++
 .../src/kdl_kinematics_plugin.cpp                      | 10 ++++++----
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h
index 74760149b3..5757655b7e 100644
--- a/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h
+++ b/moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h
@@ -159,6 +159,8 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase
   /// clip q_delta such that joint limits will not be violated
   void clipToJointLimits(const KDL::JntArray& q, KDL::JntArray& q_delta, Eigen::ArrayXd& weighting) const;
 
+  static rclcpp::Clock steady_clock_;
+
   bool initialized_;  ///< Internal variable that indicates whether solver is configured and ready
 
   unsigned int dimension_;                             ///< Dimension of the group
diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp
index 0cef9ec308..ca7cd366c7 100644
--- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp
+++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp
@@ -53,6 +53,8 @@ namespace kdl_kinematics_plugin
 {
 static rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_kdl_kinematics_plugin.kdl_kinematics_plugin");
 
+rclcpp::Clock KDLKinematicsPlugin::steady_clock_{ RCL_STEADY_TIME };
+
 KDLKinematicsPlugin::KDLKinematicsPlugin() : initialized_(false)
 {
 }
@@ -272,7 +274,7 @@ bool KDLKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const
 
 bool KDLKinematicsPlugin::timedOut(const rclcpp::Time& start_time, double duration) const
 {
-  return ((node_->now() - start_time).seconds() >= duration);
+  return ((steady_clock_.now() - start_time).seconds() >= duration);
 }
 
 bool KDLKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose,
@@ -327,7 +329,7 @@ bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_po
                                            moveit_msgs::msg::MoveItErrorCodes& error_code,
                                            const kinematics::KinematicsQueryOptions& options) const
 {
-  rclcpp::Time start_time = node_->now();
+  const rclcpp::Time start_time = steady_clock_.now();
   if (!initialized_)
   {
     RCLCPP_ERROR(LOGGER, "kinematics solver not initialized");
@@ -413,13 +415,13 @@ 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 " << (node_->now() - start_time).seconds() << " < " << timeout
+      RCLCPP_DEBUG_STREAM(LOGGER, "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 " << (node_->now() - start_time).seconds() << " > " << timeout
+  RCLCPP_DEBUG_STREAM(LOGGER, "IK timed out after " << (steady_clock_.now() - start_time).seconds() << " > " << timeout
                                                     << "s and " << attempt << " attempts");
   error_code.val = error_code.TIMED_OUT;
   return false;

From 0ce1c2902f956999629e66fd6ee84d7f2ad36ed4 Mon Sep 17 00:00:00 2001
From: Abishalini <abi.gpuram@gmail.com>
Date: Tue, 5 Apr 2022 10:29:47 -0600
Subject: [PATCH 094/114] Suppress clang tidy warning performance-no-int-to-ptr

---
 moveit_core/robot_state/src/robot_state.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
index bac7833cee..8785dd1495 100644
--- a/moveit_core/robot_state/src/robot_state.cpp
+++ b/moveit_core/robot_state/src/robot_state.cpp
@@ -109,6 +109,7 @@ void RobotState::allocMemory()
 
   // make the memory for transforms align at EIGEN_MAX_ALIGN_BYTES
   // https://eigen.tuxfamily.org/dox/classEigen_1_1aligned__allocator.html
+  // NOLINTNEXTLINE(performance-no-int-to-ptr)
   variable_joint_transforms_ = reinterpret_cast<Eigen::Isometry3d*>(((uintptr_t)memory_ + extra_alignment_bytes) &
                                                                     ~(uintptr_t)extra_alignment_bytes);
   global_link_transforms_ = variable_joint_transforms_ + robot_model_->getJointModelCount();

From 49d21bad1fb1788ad2cc78ea30c5e08e28c3befd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= <gael.ecorchard@cvut.cz>
Date: Wed, 6 Apr 2022 18:54:11 +0200
Subject: [PATCH 095/114] [moveit_ros_benchmarks] Add missing moveit_core
 dependency (#1157)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Gaël Écorchard <gael.ecorchard@cvut.cz>
---
 moveit_ros/benchmarks/CMakeLists.txt | 2 ++
 moveit_ros/benchmarks/package.xml    | 1 +
 2 files changed, 3 insertions(+)

diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt
index 4c241f0f38..bd43bca148 100644
--- a/moveit_ros/benchmarks/CMakeLists.txt
+++ b/moveit_ros/benchmarks/CMakeLists.txt
@@ -10,6 +10,7 @@ moveit_package()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(tf2_eigen REQUIRED)
+find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_ros_warehouse REQUIRED)
 find_package(pluginlib REQUIRED)
@@ -23,6 +24,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp
   Boost
   tf2_eigen
+  moveit_core
   moveit_ros_planning
   moveit_ros_warehouse
   pluginlib
diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml
index cffcc2b7fa..54d20d2555 100644
--- a/moveit_ros/benchmarks/package.xml
+++ b/moveit_ros/benchmarks/package.xml
@@ -18,6 +18,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>moveit_common</build_depend>
+  <build_depend>moveit_core</build_depend>
 
   <build_depend>libboost-dev</build_depend>
   <build_depend>libboost-date-time-dev</build_depend>

From 99939d3cd8d13835ddec9ad011671e5997c1a6c6 Mon Sep 17 00:00:00 2001
From: Tobias Fischer <info@tobiasfischer.info>
Date: Thu, 7 Apr 2022 02:57:33 +1000
Subject: [PATCH 096/114] Fix prbt_ikfast win compilation (#1161)

---
 .../CMakeLists.txt                              | 17 +++++++++++------
 1 file changed, 11 insertions(+), 6 deletions(-)

diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt
index ee7e2d0bd5..3349523adc 100644
--- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt
+++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt
@@ -4,10 +4,12 @@ project(moveit_resources_prbt_ikfast_manipulator_plugin)
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_EXTENSIONS OFF)
 
-add_compile_options(-Wall)
-add_compile_options(-Wextra)
-add_compile_options(-Wno-unused-parameter)
-add_compile_options(-Wno-unused-variable)
+if(NOT WIN32)
+  add_compile_options(-Wall)
+  add_compile_options(-Wextra)
+  add_compile_options(-Wno-unused-parameter)
+  add_compile_options(-Wno-unused-variable)
+endif()
 
 # enable aligned new in gcc7+
 if(CMAKE_COMPILER_IS_GNUCXX)
@@ -29,8 +31,11 @@ include_directories(include)
 
 add_library(prbt_manipulator_moveit_ikfast_plugin SHARED
   src/prbt_manipulator_ikfast_moveit_plugin.cpp)
-# suppress warnings about unused variables in OpenRave's solver code
-target_compile_options(prbt_manipulator_moveit_ikfast_plugin PRIVATE -Wno-unused-variable)
+
+if(NOT WIN32)
+  # suppress warnings about unused variables in OpenRave's solver code
+  target_compile_options(prbt_manipulator_moveit_ikfast_plugin PRIVATE -Wno-unused-variable)
+endif()
 ament_target_dependencies(prbt_manipulator_moveit_ikfast_plugin
   moveit_core
   pluginlib

From 96016434462e0074694dceeec28bf2e58548d1d7 Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatan@picknik.ai>
Date: Thu, 7 Apr 2022 18:10:24 +0300
Subject: [PATCH 097/114] Revert OMPL parameter loading

---
 .../ompl/ompl_interface/src/ompl_interface.cpp           | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
index f18477e3c3..b45b744ef0 100644
--- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
+++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
@@ -197,7 +197,14 @@ void OMPLInterface::loadPlannerConfigurations()
                                                                      << rclcpp::to_string(parameter.get_type()) << "]");
           continue;
         }
-        specific_group_params[name] = parameter.value_to_string();
+        if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING)
+          specific_group_params[name] = parameter.as_string();
+        else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
+          specific_group_params[name] = moveit::core::toString(parameter.as_double());
+        else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
+          specific_group_params[name] = std::to_string(parameter.as_int());
+        else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL)
+          specific_group_params[name] = std::to_string(parameter.as_bool());
       }
     }
 

From 39639b7762524e63e36f1ad693ab1986e8c151d8 Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatan@picknik.ai>
Date: Thu, 7 Apr 2022 18:10:53 +0300
Subject: [PATCH 098/114] Comment failing rdf integration test

---
 moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp
index 8e665c1cdb..e9bc3414ea 100644
--- a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp
+++ b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp
@@ -66,6 +66,9 @@ TEST(RDFIntegration, topic_based)
   EXPECT_EQ("gonzo", loader.getSRDF()->getName());
 }
 
+// Failing in Jammy disabling temporarily (TODO (vatanaksoytezer): Fix this and enable again)
+// See https://github.com/ros-planning/moveit2/issues/1156
+/*
 TEST(RDFIntegration, executor)
 {
   // RDFLoader should successfully load URDF and SRDF strings from a ROS topic when the node that is
@@ -89,6 +92,7 @@ TEST(RDFIntegration, executor)
   ASSERT_NE(nullptr, loader.getSRDF());
   EXPECT_EQ("gonzo", loader.getSRDF()->getName());
 }
+*/
 
 TEST(RDFIntegration, xacro_test)
 {

From c95084ecfa6d40b5cbe500994003b3cc0746e5fd Mon Sep 17 00:00:00 2001
From: Vatan Aksoy Tezer <vatan@picknik.ai>
Date: Thu, 7 Apr 2022 18:11:39 +0300
Subject: [PATCH 099/114] Rename panda controllers

---
 .../moveit_servo/launch/pose_tracking_example.launch.py       | 2 +-
 moveit_ros/moveit_servo/launch/servo_example.launch.py        | 2 +-
 .../moveit_servo/test/launch/servo_launch_test_common.py      | 2 +-
 .../moveit_servo/test/launch/test_servo_pose_tracking.test.py | 2 +-
 .../test/launch/move_group_launch_test_common.py              | 4 ++--
 5 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py
index 69fed7eb75..29143be647 100644
--- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py
+++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py
@@ -69,7 +69,7 @@ def generate_launch_description():
     ros2_controllers_path = os.path.join(
         get_package_share_directory("moveit_resources_panda_moveit_config"),
         "config",
-        "panda_ros2_controllers.yaml",
+        "ros2_controllers.yaml",
     )
     ros2_control_node = Node(
         package="controller_manager",
diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/servo_example.launch.py
index 4bccca9ec1..1e699cf2a6 100644
--- a/moveit_ros/moveit_servo/launch/servo_example.launch.py
+++ b/moveit_ros/moveit_servo/launch/servo_example.launch.py
@@ -70,7 +70,7 @@ def generate_launch_description():
     ros2_controllers_path = os.path.join(
         get_package_share_directory("moveit_resources_panda_moveit_config"),
         "config",
-        "panda_ros2_controllers.yaml",
+        "ros2_controllers.yaml",
     )
     ros2_control_node = Node(
         package="controller_manager",
diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py
index ee5930d6fb..493c208140 100644
--- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py
+++ b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py
@@ -47,7 +47,7 @@ def generate_servo_test_description(
     ros2_controllers_path = os.path.join(
         get_package_share_directory("moveit_resources_panda_moveit_config"),
         "config",
-        "panda_ros2_controllers.yaml",
+        "ros2_controllers.yaml",
     )
     ros2_control_node = Node(
         package="controller_manager",
diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py
index cf151faae3..ce9e48bf42 100644
--- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py
+++ b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py
@@ -34,7 +34,7 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType):
     ros2_controllers_path = os.path.join(
         get_package_share_directory("moveit_resources_panda_moveit_config"),
         "config",
-        "panda_ros2_controllers.yaml",
+        "ros2_controllers.yaml",
     )
     ros2_control_node = Node(
         package="controller_manager",
diff --git a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py
index 860eeff78d..b7fbf86cda 100644
--- a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py
+++ b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py
@@ -14,7 +14,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp
     moveit_config = (
         MoveItConfigsBuilder("moveit_resources_panda")
         .robot_description(file_path="config/panda.urdf.xacro")
-        .trajectory_execution(file_path="config/panda_gripper_controllers.yaml")
+        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
         .to_moveit_configs()
     )
 
@@ -55,7 +55,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp
     ros2_controllers_path = os.path.join(
         get_package_share_directory("moveit_resources_panda_moveit_config"),
         "config",
-        "panda_ros2_controllers.yaml",
+        "ros2_controllers.yaml",
     )
     ros2_control_node = Node(
         package="controller_manager",

From f8da9512675d878c095f1421927dd6b2c2d3a291 Mon Sep 17 00:00:00 2001
From: JafarAbdi <cafer.abdi@gmail.com>
Date: Thu, 7 Apr 2022 16:59:34 +0000
Subject: [PATCH 100/114] Fix failing test

---
 .../rdf_loader/test/test_rdf_integration.cpp       | 14 +++++---------
 1 file changed, 5 insertions(+), 9 deletions(-)

diff --git a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp
index e9bc3414ea..9d813402b5 100644
--- a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp
+++ b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp
@@ -66,9 +66,6 @@ TEST(RDFIntegration, topic_based)
   EXPECT_EQ("gonzo", loader.getSRDF()->getName());
 }
 
-// Failing in Jammy disabling temporarily (TODO (vatanaksoytezer): Fix this and enable again)
-// See https://github.com/ros-planning/moveit2/issues/1156
-/*
 TEST(RDFIntegration, executor)
 {
   // RDFLoader should successfully load URDF and SRDF strings from a ROS topic when the node that is
@@ -77,11 +74,9 @@ TEST(RDFIntegration, executor)
   rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("executor");
 
   // Create a thread to spin an Executor.
-  std::thread([node]() {
-    rclcpp::executors::SingleThreadedExecutor executor;
-    executor.add_node(node);
-    executor.spin();
-  }).detach();
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+  std::thread spinning_thread([&executor] { executor.spin(); });
 
   // WHEN the RDFLoader is created
   rdf_loader::RDFLoader loader(node, "topic_description");
@@ -91,8 +86,9 @@ TEST(RDFIntegration, executor)
   EXPECT_EQ("gonzo", loader.getURDF()->name_);
   ASSERT_NE(nullptr, loader.getSRDF());
   EXPECT_EQ("gonzo", loader.getSRDF()->getName());
+  executor.cancel();
+  spinning_thread.join();
 }
-*/
 
 TEST(RDFIntegration, xacro_test)
 {

From a0c4d34ddb4698589202784810c3379f83107c27 Mon Sep 17 00:00:00 2001
From: "David V. Lu!!" <davidvlu@gmail.com>
Date: Thu, 7 Apr 2022 17:29:00 -0400
Subject: [PATCH 101/114] Remove moveit_msgs and ompl from moveit2.repos
 (#1166)

---
 moveit2.repos | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/moveit2.repos b/moveit2.repos
index fab71c7d54..5211f196ce 100644
--- a/moveit2.repos
+++ b/moveit2.repos
@@ -3,18 +3,10 @@ repositories:
     type: git
     url: https://github.com/ros-planning/geometric_shapes
     version: ros2
-  moveit_msgs:
-    type: git
-    url: https://github.com/ros-planning/moveit_msgs
-    version: 2.2.0
   moveit_resources:
     type: git
     url: https://github.com/ros-planning/moveit_resources
     version: ros2
-  ompl:
-    type: git
-    url: https://github.com/ompl/ompl.git
-    version: main
   srdfdom:
     type: git
     url: https://github.com/ros-planning/srdfdom.git

From 3fe5b8221d1739ab4a54e58d6b85eeb3b2d85c27 Mon Sep 17 00:00:00 2001
From: Burak Payzun <brkpyzn@gmail.com>
Date: Tue, 12 Apr 2022 03:55:53 +0300
Subject: [PATCH 102/114] Replace num_dof and idx variables with JointGroup API
 (#1152)

---
 .../ruckig_traj_smoothing.h                   | 25 +++++++------
 .../src/iterative_spline_parameterization.cpp | 17 ++++++---
 .../src/ruckig_traj_smoothing.cpp             | 37 ++++++++++++-------
 3 files changed, 48 insertions(+), 31 deletions(-)

diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
index cc22f4d0d7..bb90df1f2c 100644
--- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
+++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
@@ -54,30 +54,32 @@ class RuckigSmoothing
    * \brief Feed previous output back as input for next iteration. Get next target state from the next waypoint.
    * \param ruckig_output   The previous output from Ruckig
    * \param next_waypoint   The nominal, desired state at the next waypoint
-   * \param num_dof         Number  of actuated joints
-   * \param idx             MoveIt list of joint group indices
+   * \param joint_group     The MoveIt JointModelGroup of interest
    * \param ruckig_input    Output. The Rucking parameters for the next iteration
    */
   static void getNextRuckigInput(const ruckig::OutputParameter<0>& ruckig_output,
-                                 const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof,
-                                 const std::vector<int>& idx, ruckig::InputParameter<0>& ruckig_input);
+                                 const moveit::core::RobotStatePtr& next_waypoint,
+                                 const moveit::core::JointModelGroup* joint_group,
+                                 ruckig::InputParameter<0>& ruckig_input);
 
   /**
    * \brief Check for lagging motion of any joint at a waypoint.
-   * \param num_dof       Number  of actuated joints
+   * \param joint_group     The MoveIt JointModelGroup of interest
    * \param ruckig_input  Input parameters to Ruckig
    * \param ruckig_output Output parameters from Ruckig
    * \return              true if lagging motion is detected on any joint
    */
-  static bool checkForLaggingMotion(const size_t num_dof, const ruckig::InputParameter<0>& ruckig_input,
+  static bool checkForLaggingMotion(const moveit::core::JointModelGroup* joint_group,
+                                    const ruckig::InputParameter<0>& ruckig_input,
                                     const ruckig::OutputParameter<0>& ruckig_output);
 
   /**
    * \brief Return L2-norm of velocity, taking all joints into account.
    * \param ruckig_input  Input parameters to Ruckig
-   * \param num_dof       Number  of actuated joints
+   * \param joint_group   The MoveIt JointModelGroup of interest
    */
-  static double getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, size_t num_dof);
+  static double getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input,
+                                           const moveit::core::JointModelGroup* joint_group);
 
   /**
    * \brief Check if the joint positions of two waypoints are very similar.
@@ -94,11 +96,10 @@ class RuckigSmoothing
    * \param rucking_input   Input parameters to Ruckig. Initialized here.
    * \param ruckig_output   Output from the Ruckig algorithm. Initialized here.
    * \param first_waypoint  The Ruckig input/output parameters are initialized to the values at this waypoint
-   * \param num_dof         Number  of actuated joints
-   * \param joint_idx       MoveIt list of joint group indices
+   * \param joint_group     The MoveIt JointModelGroup of interest
    */
   static void initializeRuckigState(ruckig::InputParameter<0>& ruckig_input, ruckig::OutputParameter<0>& ruckig_output,
-                                    const moveit::core::RobotState& first_waypoint, size_t num_dof,
-                                    const std::vector<int>& joint_idx);
+                                    const moveit::core::RobotState& first_waypoint,
+                                    const moveit::core::JointModelGroup* joint_group);
 };
 }  // namespace trajectory_processing
diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp
index 49c23b931a..1b1646e6a9 100644
--- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp
+++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp
@@ -68,7 +68,7 @@ struct SingleJointTrajectory
   double max_acceleration_;
 };
 
-void globalAdjustment(std::vector<SingleJointTrajectory>& t2, int num_joints, const int num_points,
+void globalAdjustment(std::vector<SingleJointTrajectory>& t2, robot_trajectory::RobotTrajectory& trajectory,
                       std::vector<double>& time_diff);
 
 IterativeSplineParameterization::IterativeSplineParameterization(bool add_points) : add_points_(add_points)
@@ -321,7 +321,7 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT
   }
 
   // Final adjustment forces the trajectory within bounds
-  globalAdjustment(t2, num_joints, num_points, time_diff);
+  globalAdjustment(t2, trajectory, time_diff);
 
   // Convert back to JointTrajectory form
   for (unsigned int i = 1; i < num_points; ++i)
@@ -570,11 +570,16 @@ static double global_adjustment_factor(const int n, double x1[], double x2[], co
 }
 
 // Expands the entire trajectory to fit exactly within bounds
-void globalAdjustment(std::vector<SingleJointTrajectory>& t2, int num_joints, const int num_points,
+void globalAdjustment(std::vector<SingleJointTrajectory>& t2, robot_trajectory::RobotTrajectory& trajectory,
                       std::vector<double>& time_diff)
 {
+  const moveit::core::JointModelGroup* group = trajectory.getGroup();
+
+  const unsigned int num_points = trajectory.getWayPointCount();
+  const unsigned int num_joints = group->getVariableCount();
+
   double gtfactor = 1.0;
-  for (int j = 0; j < num_joints; ++j)
+  for (unsigned int j = 0; j < num_joints; ++j)
   {
     double tfactor;
     tfactor = global_adjustment_factor(num_points, &t2[j].velocities_[0], &t2[j].accelerations_[0], t2[j].max_velocity_,
@@ -584,10 +589,10 @@ void globalAdjustment(std::vector<SingleJointTrajectory>& t2, int num_joints, co
   }
 
   // printf("# Global adjustment: %0.4f%%\n", 100.0 * (gtfactor - 1.0));
-  for (int i = 0; i < num_points - 1; ++i)
+  for (unsigned int i = 0; i < num_points - 1; ++i)
     time_diff[i] *= gtfactor;
 
-  for (int j = 0; j < num_joints; ++j)
+  for (unsigned int j = 0; j < num_joints; ++j)
   {
     fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], &t2[j].accelerations_[0]);
   }
diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
index 6f74994bf3..c485396e75 100644
--- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
@@ -87,7 +87,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
 
   // Initialize the smoother
   const std::vector<int>& idx = group->getVariableIndexList();
-  initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, idx);
+  initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
 
   // Kinematic limits (vel/accel/jerk)
   const std::vector<std::string>& vars = group->getVariableNames();
@@ -127,16 +127,16 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
     {
       moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1);
 
-      getNextRuckigInput(ruckig_output, next_waypoint, num_dof, idx, ruckig_input);
+      getNextRuckigInput(ruckig_output, next_waypoint, group, ruckig_input);
 
       // Run Ruckig
       ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);
 
       // If the requested velocity is too great, a joint can actually "move backward" to give itself more time to
       // accelerate to the target velocity. Iterate and decrease velocities until that behavior is gone.
-      bool backward_motion_detected = checkForLaggingMotion(num_dof, ruckig_input, ruckig_output);
+      bool backward_motion_detected = checkForLaggingMotion(group, ruckig_input, ruckig_output);
 
-      double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof);
+      double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, group);
       while (backward_motion_detected && (velocity_magnitude > MINIMUM_VELOCITY_SEARCH_MAGNITUDE))
       {
         // Skip repeated waypoints with no change in position. Ruckig does not handle this well and there's really no
@@ -156,12 +156,12 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
           ruckig_input.target_acceleration.at(joint) =
               (ruckig_input.target_velocity.at(joint) - ruckig_output.new_velocity.at(joint)) / timestep;
         }
-        velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof);
+        velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, group);
         // Run Ruckig
         ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);
 
         // check for backward motion
-        backward_motion_detected = checkForLaggingMotion(num_dof, ruckig_input, ruckig_output);
+        backward_motion_detected = checkForLaggingMotion(group, ruckig_input, ruckig_output);
       }
 
       // Overwrite pos/vel/accel of the target waypoint
@@ -185,7 +185,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
     {
       // If Ruckig failed, it's likely because the original seed trajectory did not have a long enough duration when
       // jerk is taken into account. Extend the duration and try again.
-      initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, idx);
+      initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
       duration_extension_factor *= DURATION_EXTENSION_FRACTION;
       for (size_t waypoint_idx = 1; waypoint_idx < num_waypoints; ++waypoint_idx)
       {
@@ -212,9 +212,12 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
 
 void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<0>& ruckig_input,
                                             ruckig::OutputParameter<0>& ruckig_output,
-                                            const moveit::core::RobotState& first_waypoint, size_t num_dof,
-                                            const std::vector<int>& idx)
+                                            const moveit::core::RobotState& first_waypoint,
+                                            const moveit::core::JointModelGroup* joint_group)
 {
+  const size_t num_dof = joint_group->getVariableCount();
+  const std::vector<int>& idx = joint_group->getVariableIndexList();
+
   std::vector<double> current_positions_vector(num_dof);
   std::vector<double> current_velocities_vector(num_dof);
   std::vector<double> current_accelerations_vector(num_dof);
@@ -243,8 +246,10 @@ bool RuckigSmoothing::checkForIdenticalWaypoints(const moveit::core::RobotState&
   return (magnitude_position_difference <= IDENTICAL_POSITION_EPSILON);
 }
 
-double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, size_t num_dof)
+double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input,
+                                                   const moveit::core::JointModelGroup* joint_group)
 {
+  const size_t num_dof = joint_group->getVariableCount();
   double vel_magnitude = 0;
   for (size_t joint = 0; joint < num_dof; ++joint)
   {
@@ -253,9 +258,11 @@ double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<
   return sqrt(vel_magnitude);
 }
 
-bool RuckigSmoothing::checkForLaggingMotion(const size_t num_dof, const ruckig::InputParameter<0>& ruckig_input,
+bool RuckigSmoothing::checkForLaggingMotion(const moveit::core::JointModelGroup* joint_group,
+                                            const ruckig::InputParameter<0>& ruckig_input,
                                             const ruckig::OutputParameter<0>& ruckig_output)
 {
+  const size_t num_dof = joint_group->getVariableCount();
   // Check for backward motion of any joint
   for (size_t joint = 0; joint < num_dof; ++joint)
   {
@@ -269,12 +276,16 @@ bool RuckigSmoothing::checkForLaggingMotion(const size_t num_dof, const ruckig::
 }
 
 void RuckigSmoothing::getNextRuckigInput(const ruckig::OutputParameter<0>& ruckig_output,
-                                         const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof,
-                                         const std::vector<int>& idx, ruckig::InputParameter<0>& ruckig_input)
+                                         const moveit::core::RobotStatePtr& next_waypoint,
+                                         const moveit::core::JointModelGroup* joint_group,
+                                         ruckig::InputParameter<0>& ruckig_input)
 {
   // TODO(andyz): https://github.com/ros-planning/moveit2/issues/766
   // ruckig_output.pass_to_input(ruckig_input);
 
+  const size_t num_dof = joint_group->getVariableCount();
+  const std::vector<int>& idx = joint_group->getVariableIndexList();
+
   for (size_t joint = 0; joint < num_dof; ++joint)
   {
     // Feed output from the previous timestep back as input

From 3d87e58ae2886ad70773752895aff470023d9069 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Wed, 16 Mar 2022 15:18:37 -0500
Subject: [PATCH 103/114] Simplify. Use Ruckig as a check if duration needs
 extension.

---
 .../ruckig_traj_smoothing.h                   |  31 +---
 .../src/ruckig_traj_smoothing.cpp             | 166 ++++++------------
 .../test/test_ruckig_traj_smoothing.cpp       |  21 ---
 3 files changed, 52 insertions(+), 166 deletions(-)

diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
index bb90df1f2c..d7103f3a58 100644
--- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
+++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
@@ -31,7 +31,7 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *******************************************************************************/
 
-/* Author: Jack Center, Wyatt Rees, Andy Zelenak */
+/* Author: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng */
 
 #pragma once
 
@@ -62,35 +62,6 @@ class RuckigSmoothing
                                  const moveit::core::JointModelGroup* joint_group,
                                  ruckig::InputParameter<0>& ruckig_input);
 
-  /**
-   * \brief Check for lagging motion of any joint at a waypoint.
-   * \param joint_group     The MoveIt JointModelGroup of interest
-   * \param ruckig_input  Input parameters to Ruckig
-   * \param ruckig_output Output parameters from Ruckig
-   * \return              true if lagging motion is detected on any joint
-   */
-  static bool checkForLaggingMotion(const moveit::core::JointModelGroup* joint_group,
-                                    const ruckig::InputParameter<0>& ruckig_input,
-                                    const ruckig::OutputParameter<0>& ruckig_output);
-
-  /**
-   * \brief Return L2-norm of velocity, taking all joints into account.
-   * \param ruckig_input  Input parameters to Ruckig
-   * \param joint_group   The MoveIt JointModelGroup of interest
-   */
-  static double getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input,
-                                           const moveit::core::JointModelGroup* joint_group);
-
-  /**
-   * \brief Check if the joint positions of two waypoints are very similar.
-   * \param prev_waypoint State at waypoint i-1
-   * \param prev_waypoint State at waypoint i
-   * \joint_group         The MoveIt JointModelGroup of interest
-   */
-  static bool checkForIdenticalWaypoints(const moveit::core::RobotState& prev_waypoint,
-                                         const moveit::core::RobotState& next_waypoint,
-                                         const moveit::core::JointModelGroup* joint_group);
-
   /**
    * \brief Initialize Ruckig position/vel/accel. This initializes ruckig_input and ruckig_output to the same values
    * \param rucking_input   Input parameters to Ruckig. Initialized here.
diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
index c485396e75..da2c2cf419 100644
--- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
@@ -31,7 +31,7 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *******************************************************************************/
 
-/* Author: Jack Center, Wyatt Rees, Andy Zelenak */
+/* Author: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng */
 
 #include <algorithm>
 #include <cmath>
@@ -46,13 +46,11 @@ namespace trajectory_processing
 namespace
 {
 const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing");
-constexpr double DEFAULT_MAX_VELOCITY = 5;           // rad/s
-constexpr double DEFAULT_MAX_ACCELERATION = 10;      // rad/s^2
-constexpr double DEFAULT_MAX_JERK = 20;              // rad/s^3
-constexpr double IDENTICAL_POSITION_EPSILON = 1e-3;  // rad
-constexpr double MAX_DURATION_EXTENSION_FACTOR = 5.0;
+constexpr double DEFAULT_MAX_VELOCITY = 5;       // rad/s
+constexpr double DEFAULT_MAX_ACCELERATION = 10;  // rad/s^2
+constexpr double DEFAULT_MAX_JERK = 200;         // rad/s^3
+constexpr double MAX_DURATION_EXTENSION_FACTOR = 10.0;
 constexpr double DURATION_EXTENSION_FRACTION = 1.1;
-constexpr double MINIMUM_VELOCITY_SEARCH_MAGNITUDE = 0.01;  // rad/s. Stop searching when velocity drops below this
 }  // namespace
 
 bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
@@ -69,21 +67,25 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
   const size_t num_waypoints = trajectory.getWayPointCount();
   if (num_waypoints < 2)
   {
-    RCLCPP_ERROR(LOGGER, "Trajectory does not have enough points to smooth with Ruckig");
-    return false;
+    RCLCPP_WARN(LOGGER,
+                "Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
+    return true;
   }
 
+  // Cache the trajectory in case we need to reset it
+  robot_trajectory::RobotTrajectory original_trajectory = trajectory;
+
   const size_t num_dof = group->getVariableCount();
 
   // This lib does not actually work properly when angles wrap around, so we need to unwind the path first
   trajectory.unwind();
 
   // Instantiate the smoother
-  double timestep = trajectory.getAverageSegmentDuration();
-  std::unique_ptr<ruckig::Ruckig<0>> ruckig_ptr;
-  ruckig_ptr = std::make_unique<ruckig::Ruckig<0>>(num_dof, timestep);
-  ruckig::InputParameter<0> ruckig_input{ num_dof };
-  ruckig::OutputParameter<0> ruckig_output{ num_dof };
+  double timestep = trajectory.getWayPointDurationFromStart(num_waypoints - 1) / (trajectory.getWayPointCount() - 1);
+  std::unique_ptr<ruckig::Ruckig<ruckig::DynamicDOFs>> ruckig_ptr;
+  ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
+  ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
+  ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ num_dof };
 
   // Initialize the smoother
   const std::vector<int>& idx = group->getVariableIndexList();
@@ -119,8 +121,8 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
   }
 
   ruckig::Result ruckig_result;
-  bool smoothing_complete = false;
   double duration_extension_factor = 1;
+  bool smoothing_complete = false;
   while ((duration_extension_factor < MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete)
   {
     for (size_t waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx)
@@ -132,76 +134,48 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
       // Run Ruckig
       ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);
 
-      // If the requested velocity is too great, a joint can actually "move backward" to give itself more time to
-      // accelerate to the target velocity. Iterate and decrease velocities until that behavior is gone.
-      bool backward_motion_detected = checkForLaggingMotion(group, ruckig_input, ruckig_output);
-
-      double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, group);
-      while (backward_motion_detected && (velocity_magnitude > MINIMUM_VELOCITY_SEARCH_MAGNITUDE))
+      if ((waypoint_idx == num_waypoints - 2) && ruckig_result == ruckig::Result::Finished)
       {
-        // Skip repeated waypoints with no change in position. Ruckig does not handle this well and there's really no
-        // need to smooth it Simply set it equal to the previous (identical) waypoint.
-        if (checkForIdenticalWaypoints(*trajectory.getWayPointPtr(waypoint_idx), *next_waypoint, trajectory.getGroup()))
-        {
-          *next_waypoint = trajectory.getWayPoint(waypoint_idx);
-          continue;
-        }
-
-        // decrease target velocity
-        for (size_t joint = 0; joint < num_dof; ++joint)
-        {
-          ruckig_input.target_velocity.at(joint) *= 0.9;
-          // Propagate the change in velocity to acceleration, too.
-          // We don't change the position to ensure the exact target position is achieved.
-          ruckig_input.target_acceleration.at(joint) =
-              (ruckig_input.target_velocity.at(joint) - ruckig_output.new_velocity.at(joint)) / timestep;
-        }
-        velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, group);
-        // Run Ruckig
-        ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);
-
-        // check for backward motion
-        backward_motion_detected = checkForLaggingMotion(group, ruckig_input, ruckig_output);
+        smoothing_complete = true;
+        break;
       }
 
-      // Overwrite pos/vel/accel of the target waypoint
-      for (size_t joint = 0; joint < num_dof; ++joint)
+      // Extend the trajectory duration if Ruckig could not reach the waypoint successfully
+      if (ruckig_result != ruckig::Result::Finished)
       {
-        next_waypoint->setVariablePosition(idx.at(joint), ruckig_output.new_position.at(joint));
-        next_waypoint->setVariableVelocity(idx.at(joint), ruckig_output.new_velocity.at(joint));
-        next_waypoint->setVariableAcceleration(idx.at(joint), ruckig_output.new_acceleration.at(joint));
-      }
-      next_waypoint->update();
-    }
-
-    // If ruckig failed, the duration of the seed trajectory likely wasn't long enough.
-    // Try duration extension several times.
-    // TODO: see issue 767.  (https://github.com/ros-planning/moveit2/issues/767)
-    if (ruckig_result == ruckig::Result::Working)
-    {
-      smoothing_complete = true;
-    }
-    else
-    {
-      // If Ruckig failed, it's likely because the original seed trajectory did not have a long enough duration when
-      // jerk is taken into account. Extend the duration and try again.
-      initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
-      duration_extension_factor *= DURATION_EXTENSION_FRACTION;
-      for (size_t waypoint_idx = 1; waypoint_idx < num_waypoints; ++waypoint_idx)
-      {
-        trajectory.setWayPointDurationFromPrevious(
-            waypoint_idx, DURATION_EXTENSION_FRACTION * trajectory.getWayPointDurationFromPrevious(waypoint_idx));
-        // TODO(andyz): re-calculate waypoint velocity and acceleration here?
+        duration_extension_factor *= DURATION_EXTENSION_FRACTION;
+        // Reset the trajectory
+        trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */);
+        for (size_t time_stretch_idx = 1; time_stretch_idx < num_waypoints; ++time_stretch_idx)
+        {
+          trajectory.setWayPointDurationFromPrevious(
+              time_stretch_idx,
+              duration_extension_factor * original_trajectory.getWayPointDurationFromPrevious(time_stretch_idx));
+          // re-calculate waypoint velocity and acceleration
+          auto target_state = trajectory.getWayPointPtr(time_stretch_idx);
+          auto const prev_state = trajectory.getWayPointPtr(time_stretch_idx - 1);
+          timestep = trajectory.getWayPointDurationFromStart(num_waypoints - 1) / (trajectory.getWayPointCount() - 1);
+          for (size_t joint = 0; joint < num_dof; ++joint)
+          {
+            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);
+          }
+          target_state->update();
+        }
+        ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
+        waypoint_idx = 0;
+        initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, move_group_idx);
+        break;
       }
-
-      timestep = trajectory.getAverageSegmentDuration();
-      ruckig_ptr = std::make_unique<ruckig::Ruckig<0>>(num_dof, timestep);
     }
   }
 
-  // Either of these results is acceptable.
-  // Working means smoothing worked well but the final target position wasn't exactly achieved (I think) -- Andy Z.
-  if ((ruckig_result != ruckig::Result::Working) && (ruckig_result != ruckig::Result::Finished))
+  if (ruckig_result != ruckig::Result::Finished)
   {
     RCLCPP_ERROR_STREAM(LOGGER, "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result);
     return false;
@@ -237,44 +211,6 @@ void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<0>& ruckig_in
   ruckig_output.new_acceleration = ruckig_input.current_acceleration;
 }
 
-bool RuckigSmoothing::checkForIdenticalWaypoints(const moveit::core::RobotState& prev_waypoint,
-                                                 const moveit::core::RobotState& next_waypoint,
-                                                 const moveit::core::JointModelGroup* joint_group)
-{
-  double magnitude_position_difference = prev_waypoint.distance(next_waypoint, joint_group);
-
-  return (magnitude_position_difference <= IDENTICAL_POSITION_EPSILON);
-}
-
-double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input,
-                                                   const moveit::core::JointModelGroup* joint_group)
-{
-  const size_t num_dof = joint_group->getVariableCount();
-  double vel_magnitude = 0;
-  for (size_t joint = 0; joint < num_dof; ++joint)
-  {
-    vel_magnitude += ruckig_input.target_velocity.at(joint) * ruckig_input.target_velocity.at(joint);
-  }
-  return sqrt(vel_magnitude);
-}
-
-bool RuckigSmoothing::checkForLaggingMotion(const moveit::core::JointModelGroup* joint_group,
-                                            const ruckig::InputParameter<0>& ruckig_input,
-                                            const ruckig::OutputParameter<0>& ruckig_output)
-{
-  const size_t num_dof = joint_group->getVariableCount();
-  // Check for backward motion of any joint
-  for (size_t joint = 0; joint < num_dof; ++joint)
-  {
-    // This indicates the jerk-limited output lags the target output
-    if ((ruckig_output.new_velocity.at(joint) / ruckig_input.target_velocity.at(joint)) < 1)
-    {
-      return true;
-    }
-  }
-  return false;
-}
-
 void RuckigSmoothing::getNextRuckigInput(const ruckig::OutputParameter<0>& ruckig_output,
                                          const moveit::core::RobotStatePtr& next_waypoint,
                                          const moveit::core::JointModelGroup* joint_group,
diff --git a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
index c1fbcf0834..f2477e46ab 100644
--- a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
@@ -59,27 +59,6 @@ class RuckigTests : public testing::Test
 
 }  // namespace
 
-TEST_F(RuckigTests, empty_trajectory)
-{
-  // This should fail because the trajectory is empty
-  EXPECT_FALSE(
-      smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));
-}
-
-TEST_F(RuckigTests, not_enough_waypoints)
-{
-  moveit::core::RobotState robot_state(robot_model_);
-  robot_state.setToDefaultValues();
-  // First waypoint is default joint positions
-  trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
-
-  robot_state.update();
-
-  // Fails due to not enough waypoints
-  EXPECT_FALSE(
-      smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));
-}
-
 TEST_F(RuckigTests, basic_trajectory)
 {
   moveit::core::RobotState robot_state(robot_model_);

From 865888d342d18b311712dc0044fb936b6c8854c6 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Wed, 16 Mar 2022 15:37:15 -0500
Subject: [PATCH 104/114] Add a test for a single waypoint

---
 .../test/test_ruckig_traj_smoothing.cpp       | 28 +++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
index f2477e46ab..f8fb9d7c3a 100644
--- a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
@@ -82,6 +82,34 @@ TEST_F(RuckigTests, basic_trajectory)
       smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));
 }
 
+TEST_F(RuckigTests, single_waypoint)
+{
+  // With only one waypoint, Ruckig cannot smooth the trajectory.
+  // It should simply pass the trajectory through unmodified and return true.
+
+  moveit::core::RobotState robot_state(robot_model_);
+  robot_state.setToDefaultValues();
+  // First waypoint is default joint positions
+  trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
+
+  robot_state.update();
+
+  // Trajectory should not change
+  auto first_waypoint_input = robot_state;
+
+  // Only one waypoint is OK, it does not fail
+  EXPECT_TRUE(
+      smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));
+  // And the waypoint did not change
+  auto const new_first_waypoint = trajectory_->getFirstWayPointPtr();
+  auto const& variable_names = new_first_waypoint->getVariableNames();
+  for (std::string const& variable_name : variable_names)
+  {
+    EXPECT_EQ(first_waypoint_input.getVariablePosition(variable_name),
+              new_first_waypoint->getVariablePosition(variable_name));
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);

From eb328cee1c75005c94ed325631b3cc5557bf8086 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Wed, 16 Mar 2022 16:14:38 -0500
Subject: [PATCH 105/114] Add a unit test that actually requires math

---
 .../test/test_ruckig_traj_smoothing.cpp       | 28 ++++++++++++++++++-
 1 file changed, 27 insertions(+), 1 deletion(-)

diff --git a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
index f8fb9d7c3a..6e12c9d950 100644
--- a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
@@ -71,15 +71,41 @@ TEST_F(RuckigTests, basic_trajectory)
   robot_state.copyJointGroupPositions(JOINT_GROUP, joint_positions);
   joint_positions.at(0) += 0.05;
   robot_state.setJointGroupPositions(JOINT_GROUP, joint_positions);
+  robot_state.update();
   trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
 
   joint_positions.at(0) += 0.05;
   robot_state.setJointGroupPositions(JOINT_GROUP, joint_positions);
+  robot_state.update();
   trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
 
+  EXPECT_TRUE(
+      smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));
+}
+
+TEST_F(RuckigTests, trajectory_duration)
+{
+  // Ideal duration is calculated from:
+  // x1 = x0 + v0*t + 0.5*a0*t^2
+  // Where x0 and v0 are zero
+  const double IDEAL_DURATION = 0.231;
+
+  moveit::core::RobotState robot_state(robot_model_);
+  robot_state.setToDefaultValues();
+  // Special attention to Joint 0. It is the only joint to move in this test.
+  // Zero velocities and accelerations at the endpoints
+  robot_state.setVariablePosition("panda_joint1", 0.0);
   robot_state.update();
+  trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
+
+  robot_state.setVariablePosition("panda_joint1", 0.1);
+  robot_state.update();
+  trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
+
   EXPECT_TRUE(
       smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));
+  EXPECT_GT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 0.9999 * IDEAL_DURATION);
+  EXPECT_LT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 1.2 * IDEAL_DURATION);
 }
 
 TEST_F(RuckigTests, single_waypoint)
@@ -97,7 +123,7 @@ TEST_F(RuckigTests, single_waypoint)
   // Trajectory should not change
   auto first_waypoint_input = robot_state;
 
-  // Only one waypoint is OK, it does not fail
+  // Only one waypoint is acceptable. True is returned.
   EXPECT_TRUE(
       smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));
   // And the waypoint did not change

From 3c62c13fbc2d397599955f9391413160a166d212 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Thu, 17 Mar 2022 08:32:05 -0500
Subject: [PATCH 106/114] Use a traj deep copy. Right-const everywhere. Minor
 cleanup.

---
 .../ruckig_traj_smoothing.h                   |  4 +--
 .../src/ruckig_traj_smoothing.cpp             | 33 ++++++++++---------
 2 files changed, 19 insertions(+), 18 deletions(-)

diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
index d7103f3a58..2da492130a 100644
--- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
+++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
@@ -46,8 +46,8 @@ class RuckigSmoothing
 {
 public:
   static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
-                             const double max_velocity_scaling_factor = 1.0,
-                             const double max_acceleration_scaling_factor = 1.0);
+                             double const max_velocity_scaling_factor = 1.0,
+                             double const max_acceleration_scaling_factor = 1.0);
 
 private:
   /**
diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
index da2c2cf419..f0b0416809 100644
--- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
@@ -45,26 +45,26 @@ namespace trajectory_processing
 {
 namespace
 {
-const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing");
-constexpr double DEFAULT_MAX_VELOCITY = 5;       // rad/s
-constexpr double DEFAULT_MAX_ACCELERATION = 10;  // rad/s^2
-constexpr double DEFAULT_MAX_JERK = 200;         // rad/s^3
-constexpr double MAX_DURATION_EXTENSION_FACTOR = 10.0;
-constexpr double DURATION_EXTENSION_FRACTION = 1.1;
+rclcpp::Logger const LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing");
+double constexpr DEFAULT_MAX_VELOCITY = 5;       // rad/s
+double constexpr DEFAULT_MAX_ACCELERATION = 10;  // rad/s^2
+double constexpr DEFAULT_MAX_JERK = 200;         // rad/s^3
+double constexpr MAX_DURATION_EXTENSION_FACTOR = 10.0;
+double constexpr DURATION_EXTENSION_FRACTION = 1.1;
 }  // namespace
 
 bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
                                      const double max_velocity_scaling_factor,
                                      const double max_acceleration_scaling_factor)
 {
-  const moveit::core::JointModelGroup* group = trajectory.getGroup();
+  moveit::core::JointModelGroup const* const group = trajectory.getGroup();
   if (!group)
   {
     RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for");
     return false;
   }
 
-  const size_t num_waypoints = trajectory.getWayPointCount();
+  size_t const num_waypoints = trajectory.getWayPointCount();
   if (num_waypoints < 2)
   {
     RCLCPP_WARN(LOGGER,
@@ -73,15 +73,16 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
   }
 
   // Cache the trajectory in case we need to reset it
-  robot_trajectory::RobotTrajectory original_trajectory = trajectory;
+  robot_trajectory::RobotTrajectory original_trajectory =
+      robot_trajectory::RobotTrajectory(trajectory, true /* deep copy */);
 
-  const size_t num_dof = group->getVariableCount();
+  size_t const num_dof = group->getVariableCount();
 
   // This lib does not actually work properly when angles wrap around, so we need to unwind the path first
   trajectory.unwind();
 
   // Instantiate the smoother
-  double timestep = trajectory.getWayPointDurationFromStart(num_waypoints - 1) / (trajectory.getWayPointCount() - 1);
+  double timestep = trajectory.getAverageSegmentDuration();
   std::unique_ptr<ruckig::Ruckig<ruckig::DynamicDOFs>> ruckig_ptr;
   ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
   ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
@@ -92,14 +93,14 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
   initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
 
   // Kinematic limits (vel/accel/jerk)
-  const std::vector<std::string>& vars = group->getVariableNames();
-  const moveit::core::RobotModel& rmodel = group->getParentModel();
+  std::vector<std::string> const& vars = group->getVariableNames();
+  moveit::core::RobotModel const& rmodel = group->getParentModel();
   for (size_t i = 0; i < num_dof; ++i)
   {
     // TODO(andyz): read this from the joint group if/when jerk limits are added to the JointModel
     ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;
 
-    const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars.at(i));
+    moveit::core::VariableBounds const& bounds = rmodel.getVariableBounds(vars.at(i));
 
     // This assumes min/max bounds are symmetric
     if (bounds.velocity_bounded_)
@@ -154,7 +155,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
           // re-calculate waypoint velocity and acceleration
           auto target_state = trajectory.getWayPointPtr(time_stretch_idx);
           auto const prev_state = trajectory.getWayPointPtr(time_stretch_idx - 1);
-          timestep = trajectory.getWayPointDurationFromStart(num_waypoints - 1) / (trajectory.getWayPointCount() - 1);
+          timestep = trajectory.getAverageSegmentDuration();
           for (size_t joint = 0; joint < num_dof; ++joint)
           {
             target_state->setVariableVelocity(move_group_idx.at(joint),
@@ -168,8 +169,8 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
           target_state->update();
         }
         ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
-        waypoint_idx = 0;
         initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, move_group_idx);
+        // Begin the while() loop again
         break;
       }
     }

From 7db9e49f179b0c61fba7250b610fece35aa69ec1 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Fri, 18 Mar 2022 09:16:33 -0500
Subject: [PATCH 107/114] Fix the test timestep

---
 .../test/test_ruckig_traj_smoothing.cpp                   | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
index 6e12c9d950..a610a69590 100644
--- a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
@@ -88,7 +88,7 @@ TEST_F(RuckigTests, trajectory_duration)
   // Ideal duration is calculated from:
   // x1 = x0 + v0*t + 0.5*a0*t^2
   // Where x0 and v0 are zero
-  const double IDEAL_DURATION = 0.231;
+  const double ideal_duration = 0.231;
 
   moveit::core::RobotState robot_state(robot_model_);
   robot_state.setToDefaultValues();
@@ -96,7 +96,7 @@ TEST_F(RuckigTests, trajectory_duration)
   // Zero velocities and accelerations at the endpoints
   robot_state.setVariablePosition("panda_joint1", 0.0);
   robot_state.update();
-  trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
+  trajectory_->addSuffixWayPoint(robot_state, 0.0);
 
   robot_state.setVariablePosition("panda_joint1", 0.1);
   robot_state.update();
@@ -104,8 +104,8 @@ TEST_F(RuckigTests, trajectory_duration)
 
   EXPECT_TRUE(
       smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));
-  EXPECT_GT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 0.9999 * IDEAL_DURATION);
-  EXPECT_LT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 1.2 * IDEAL_DURATION);
+  EXPECT_GT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 0.9999 * ideal_duration);
+  EXPECT_LT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 1.3 * ideal_duration);
 }
 
 TEST_F(RuckigTests, single_waypoint)

From d7fbc3dbfeea0893b702ee5ab0bbb21c9c8b0925 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Fri, 18 Mar 2022 13:25:50 -0500
Subject: [PATCH 108/114] Read jerk from the robot model

---
 .../src/ruckig_traj_smoothing.cpp                     | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
index f0b0416809..183aa0a02c 100644
--- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
@@ -97,9 +97,6 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
   moveit::core::RobotModel const& rmodel = group->getParentModel();
   for (size_t i = 0; i < num_dof; ++i)
   {
-    // TODO(andyz): read this from the joint group if/when jerk limits are added to the JointModel
-    ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;
-
     moveit::core::VariableBounds const& bounds = rmodel.getVariableBounds(vars.at(i));
 
     // This assumes min/max bounds are symmetric
@@ -119,6 +116,14 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
     {
       ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * DEFAULT_MAX_ACCELERATION;
     }
+    if (bounds.jerk_bounded_)
+    {
+      ruckig_input.max_jerk.at(i) = bounds.max_jerk_;
+    }
+    else
+    {
+      ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;
+    }
   }
 
   ruckig::Result ruckig_result;

From 87d473fbf0f3943f86efb35d3e78844f89e790b6 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Fri, 1 Apr 2022 13:22:20 -0500
Subject: [PATCH 109/114] Revert to right-const everywhere

---
 .../ruckig_traj_smoothing.h                   |  4 +--
 .../src/ruckig_traj_smoothing.cpp             | 25 +++++++++----------
 2 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
index 2da492130a..d7103f3a58 100644
--- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
+++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
@@ -46,8 +46,8 @@ class RuckigSmoothing
 {
 public:
   static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
-                             double const max_velocity_scaling_factor = 1.0,
-                             double const max_acceleration_scaling_factor = 1.0);
+                             const double max_velocity_scaling_factor = 1.0,
+                             const double max_acceleration_scaling_factor = 1.0);
 
 private:
   /**
diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
index 183aa0a02c..9686a988c4 100644
--- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
@@ -45,12 +45,12 @@ namespace trajectory_processing
 {
 namespace
 {
-rclcpp::Logger const LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing");
-double constexpr DEFAULT_MAX_VELOCITY = 5;       // rad/s
-double constexpr DEFAULT_MAX_ACCELERATION = 10;  // rad/s^2
-double constexpr DEFAULT_MAX_JERK = 200;         // rad/s^3
-double constexpr MAX_DURATION_EXTENSION_FACTOR = 10.0;
-double constexpr DURATION_EXTENSION_FRACTION = 1.1;
+const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing");
+constexpr double DEFAULT_MAX_VELOCITY = 5;       // rad/s
+constexpr double DEFAULT_MAX_ACCELERATION = 10;  // rad/s^2
+constexpr double DEFAULT_MAX_JERK = 200;         // rad/s^3
+constexpr double MAX_DURATION_EXTENSION_FACTOR = 10.0;
+constexpr double DURATION_EXTENSION_FRACTION = 1.1;
 }  // namespace
 
 bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
@@ -64,7 +64,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
     return false;
   }
 
-  size_t const num_waypoints = trajectory.getWayPointCount();
+  const size_t num_waypoints = trajectory.getWayPointCount();
   if (num_waypoints < 2)
   {
     RCLCPP_WARN(LOGGER,
@@ -76,7 +76,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
   robot_trajectory::RobotTrajectory original_trajectory =
       robot_trajectory::RobotTrajectory(trajectory, true /* deep copy */);
 
-  size_t const num_dof = group->getVariableCount();
+  const size_t num_dof = group->getVariableCount();
 
   // This lib does not actually work properly when angles wrap around, so we need to unwind the path first
   trajectory.unwind();
@@ -89,15 +89,14 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
   ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ num_dof };
 
   // Initialize the smoother
-  const std::vector<int>& idx = group->getVariableIndexList();
   initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
 
   // Kinematic limits (vel/accel/jerk)
-  std::vector<std::string> const& vars = group->getVariableNames();
-  moveit::core::RobotModel const& rmodel = group->getParentModel();
+  const std::vector<std::string>& vars = group->getVariableNames();
+  const moveit::core::RobotModel& rmodel = group->getParentModel();
   for (size_t i = 0; i < num_dof; ++i)
   {
-    moveit::core::VariableBounds const& bounds = rmodel.getVariableBounds(vars.at(i));
+    const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars.at(i));
 
     // This assumes min/max bounds are symmetric
     if (bounds.velocity_bounded_)
@@ -159,7 +158,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
               duration_extension_factor * original_trajectory.getWayPointDurationFromPrevious(time_stretch_idx));
           // re-calculate waypoint velocity and acceleration
           auto target_state = trajectory.getWayPointPtr(time_stretch_idx);
-          auto const prev_state = trajectory.getWayPointPtr(time_stretch_idx - 1);
+          const auto prev_state = trajectory.getWayPointPtr(time_stretch_idx - 1);
           timestep = trajectory.getAverageSegmentDuration();
           for (size_t joint = 0; joint < num_dof; ++joint)
           {

From 100ad5fd7cc624c032d71dda4d3727d7aa7b7c8d Mon Sep 17 00:00:00 2001
From: AndyZe <andyz@utexas.edu>
Date: Fri, 1 Apr 2022 13:24:05 -0500
Subject: [PATCH 110/114] Simplify jerk limit expression

Co-authored-by: Henning Kayser <henningkayser@picknik.ai>
---
 .../trajectory_processing/src/ruckig_traj_smoothing.cpp  | 9 +--------
 1 file changed, 1 insertion(+), 8 deletions(-)

diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
index 9686a988c4..da1640e875 100644
--- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
@@ -115,14 +115,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
     {
       ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * DEFAULT_MAX_ACCELERATION;
     }
-    if (bounds.jerk_bounded_)
-    {
-      ruckig_input.max_jerk.at(i) = bounds.max_jerk_;
-    }
-    else
-    {
-      ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;
-    }
+    ruckig_input.max_jerk.at(i) = bounds.jerk_bounded_ ? bounds.max_jerk_ : DEFAULT_MAX_JERK;
   }
 
   ruckig::Result ruckig_result;

From 070b159a0ccc87941a4f770196a811a163c389a8 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Sun, 3 Apr 2022 17:17:42 -0500
Subject: [PATCH 111/114] Add a helpful hint if no vel/accel/jerk limits are
 defined.

---
 .../src/ruckig_traj_smoothing.cpp             | 21 ++++++++++++++++++-
 1 file changed, 20 insertions(+), 1 deletion(-)

diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
index da1640e875..da2448e8e1 100644
--- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
@@ -48,7 +48,7 @@ namespace
 const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing");
 constexpr double DEFAULT_MAX_VELOCITY = 5;       // rad/s
 constexpr double DEFAULT_MAX_ACCELERATION = 10;  // rad/s^2
-constexpr double DEFAULT_MAX_JERK = 200;         // rad/s^3
+constexpr double DEFAULT_MAX_JERK = 1000;        // rad/s^3
 constexpr double MAX_DURATION_EXTENSION_FACTOR = 10.0;
 constexpr double DURATION_EXTENSION_FRACTION = 1.1;
 }  // namespace
@@ -105,6 +105,10 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
     }
     else
     {
+      RCLCPP_WARN_STREAM_ONCE(LOGGER,
+                              "Joint velocity limits are not defined. Using the default "
+                                  << DEFAULT_MAX_VELOCITY
+                                  << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml.");
       ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * DEFAULT_MAX_VELOCITY;
     }
     if (bounds.acceleration_bounded_)
@@ -113,9 +117,24 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
     }
     else
     {
+      RCLCPP_WARN_STREAM_ONCE(LOGGER,
+                              "Joint acceleration limits are not defined. Using the default "
+                                  << DEFAULT_MAX_ACCELERATION
+                                  << " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.");
       ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * DEFAULT_MAX_ACCELERATION;
     }
     ruckig_input.max_jerk.at(i) = bounds.jerk_bounded_ ? bounds.max_jerk_ : DEFAULT_MAX_JERK;
+    if (bounds.jerk_bounded_)
+    {
+      ruckig_input.max_jerk.at(i) = bounds.max_jerk_;
+    }
+    else
+    {
+      RCLCPP_WARN_STREAM_ONCE(LOGGER, "Joint jerk limits are not defined. Using the default "
+                                          << DEFAULT_MAX_JERK
+                                          << " rad/s^3. You can define jerk limits in joint_limits.yaml.");
+      ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;
+    }
   }
 
   ruckig::Result ruckig_result;

From c4afbae639ebe9ce2cf631197c2dfb41d221cd69 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Mon, 4 Apr 2022 14:06:20 -0500
Subject: [PATCH 112/114] Update test conditions since I changed the jerk limit

---
 .../test/test_ruckig_traj_smoothing.cpp               | 11 ++---------
 1 file changed, 2 insertions(+), 9 deletions(-)

diff --git a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
index a610a69590..1a2b1f69b4 100644
--- a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
@@ -74,21 +74,14 @@ TEST_F(RuckigTests, basic_trajectory)
   robot_state.update();
   trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
 
-  joint_positions.at(0) += 0.05;
-  robot_state.setJointGroupPositions(JOINT_GROUP, joint_positions);
-  robot_state.update();
-  trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
-
   EXPECT_TRUE(
       smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));
 }
 
 TEST_F(RuckigTests, trajectory_duration)
 {
-  // Ideal duration is calculated from:
-  // x1 = x0 + v0*t + 0.5*a0*t^2
-  // Where x0 and v0 are zero
-  const double ideal_duration = 0.231;
+  // Compare against the OJET online trajectory generator: https://www.trajectorygenerator.com/ojet-online/
+  const double ideal_duration = 0.21025;
 
   moveit::core::RobotState robot_state(robot_model_);
   robot_state.setToDefaultValues();

From 05afc04e191847f0a6cbd8f3c7ca00343efea790 Mon Sep 17 00:00:00 2001
From: AndyZe <zelenak@picknik.ai>
Date: Tue, 12 Apr 2022 09:34:50 -0500
Subject: [PATCH 113/114] Rebase cleanup

---
 .../trajectory_processing/src/ruckig_traj_smoothing.cpp        | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
index da2448e8e1..cdd940509d 100644
--- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
+++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
@@ -94,6 +94,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
   // Kinematic limits (vel/accel/jerk)
   const std::vector<std::string>& vars = group->getVariableNames();
   const moveit::core::RobotModel& rmodel = group->getParentModel();
+  const std::vector<int>& move_group_idx = group->getVariableIndexList();
   for (size_t i = 0; i < num_dof; ++i)
   {
     const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars.at(i));
@@ -185,7 +186,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
           target_state->update();
         }
         ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
-        initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, move_group_idx);
+        initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
         // Begin the while() loop again
         break;
       }

From 0f3f882a4dddb85989566d0a59fe0a7c85662d75 Mon Sep 17 00:00:00 2001
From: "David V. Lu!!" <davidvlu@gmail.com>
Date: Tue, 12 Apr 2022 10:48:41 -0400
Subject: [PATCH 114/114] Fix Moveit Configs Utils Bug (#1174)

---
 .../moveit_configs_utils/moveit_configs_builder.py          | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
index a605bb35e2..1ba9607da6 100644
--- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
+++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
@@ -229,7 +229,7 @@ def __init__(self, robot_name: str, robot_description="robot_description"):
             self.__urdf_file_path = self.__config_dir_path / (
                 self.__robot_name + ".urdf"
             )
-            self.__srdf_filename = self.__config_dir_path / (
+            self.__srdf_file_path = self.__config_dir_path / (
                 self.__robot_name + ".srdf"
             )
         else:
@@ -246,7 +246,7 @@ def __init__(self, robot_name: str, robot_description="robot_description"):
                     "relative_path"
                 ]
             )
-            self.__srdf_filename = Path(
+            self.__srdf_file_path = Path(
                 setup_assistant_yaml["moveit_setup_assistant_config"]["SRDF"][
                     "relative_path"
                 ]
@@ -283,7 +283,7 @@ def robot_description_semantic(
         self.__moveit_configs.robot_description_semantic = {
             self.__robot_description
             + "_semantic": load_xacro(
-                self._package_path / (file_path or self.__srdf_filename),
+                self._package_path / (file_path or self.__srdf_file_path),
                 mappings=mappings,
             )
         }