From 22a5fbcaf10a49c0e3d936c82cbccaea4c3b2e3b Mon Sep 17 00:00:00 2001
From: AndyZe <andyz@utexas.edu>
Date: Thu, 23 Mar 2023 08:36:10 -0500
Subject: [PATCH] Delete createRobotModel(). Minor cleanup.

---
 .../moveit/planning_scene/utilities.hpp       | 10 ----------
 .../planning_scene/src/planning_scene.cpp     |  7 +++++--
 moveit_core/planning_scene/src/utilities.cpp  | 20 +------------------
 3 files changed, 6 insertions(+), 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
index a5b7d8fa01..4ab994691b 100644
--- 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
index 47db6ff94e..b00fb7ba7d 100644
--- 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
index ce8ca9de29..3c47665664 100644
--- 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;
 }