Merge PR #2940: Improve error messages of Pilz planner

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h

@@ -63,7 +63,7 @@ class PlanningContextBase : public planning_interface::PlanningContext
     , terminated_(false)
     , model_(model)
     , limits_(limits)
-    , generator_(model, limits_)
+    , generator_(model, limits_, group)
   {
   }
 

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h

@@ -79,7 +79,8 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator
    *
    */
   TrajectoryGeneratorCIRC(const robot_model::RobotModelConstPtr& robot_model,
-                          const pilz_industrial_motion_planner::LimitsContainer& planner_limits);
+                          const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
+                          const std::string& group_name);
 
 private:
   void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override;

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h

@@ -68,7 +68,8 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator
    * @param planner_limits: limits in joint and Cartesian spaces
    */
   TrajectoryGeneratorLIN(const robot_model::RobotModelConstPtr& robot_model,
-                         const pilz_industrial_motion_planner::LimitsContainer& planner_limits);
+                         const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
+                         const std::string& group_name);
 
 private:
   void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h

@@ -63,7 +63,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator
    * @param model: a map of joint limits information
    */
   TrajectoryGeneratorPTP(const robot_model::RobotModelConstPtr& robot_model,
-                         const pilz_industrial_motion_planner::LimitsContainer& planner_limits);
+                         const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
+                         const std::string& group_name);
 
 private:
   void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
@@ -80,9 +81,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator
    * @param sampling_time
    */
   void planPTP(const std::map<std::string, double>& start_pos, const std::map<std::string, double>& goal_pos,
-               trajectory_msgs::JointTrajectory& joint_trajectory, const std::string& group_name,
-               const double& velocity_scaling_factor, const double& acceleration_scaling_factor,
-               const double& sampling_time);
+               trajectory_msgs::JointTrajectory& joint_trajectory, const double& velocity_scaling_factor,
+               const double& acceleration_scaling_factor, const double& sampling_time);
 
   void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
             const MotionPlanInfo& plan_info, const double& sampling_time,
@@ -91,8 +91,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator
 private:
   const double MIN_MOVEMENT = 0.001;
   pilz_industrial_motion_planner::JointLimitsContainer joint_limits_;
-  // most strict joint limits for each group
-  std::map<std::string, JointLimit> most_strict_limits_;
+  // most strict joint limits
+  JointLimit most_strict_limit_;
 };
 
 }  // namespace pilz_industrial_motion_planner

moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp

@@ -105,7 +105,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF
   {
     // LCOV_EXCL_START
     case 0:
-      ROS_ERROR_STREAM("no bounds set for joint " << joint_model->getName());
+      ROS_WARN_STREAM("no bounds set for joint " << joint_model->getName());
       break;
     // LCOV_EXCL_STOP
     case 1:
@@ -115,7 +115,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF
       break;
     // LCOV_EXCL_START
     default:
-      ROS_ERROR_STREAM("Multi-DOF-Joints not supported. The robot won't move. " << joint_model->getName());
+      ROS_WARN_STREAM("Multi-DOF-Joint '" << joint_model->getName() << "' not supported.");
       joint_limit.has_position_limits = true;
       joint_limit.min_position = 0;
       joint_limit.max_position = 0;
@@ -134,7 +134,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF
   {
     // LCOV_EXCL_START
     case 0:
-      ROS_ERROR_STREAM("no bounds set for joint " << joint_model->getName());
+      ROS_WARN_STREAM("no bounds set for joint " << joint_model->getName());
       break;
     // LCOV_EXCL_STOP
     case 1:
@@ -143,7 +143,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF
       break;
     // LCOV_EXCL_START
     default:
-      ROS_ERROR_STREAM("Multi-DOF-Joints not supported. The robot won't move.");
+      ROS_WARN_STREAM("Multi-DOF-Joint '" << joint_model->getName() << "' not supported.");
       joint_limit.has_velocity_limits = true;
       joint_limit.max_velocity = 0;
       break;

moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp

@@ -127,7 +127,7 @@ CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr&
   // Check that a loaded for this request exists
   if (!canServiceRequest(req))
   {
-    ROS_ERROR_STREAM("No ContextLoader for planner_id " << req.planner_id.c_str() << " found. Planning not possible.");
+    ROS_ERROR_STREAM("No ContextLoader for planner_id '" << req.planner_id.c_str() << "' found. Planning not possible.");
     return nullptr;
   }
 

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp

@@ -51,7 +51,8 @@
 namespace pilz_industrial_motion_planner
 {
 TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model,
-                                                 const LimitsContainer& planner_limits)
+                                                 const LimitsContainer& planner_limits,
+                                                 const std::string& /*group_name*/)
   : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits)
 {
   if (!planner_limits_.hasFullCartesianLimits())

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp

@@ -52,7 +52,7 @@
 namespace pilz_industrial_motion_planner
 {
 TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model,
-                                               const LimitsContainer& planner_limits)
+                                               const LimitsContainer& planner_limits, const std::string& /*group_name*/)
   : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits)
 {
   if (!planner_limits_.hasFullCartesianLimits())

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp

@@ -45,7 +45,7 @@
 namespace pilz_industrial_motion_planner
 {
 TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelConstPtr& robot_model,
-                                               const LimitsContainer& planner_limits)
+                                               const LimitsContainer& planner_limits, const std::string& group_name)
   : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits)
 {
   if (!planner_limits_.hasJointLimits())
@@ -56,43 +56,31 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelCons
   joint_limits_ = planner_limits_.getJointLimitContainer();
 
   // collect most strict joint limits for each group in robot model
-  for (const auto& jmg : robot_model->getJointModelGroups())
-  {
-    const auto& active_joints = jmg->getActiveJointModelNames();
-
-    // no active joints
-    if (active_joints.empty())
-    {
-      continue;
-    }
+  const auto* jmg = robot_model->getJointModelGroup(group_name);
+  if (!jmg)
+    throw TrajectoryGeneratorInvalidLimitsException("invalid group: " + group_name);
 
-    JointLimit most_strict_limit = joint_limits_.getCommonLimit(active_joints);
+  const auto& active_joints = jmg->getActiveJointModelNames();
 
-    if (!most_strict_limit.has_velocity_limits)
-    {
-      ROS_ERROR_STREAM("velocity limit not set for group " << jmg->getName());
-      throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + jmg->getName());
-    }
-    if (!most_strict_limit.has_acceleration_limits)
-    {
-      ROS_ERROR_STREAM("acceleration limit not set for group " << jmg->getName());
-      throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + jmg->getName());
-    }
-    if (!most_strict_limit.has_deceleration_limits)
-    {
-      ROS_ERROR_STREAM("deceleration limit not set for group " << jmg->getName());
-      throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + jmg->getName());
-    }
+  // no active joints
+  if (!active_joints.empty())
+  {
+    most_strict_limit_ = joint_limits_.getCommonLimit(active_joints);
 
-    most_strict_limits_.insert(std::pair<std::string, JointLimit>(jmg->getName(), most_strict_limit));
+    if (!most_strict_limit_.has_velocity_limits)
+      throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + group_name);
+    if (!most_strict_limit_.has_acceleration_limits)
+      throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + group_name);
+    if (!most_strict_limit_.has_deceleration_limits)
+      throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + group_name);
   }
 
   ROS_INFO("Initialized Point-to-Point Trajectory Generator.");
 }
 
 void TrajectoryGeneratorPTP::planPTP(const std::map<std::string, double>& start_pos,
                                      const std::map<std::string, double>& goal_pos,
-                                     trajectory_msgs::JointTrajectory& joint_trajectory, const std::string& group_name,
+                                     trajectory_msgs::JointTrajectory& joint_trajectory,
                                      const double& velocity_scaling_factor, const double& acceleration_scaling_factor,
                                      const double& sampling_time)
 {
@@ -139,10 +127,9 @@ void TrajectoryGeneratorPTP::planPTP(const std::map<std::string, double>& start_
   {
     // create vecocity profile if necessary
     velocity_profile.insert(std::make_pair(
-        joint_name,
-        VelocityProfileATrap(velocity_scaling_factor * most_strict_limits_.at(group_name).max_velocity,
-                             acceleration_scaling_factor * most_strict_limits_.at(group_name).max_acceleration,
-                             acceleration_scaling_factor * most_strict_limits_.at(group_name).max_deceleration)));
+        joint_name, VelocityProfileATrap(velocity_scaling_factor * most_strict_limit_.max_velocity,
+                                         acceleration_scaling_factor * most_strict_limit_.max_acceleration,
+                                         acceleration_scaling_factor * most_strict_limit_.max_deceleration)));
 
     velocity_profile.at(joint_name).SetProfile(start_pos.at(joint_name), goal_pos.at(joint_name));
     if (velocity_profile.at(joint_name).Duration() > max_duration)
@@ -262,7 +249,7 @@ void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& /
                                   const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory)
 {
   // plan the ptp trajectory
-  planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory, plan_info.group_name,
+  planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory,
           req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, sampling_time);
 }
 

moveit_planners/pilz_industrial_motion_planner/test/test_robots/prbt/launch/test_context.launch

@@ -10,10 +10,10 @@
 
   <!-- Load universal robot description format (URDF) -->
   <param name="$(arg robot_description)"
-         command="$(find xacro)/xacro --inorder $(find moveit_resources_prbt_support)/urdf/prbt.xacro gripper:=$(arg gripper)"/>
+         command="$(find xacro)/xacro $(find moveit_resources_prbt_support)/urdf/prbt.xacro gripper:=$(arg gripper)"/>
 
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro --inorder
+  <param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro
          $(find moveit_resources_prbt_moveit_config)/config/prbt.srdf.xacro gripper:=$(arg gripper)" />
 
 

moveit_planners/pilz_industrial_motion_planner/test/test_utils.h

@@ -89,7 +89,7 @@ inline std::string getJointName(size_t joint_number, const std::string& joint_pr
  */
 pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector<std::string>& joint_names);
 
-inline std::string demangel(char const* name)
+inline std::string demangle(char const* name)
 {
   return boost::core::demangle(name);
 }

moveit_planners/pilz_industrial_motion_planner/test/unittest_pilz_industrial_motion_planner.cpp

@@ -203,6 +203,7 @@ TEST_P(CommandPlannerTest, CheckPlanningContextRequest)
   moveit_msgs::MotionPlanRequest req;
   moveit_msgs::MoveItErrorCodes error_code;
 
+  req.group_name = "manipulator";
   // Check for the algorithms
   std::vector<std::string> algs;
   planner_instance_->getPlanningAlgorithms(algs);

moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context.cpp

@@ -113,7 +113,7 @@ class PlanningContextTest : public ::testing::Test
     limits.setCartesianLimits(cartesian_limit);
 
     planning_context_ = std::unique_ptr<typename T::Type_>(
-        new typename T::Type_("TestPlanningContext", "TestGroup", robot_model_, limits));
+        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_));
@@ -199,9 +199,9 @@ TYPED_TEST(PlanningContextTest, NoRequest)
   planning_interface::MotionPlanResponse res;
   bool result = this->planning_context_->solve(res);
 
-  EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name());
+  EXPECT_FALSE(result) << testutils::demangle(typeid(TypeParam).name());
   EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN, res.error_code_.val)
-      << testutils::demangel(typeid(TypeParam).name());
+      << testutils::demangle(typeid(TypeParam).name());
 }
 
 /**
@@ -210,23 +210,23 @@ TYPED_TEST(PlanningContextTest, NoRequest)
 TYPED_TEST(PlanningContextTest, SolveValidRequest)
 {
   planning_interface::MotionPlanResponse res;
-  planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name()));
+  planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangle(typeid(TypeParam).name()));
 
   this->planning_context_->setMotionPlanRequest(req);
 
   // TODO Formulate valid request
   bool result = this->planning_context_->solve(res);
 
-  EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name());
+  EXPECT_TRUE(result) << testutils::demangle(typeid(TypeParam).name());
   EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val)
-      << testutils::demangel(typeid(TypeParam).name());
+      << testutils::demangle(typeid(TypeParam).name());
 
   planning_interface::MotionPlanDetailedResponse res_detailed;
   bool result_detailed = this->planning_context_->solve(res_detailed);
 
-  EXPECT_TRUE(result_detailed) << testutils::demangel(typeid(TypeParam).name());
+  EXPECT_TRUE(result_detailed) << testutils::demangle(typeid(TypeParam).name());
   EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val)
-      << testutils::demangel(typeid(TypeParam).name());
+      << testutils::demangle(typeid(TypeParam).name());
 }
 
 /**
@@ -235,14 +235,14 @@ TYPED_TEST(PlanningContextTest, SolveValidRequest)
 TYPED_TEST(PlanningContextTest, SolveValidRequestDetailedResponse)
 {
   planning_interface::MotionPlanDetailedResponse res;  //<-- Detailed!
-  planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name()));
+  planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangle(typeid(TypeParam).name()));
 
   this->planning_context_->setMotionPlanRequest(req);
   bool result = this->planning_context_->solve(res);
 
-  EXPECT_TRUE(result) << testutils::demangel(typeid(TypeParam).name());
+  EXPECT_TRUE(result) << testutils::demangle(typeid(TypeParam).name());
   EXPECT_EQ(moveit_msgs::MoveItErrorCodes::SUCCESS, res.error_code_.val)
-      << testutils::demangel(typeid(TypeParam).name());
+      << testutils::demangle(typeid(TypeParam).name());
 }
 
 /**
@@ -251,26 +251,26 @@ TYPED_TEST(PlanningContextTest, SolveValidRequestDetailedResponse)
 TYPED_TEST(PlanningContextTest, SolveOnTerminated)
 {
   planning_interface::MotionPlanResponse res;
-  planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangel(typeid(TypeParam).name()));
+  planning_interface::MotionPlanRequest req = this->getValidRequest(testutils::demangle(typeid(TypeParam).name()));
 
   this->planning_context_->setMotionPlanRequest(req);
 
   bool result_termination = this->planning_context_->terminate();
-  EXPECT_TRUE(result_termination) << testutils::demangel(typeid(TypeParam).name());
+  EXPECT_TRUE(result_termination) << testutils::demangle(typeid(TypeParam).name());
 
   bool result = this->planning_context_->solve(res);
-  EXPECT_FALSE(result) << testutils::demangel(typeid(TypeParam).name());
+  EXPECT_FALSE(result) << testutils::demangle(typeid(TypeParam).name());
 
   EXPECT_EQ(moveit_msgs::MoveItErrorCodes::PLANNING_FAILED, res.error_code_.val)
-      << testutils::demangel(typeid(TypeParam).name());
+      << testutils::demangle(typeid(TypeParam).name());
 }
 
 /**
  * @brief Check if clear can be called. So far only stability is expected.
  */
 TYPED_TEST(PlanningContextTest, Clear)
 {
-  EXPECT_NO_THROW(this->planning_context_->clear()) << testutils::demangel(typeid(TypeParam).name());
+  EXPECT_NO_THROW(this->planning_context_->clear()) << testutils::demangle(typeid(TypeParam).name());
 }
 
 int main(int argc, char** argv)

moveit_planners/pilz_industrial_motion_planner/test/unittest_planning_context_loaders.cpp

@@ -139,9 +139,10 @@ TEST_P(PlanningContextLoadersTest, GetAlgorithm)
 TEST_P(PlanningContextLoadersTest, LoadContext)
 {
   planning_interface::PlanningContextPtr planning_context;
+  const std::string& group_name = "manipulator";
 
   // Without limits should return false
-  bool res = planning_context_loader_->loadContext(planning_context, "test", "test");
+  bool res = planning_context_loader_->loadContext(planning_context, "test", group_name);
   EXPECT_EQ(false, res) << "Context returned even when no limits where set";
 
   // After setting the limits this should work
@@ -161,7 +162,7 @@ TEST_P(PlanningContextLoadersTest, LoadContext)
 
   try
   {
-    res = planning_context_loader_->loadContext(planning_context, "test", "test");
+    res = planning_context_loader_->loadContext(planning_context, "test", group_name);
   }
   catch (std::exception& ex)
   {

moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp

@@ -137,7 +137,7 @@ void TrajectoryBlenderTransitionWindowTest::SetUp()
   planner_limits_.setCartesianLimits(cart_limits);
 
   // initialize trajectory generators and blender
-  lin_generator_ = std::make_unique<TrajectoryGeneratorLIN>(robot_model_, planner_limits_);
+  lin_generator_ = std::make_unique<TrajectoryGeneratorLIN>(robot_model_, planner_limits_, planning_group_);
   ASSERT_NE(nullptr, lin_generator_) << "failed to create LIN trajectory generator";
   blender_ = std::make_unique<TrajectoryBlenderTransitionWindow>(planner_limits_);
   ASSERT_NE(nullptr, blender_) << "failed to create trajectory blender";

moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp

@@ -141,7 +141,7 @@ void TrajectoryGeneratorCIRCTest::SetUp()
   planner_limits_.setCartesianLimits(cart_limits);
 
   // initialize the LIN trajectory generator
-  circ_ = std::make_unique<TrajectoryGeneratorCIRC>(robot_model_, planner_limits_);
+  circ_ = std::make_unique<TrajectoryGeneratorCIRC>(robot_model_, planner_limits_, planning_group_);
   ASSERT_NE(nullptr, circ_) << "failed to create CIRC trajectory generator";
 }
 
@@ -284,7 +284,8 @@ INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryGeneratorCIRCTest,
 TEST_P(TrajectoryGeneratorCIRCTest, noLimits)
 {
   LimitsContainer planner_limits;
-  EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits), TrajectoryGeneratorInvalidLimitsException);
+  EXPECT_THROW(TrajectoryGeneratorCIRC(this->robot_model_, planner_limits, planning_group_),
+               TrajectoryGeneratorInvalidLimitsException);
 }
 
 /**