Delete createRobotModel(). Minor cleanup.

moveit · Mar 23, 2023 · 22a5fbc · 22a5fbc
1 parent a6a1ccc
commit 22a5fbc
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 31 deletions.
diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/utilities.hpp b/moveit_core/planning_scene/include/moveit/planning_scene/utilities.hpp
@@ -42,16 +42,6 @@
 
 namespace planning_scene
 {
-
-/**
- * Helper function to create a RobotModel from a urdf/srdf.
- * @param urdf_model The universal robot description
- * @param srdf_model The semantic robot description
- * @return nullptr on failure
- */
-moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
-                                             const srdf::ModelConstSharedPtr& srdf_model);
-
 /**
  * convert Pose msg to Eigen::Isometry, normalizing the quaternion part if necessary.
  * @param msg Input message

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -136,9 +136,12 @@ PlanningScene::PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model,
   if (!srdf_model)
     throw moveit::ConstructException("The SRDF model cannot be nullptr");
 
-  robot_model_ = createRobotModel(urdf_model, srdf_model);
-  if (!robot_model_)
+  robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
+  if (!robot_model_ || !robot_model_->getRootJoint())
+  {
+    robot_model_ = nullptr;
     throw moveit::ConstructException("Could not create RobotModel");
+  }
 
   initialize();
 }

diff --git a/moveit_core/planning_scene/src/utilities.cpp b/moveit_core/planning_scene/src/utilities.cpp
@@ -44,29 +44,11 @@ namespace
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_planning_scene.utilities");
 }
 
-moveit::core::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr& urdf_model,
-                                             const srdf::ModelConstSharedPtr& 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();
-
-  return robot_model;
-}
-
 void poseMsgToEigen(const geometry_msgs::msg::Pose& msg, Eigen::Isometry3d& out)
 {
   Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z);
   Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z);
-  if ((quaternion.x() == 0) && (quaternion.y() == 0) && (quaternion.z() == 0) && (quaternion.w() == 0))
-  {
-    RCLCPP_WARN(LOGGER, "Empty quaternion found in pose message. Setting to neutral orientation.");
-    quaternion.setIdentity();
-  }
-  else
-  {
-    quaternion.normalize();
-  }
+  quaternion.normalize();
   out = translation * quaternion;
 }