Use sdf FindElement API to avoid const_cast (#2231)

Several systems use const_cast in order to call
sdf::Element::GetElement with const ElementPtrs,
but the FindElement API can be used instead.

Signed-off-by: Steve Peters <[email protected]>

src/systems/diff_drive/DiffDrive.cc

@@ -182,18 +182,14 @@ void DiffDrive::Configure(const Entity &_entity,
     return;
   }
 
-  // Ugly, but needed because the sdf::Element::GetElement is not a const
-  // function and _sdf is a const shared pointer to a const sdf::Element.
-  auto ptr = const_cast<sdf::Element *>(_sdf.get());
-
   // Get params from SDF
-  sdf::ElementPtr sdfElem = ptr->GetElement("left_joint");
+  auto sdfElem = _sdf->FindElement("left_joint");
   while (sdfElem)
   {
     this->dataPtr->leftJointNames.push_back(sdfElem->Get<std::string>());
     sdfElem = sdfElem->GetNextElement("left_joint");
   }
-  sdfElem = ptr->GetElement("right_joint");
+  sdfElem = _sdf->FindElement("right_joint");
   while (sdfElem)
   {
     this->dataPtr->rightJointNames.push_back(sdfElem->Get<std::string>());

src/systems/joint_state_publisher/JointStatePublisher.cc

@@ -64,8 +64,7 @@ void JointStatePublisher::Configure(
   // specified joints. Otherwise, publish all the joints.
   if (_sdf->HasElement("joint_name"))
   {
-    sdf::Element *ptr = const_cast<sdf::Element *>(_sdf.get());
-    sdf::ElementPtr elem = ptr->GetElement("joint_name");
+    auto elem = _sdf->FindElement("joint_name");
     while (elem)
     {
       std::string jointName = elem->Get<std::string>();

src/systems/joint_traj_control/JointTrajectoryController.cc

@@ -811,8 +811,7 @@ std::vector<T> JointParameters::Parse(
 
   if (_sdf->HasElement(_parameterName))
   {
-    sdf::ElementPtr param = const_cast<sdf::Element *>(
-                                _sdf.get())->GetElement(_parameterName);
+    auto param = _sdf->FindElement(_parameterName);
     while (param)
     {
       output.push_back(param->Get<T>());

src/systems/log/LogRecord.cc

@@ -338,8 +338,7 @@ bool LogRecordPrivate::Start(const std::string &_logPath,
   // Get the topics to record, if any.
   if (this->sdf->HasElement("record_topic"))
   {
-    auto ptr = const_cast<sdf::Element *>(this->sdf.get());
-    sdf::ElementPtr recordTopicElem = ptr->GetElement("record_topic");
+    auto recordTopicElem = this->sdf->FindElement("record_topic");
 
     // This is used to determine if a topic is a regular expression.
     std::regex regexMatch(".*[\\*\\?\\[\\]\\(\\)\\.]+.*");

src/systems/log_video_recorder/LogVideoRecorder.cc

@@ -168,13 +168,9 @@ void LogVideoRecorder::Configure(
       this->dataPtr->node.Advertise<msgs::StringMsg>(
       "/log_video_recorder/status");
 
-  // Ugly, but needed because the sdf::Element::GetElement is not a const
-  // function and _sdf is a const shared pointer to a const sdf::Element.
-  auto ptr = const_cast<sdf::Element *>(_sdf.get());
-
   if (_sdf->HasElement("entity"))
   {
-    auto entityElem = ptr->GetElement("entity");
+    auto entityElem = _sdf->FindElement("entity");
     while (entityElem)
     {
       this->dataPtr->modelsToRecord.insert(entityElem->Get<std::string>());
@@ -184,7 +180,7 @@ void LogVideoRecorder::Configure(
 
   if (_sdf->HasElement("region"))
   {
-    sdf::ElementPtr regionElem = ptr->GetElement("region");
+    auto regionElem = _sdf->FindElement("region");
     while (regionElem)
     {
       auto min = regionElem->Get<math::Vector3d>("min");
@@ -198,14 +194,14 @@ void LogVideoRecorder::Configure(
 
   if (_sdf->HasElement("start_time"))
   {
-    auto t = ptr->Get<double>("start_time");
+    auto t = _sdf->Get<double>("start_time");
     this->dataPtr->startTime =
         std::chrono::milliseconds(static_cast<int64_t>(t * 1000.0));
   }
 
   if (_sdf->HasElement("end_time"))
   {
-    auto t = ptr->Get<double>("end_time");
+    auto t = _sdf->Get<double>("end_time");
     std::chrono::milliseconds ms(static_cast<int64_t>(t * 1000.0));
     if (this->dataPtr->startTime > ms)
     {
@@ -219,7 +215,7 @@ void LogVideoRecorder::Configure(
 
   if (_sdf->HasElement("exit_on_finish"))
   {
-    this->dataPtr->exitOnFinish = ptr->Get<bool>("exit_on_finish");
+    this->dataPtr->exitOnFinish = _sdf->Get<bool>("exit_on_finish");
   }
 
   this->dataPtr->loadTime = std::chrono::system_clock::now();

src/systems/mecanum_drive/MecanumDrive.cc

@@ -192,31 +192,27 @@ void MecanumDrive::Configure(const Entity &_entity,
     return;
   }
 
-  // Ugly, but needed because the sdf::Element::GetElement is not a const
-  // function and _sdf is a const shared pointer to a const sdf::Element.
-  auto ptr = const_cast<sdf::Element *>(_sdf.get());
-
   // Get params from SDF
-  sdf::ElementPtr sdfElem = ptr->GetElement("front_left_joint");
+  auto sdfElem = _sdf->FindElement("front_left_joint");
   while (sdfElem)
   {
     this->dataPtr->frontLeftJointNames.push_back(sdfElem->Get<std::string>());
     sdfElem = sdfElem->GetNextElement("front_left_joint");
   }
-  sdfElem = ptr->GetElement("front_right_joint");
+  sdfElem = _sdf->FindElement("front_right_joint");
   while (sdfElem)
   {
     this->dataPtr->frontRightJointNames.push_back(sdfElem->Get<std::string>());
     sdfElem = sdfElem->GetNextElement("front_right_joint");
   }
 
-  sdfElem = ptr->GetElement("back_left_joint");
+  sdfElem = _sdf->FindElement("back_left_joint");
   while (sdfElem)
   {
     this->dataPtr->backLeftJointNames.push_back(sdfElem->Get<std::string>());
     sdfElem = sdfElem->GetNextElement("back_left_joint");
   }
-  sdfElem = ptr->GetElement("back_right_joint");
+  sdfElem = _sdf->FindElement("back_right_joint");
   while (sdfElem)
   {
     this->dataPtr->backRightJointNames.push_back(sdfElem->Get<std::string>());

src/systems/shader_param/ShaderParam.cc

@@ -138,14 +138,11 @@ void ShaderParam::Configure(const Entity &_entity,
                EventManager &_eventMgr)
 {
   GZ_PROFILE("ShaderParam::Configure");
-  // Ugly, but needed because the sdf::Element::GetElement is not a const
-  // function and _sdf is a const shared pointer to a const sdf::Element.
-  auto sdf = const_cast<sdf::Element *>(_sdf.get());
 
-  if (sdf->HasElement("param"))
+  if (_sdf->HasElement("param"))
   {
     // loop and parse all shader params
-    sdf::ElementPtr paramElem = sdf->GetElement("param");
+    auto paramElem = _sdf->FindElement("param");
     while (paramElem)
     {
       if (!paramElem->HasElement("shader") ||
@@ -170,7 +167,7 @@ void ShaderParam::Configure(const Entity &_entity,
 
       if (paramElem->HasElement("arg"))
       {
-        sdf::ElementPtr argElem = paramElem->GetElement("arg");
+        auto argElem = paramElem->FindElement("arg");
         while (argElem)
         {
           spv.args.push_back(argElem->Get<std::string>());
@@ -191,14 +188,14 @@ void ShaderParam::Configure(const Entity &_entity,
   }
 
   // parse path to shaders
-  if (!sdf->HasElement("shader"))
+  if (!_sdf->HasElement("shader"))
   {
     gzerr << "Unable to load shader param system. "
            << "Missing <shader> SDF element." << std::endl;
     return;
   }
   // allow mulitple shader SDF element for different shader languages
-  sdf::ElementPtr shaderElem = sdf->GetElement("shader");
+  auto shaderElem = _sdf->FindElement("shader");
   while (shaderElem)
   {
     if (!shaderElem->HasElement("vertex") ||
@@ -217,11 +214,11 @@ void ShaderParam::Configure(const Entity &_entity,
       ShaderParamPrivate::ShaderUri shader;
       shader.language = api;
 
-      sdf::ElementPtr vertexElem = shaderElem->GetElement("vertex");
+      auto vertexElem = shaderElem->FindElement("vertex");
       shader.vertexShaderUri = common::findFile(
           asFullPath(vertexElem->Get<std::string>(),
           this->dataPtr->modelPath));
-      sdf::ElementPtr fragmentElem = shaderElem->GetElement("fragment");
+      auto fragmentElem = shaderElem->FindElement("fragment");
       shader.fragmentShaderUri = common::findFile(
           asFullPath(fragmentElem->Get<std::string>(),
           this->dataPtr->modelPath));

src/systems/tracked_vehicle/TrackedVehicle.cc

@@ -227,25 +227,21 @@ void TrackedVehicle::Configure(const Entity &_entity,
   if (!links.empty())
     this->dataPtr->canonicalLink = Link(links[0]);
 
-  // Ugly, but needed because the sdf::Element::GetElement is not a const
-  // function and _sdf is a const shared pointer to a const sdf::Element.
-  auto ptr = const_cast<sdf::Element *>(_sdf.get());
-
-  std::unordered_map<std::string, sdf::ElementPtr> tracks;
+  std::unordered_map<std::string, sdf::ElementConstPtr> tracks;
 
   if (_sdf->HasElement("body_link"))
     this->dataPtr->bodyLinkName = _sdf->Get<std::string>("body_link");
 
   // Get params from SDF
-  sdf::ElementPtr sdfElem = ptr->GetElement("left_track");
+  auto sdfElem = _sdf->FindElement("left_track");
   while (sdfElem)
   {
     const auto& linkName = sdfElem->Get<std::string>("link");
     this->dataPtr->leftTrackNames.push_back(linkName);
     tracks[linkName] = sdfElem;
     sdfElem = sdfElem->GetNextElement("left_track");
   }
-  sdfElem = ptr->GetElement("right_track");
+  sdfElem = _sdf->FindElement("right_track");
   while (sdfElem)
   {
     const auto& linkName = sdfElem->Get<std::string>("link");
@@ -288,7 +284,7 @@ void TrackedVehicle::Configure(const Entity &_entity,
     if (!_sdf->HasElement(tag))
       continue;
 
-    auto sdf = ptr->GetElement(tag);
+    auto sdf = _sdf->FindElement(tag);
 
     // Parse speed limiter parameters.
     bool hasVelocityLimits     = false;

src/systems/velocity_control/VelocityControl.cc

@@ -154,14 +154,10 @@ void VelocityControl::Configure(const Entity &_entity,
          << modelTopic << "]"
          << std::endl;
 
-  // Ugly, but needed because the sdf::Element::GetElement is not a const
-  // function and _sdf is a const shared pointer to a const sdf::Element.
-  auto ptr = const_cast<sdf::Element *>(_sdf.get());
-
-  if (!ptr->HasElement("link_name"))
+  if (!_sdf->HasElement("link_name"))
     return;
 
-  sdf::ElementPtr sdfElem = ptr->GetElement("link_name");
+  auto sdfElem = _sdf->FindElement("link_name");
   while (sdfElem)
   {
     this->dataPtr->linkNames.push_back(sdfElem->Get<std::string>());