Adds multi (joint) state validation service (#3426)

Co-authored-by: Tom Noble <[email protected]>
Co-authored-by: Mark Johnson <[email protected]>

Author: 3 people

moveit_ros/move_group/CMakeLists.txt

@@ -52,6 +52,7 @@ add_library(
   src/default_capabilities/query_planners_service_capability.cpp
   src/default_capabilities/save_geometry_to_file_service_capability.cpp
   src/default_capabilities/state_validation_service_capability.cpp
+  src/default_capabilities/multi_state_validation_service_capability.cpp
   src/default_capabilities/tf_publisher_capability.cpp)
 target_include_directories(
   moveit_move_group_default_capabilities

moveit_ros/move_group/default_capabilities_plugin_description.xml

@@ -48,6 +48,12 @@
     </description>
   </class>
 
+  <class name="move_group/MoveGroupMultiStateValidationService" type="move_group::MoveGroupMultiStateValidationService" base_class_type="move_group::MoveGroupCapability">
+    <description>
+      Provide a ROS service that allows for testing multiple joint states validity
+    </description>
+  </class>
+
   <class name="move_group/MoveGroupGetPlanningSceneService" type="move_group::MoveGroupGetPlanningSceneService" base_class_type="move_group::MoveGroupCapability">
     <description>
       Provide a ROS service that allows for querying the planning scene

moveit_ros/move_group/include/moveit/move_group/capability_names.hpp

@@ -54,6 +54,8 @@ static const std::string IK_SERVICE_NAME = "compute_ik";  // name of ik service
 static const std::string FK_SERVICE_NAME = "compute_fk";  // name of fk service
 static const std::string STATE_VALIDITY_SERVICE_NAME =
     "check_state_validity";  // name of the service that validates states
+static const std::string MULTI_STATE_VALIDITY_SERVICE_NAME =
+    "check_multi_state_validity";  // name of the service that validates multiple joint states
 static const std::string CARTESIAN_PATH_SERVICE_NAME =
     "compute_cartesian_path";  // name of the service that computes cartesian paths
 static const std::string GET_PLANNING_SCENE_SERVICE_NAME =

moveit_ros/move_group/src/default_capabilities/multi_state_validation_service_capability.cpp

@@ -0,0 +1,53 @@
+#include "multi_state_validation_service_capability.hpp"
+#include <moveit/collision_detection/collision_tools.hpp>
+#include <moveit/move_group/capability_names.hpp>
+#include <moveit/moveit_cpp/moveit_cpp.hpp>
+#include <moveit/robot_state/conversions.hpp>
+#include <moveit/utils/message_checks.hpp>
+
+namespace move_group
+{
+
+void MoveGroupMultiStateValidationService::initialize()
+{
+  validity_service_ = context_->moveit_cpp_->getNode()->create_service<moveit_msgs::srv::GetMultiStateValidity>(
+      MULTI_STATE_VALIDITY_SERVICE_NAME,
+      [this](const std::shared_ptr<rmw_request_id_t>& request_header,
+             const std::shared_ptr<moveit_msgs::srv::GetMultiStateValidity::Request>& req,
+             const std::shared_ptr<moveit_msgs::srv::GetMultiStateValidity::Response>& res) {
+        return computeService(request_header, req, res);
+      });
+}
+
+bool MoveGroupMultiStateValidationService::computeService(
+    const std::shared_ptr<rmw_request_id_t>& /* unused */,
+    const std::shared_ptr<moveit_msgs::srv::GetMultiStateValidity::Request>& req,
+    const std::shared_ptr<moveit_msgs::srv::GetMultiStateValidity::Response>& res)
+{
+  planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_);
+  moveit::core::RobotState rs = ls->getCurrentState();
+  moveit::core::robotStateMsgToRobotState(req->robot_state, rs);
+  for (size_t i = 0; i < req->joint_states.size(); ++i)
+  {
+    // Update robot state with next set of joint states
+    rs.setVariableValues(req->joint_states[i]);
+
+    // Check validity of given joint state
+    res->valid = isStateValid(ls, rs, req->group_name, req->constraints, res->contacts, res->cost_sources,
+                              res->constraint_result);
+
+    // This service only checks up to the first invalid joint state,
+    // with the result then holding the information about the first invalid
+    // state
+    if (!res->valid)
+    {
+      break;
+    }
+  }
+  return true;
+}
+}  // namespace move_group
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(move_group::MoveGroupMultiStateValidationService, move_group::MoveGroupCapability)

moveit_ros/move_group/src/default_capabilities/multi_state_validation_service_capability.hpp

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "state_validation_service_capability.hpp"
+#include <moveit_msgs/srv/get_multi_state_validity.hpp>
+
+namespace move_group
+{
+class MoveGroupMultiStateValidationService : public MoveGroupStateValidationService
+{
+public:
+  void initialize() override;
+
+private:
+  bool computeService(const std::shared_ptr<rmw_request_id_t>& request_header,
+                      const std::shared_ptr<moveit_msgs::srv::GetMultiStateValidity::Request>& req,
+                      const std::shared_ptr<moveit_msgs::srv::GetMultiStateValidity::Response>& res);
+
+  rclcpp::Service<moveit_msgs::srv::GetMultiStateValidity>::SharedPtr validity_service_;
+};
+}  // namespace move_group

moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp

@@ -35,11 +35,11 @@
 /* Author: Ioan Sucan */
 
 #include "state_validation_service_capability.hpp"
+#include <moveit/collision_detection/collision_tools.hpp>
+#include <moveit/move_group/capability_names.hpp>
 #include <moveit/moveit_cpp/moveit_cpp.hpp>
 #include <moveit/robot_state/conversions.hpp>
 #include <moveit/utils/message_checks.hpp>
-#include <moveit/collision_detection/collision_tools.hpp>
-#include <moveit/move_group/capability_names.hpp>
 
 namespace move_group
 {
@@ -57,20 +57,18 @@ void MoveGroupStateValidationService::initialize()
       });
 }
 
-bool MoveGroupStateValidationService::computeService(
-    const std::shared_ptr<rmw_request_id_t>& /* unused */,
-    const std::shared_ptr<moveit_msgs::srv::GetStateValidity::Request>& req,
-    const std::shared_ptr<moveit_msgs::srv::GetStateValidity::Response>& res)
+bool MoveGroupStateValidationService::isStateValid(
+    const planning_scene_monitor::LockedPlanningSceneRO& ls, const moveit::core::RobotState& rs,
+    const std::string& group_name, const moveit_msgs::msg::Constraints& constraints,
+    std::vector<moveit_msgs::msg::ContactInformation>& contacts,
+    std::vector<moveit_msgs::msg::CostSource>& cost_sources,
+    std::vector<moveit_msgs::msg::ConstraintEvalResult>& constraint_result)
 {
-  planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_);
-  moveit::core::RobotState rs = ls->getCurrentState();
-  moveit::core::robotStateMsgToRobotState(req->robot_state, rs);
-
-  res->valid = true;
+  bool valid = true;
 
   // configure collision request
   collision_detection::CollisionRequest creq;
-  creq.group_name = req->group_name;
+  creq.group_name = group_name;
   creq.cost = true;
   creq.contacts = true;
   creq.max_contacts = ls->getWorld()->size() + ls->getRobotModel()->getLinkModelsWithCollisionGeometry().size();
@@ -85,48 +83,63 @@ bool MoveGroupStateValidationService::computeService(
   if (cres.collision)
   {
     rclcpp::Time time_now = context_->moveit_cpp_->getNode()->get_clock()->now();
-    res->contacts.reserve(cres.contact_count);
-    res->valid = false;
+    contacts.reserve(cres.contact_count);
+    valid = false;
     for (collision_detection::CollisionResult::ContactMap::const_iterator it = cres.contacts.begin();
          it != cres.contacts.end(); ++it)
     {
       for (const collision_detection::Contact& contact : it->second)
       {
-        res->contacts.resize(res->contacts.size() + 1);
-        collision_detection::contactToMsg(contact, res->contacts.back());
-        res->contacts.back().header.frame_id = ls->getPlanningFrame();
-        res->contacts.back().header.stamp = time_now;
+        contacts.resize(contacts.size() + 1);
+        collision_detection::contactToMsg(contact, contacts.back());
+        contacts.back().header.frame_id = ls->getPlanningFrame();
+        contacts.back().header.stamp = time_now;
       }
     }
   }
 
   // copy cost sources
-  res->cost_sources.reserve(cres.cost_sources.size());
+  cost_sources.reserve(cres.cost_sources.size());
   for (const collision_detection::CostSource& cost_source : cres.cost_sources)
   {
-    res->cost_sources.resize(res->cost_sources.size() + 1);
-    collision_detection::costSourceToMsg(cost_source, res->cost_sources.back());
+    cost_sources.resize(cost_sources.size() + 1);
+    collision_detection::costSourceToMsg(cost_source, cost_sources.back());
   }
 
   // evaluate constraints
-  if (!moveit::core::isEmpty(req->constraints))
+  if (!moveit::core::isEmpty(constraints))
   {
     kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel());
-    kset.add(req->constraints, ls->getTransforms());
+    kset.add(constraints, ls->getTransforms());
     std::vector<kinematic_constraints::ConstraintEvaluationResult> kres;
     kinematic_constraints::ConstraintEvaluationResult total_result = kset.decide(rs, kres);
     if (!total_result.satisfied)
-      res->valid = false;
+    {
+      valid = false;
+    }
 
     // copy constraint results
-    res->constraint_result.resize(kres.size());
+    constraint_result.resize(kres.size());
     for (std::size_t k = 0; k < kres.size(); ++k)
     {
-      res->constraint_result[k].result = kres[k].satisfied;
-      res->constraint_result[k].distance = kres[k].distance;
+      constraint_result[k].result = kres[k].satisfied;
+      constraint_result[k].distance = kres[k].distance;
     }
   }
 
+  return valid;
+}
+
+bool MoveGroupStateValidationService::computeService(
+    const std::shared_ptr<rmw_request_id_t>& /* unused */,
+    const std::shared_ptr<moveit_msgs::srv::GetStateValidity::Request>& req,
+    const std::shared_ptr<moveit_msgs::srv::GetStateValidity::Response>& res)
+{
+  planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_);
+  moveit::core::RobotState rs = ls->getCurrentState();
+  moveit::core::robotStateMsgToRobotState(req->robot_state, rs);
+  res->valid =
+      isStateValid(ls, rs, req->group_name, req->constraints, res->contacts, res->cost_sources, res->constraint_result);
   return true;
 }
 }  // namespace move_group

moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.hpp

@@ -48,6 +48,13 @@ class MoveGroupStateValidationService : public MoveGroupCapability
 
   void initialize() override;
 
+protected:
+  bool isStateValid(const planning_scene_monitor::LockedPlanningSceneRO& ls, const moveit::core::RobotState& rs,
+                    const std::string& group_name, const moveit_msgs::msg::Constraints& constraints,
+                    std::vector<moveit_msgs::msg::ContactInformation>& contacts,
+                    std::vector<moveit_msgs::msg::CostSource>& cost_sources,
+                    std::vector<moveit_msgs::msg::ConstraintEvalResult>& constraint_result);
+
 private:
   bool computeService(const std::shared_ptr<rmw_request_id_t>& request_header,
                       const std::shared_ptr<moveit_msgs::srv::GetStateValidity::Request>& req,