From f89c44ee703fa112a988188a673ee6b420ffc0a4 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 6 Nov 2023 20:56:08 -0800 Subject: [PATCH 1/3] Use sdf FindElement API to avoid const_cast 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 --- src/systems/diff_drive/DiffDrive.cc | 8 ++------ .../JointStatePublisher.cc | 3 +-- .../JointTrajectoryController.cc | 3 +-- src/systems/log/LogRecord.cc | 4 ++-- .../log_video_recorder/LogVideoRecorder.cc | 14 +++++--------- src/systems/mecanum_drive/MecanumDrive.cc | 12 ++++-------- src/systems/shader_param/ShaderParam.cc | 17 +++++++---------- src/systems/tracked_vehicle/TrackedVehicle.cc | 12 ++++-------- src/systems/velocity_control/VelocityControl.cc | 8 ++------ 9 files changed, 28 insertions(+), 53 deletions(-) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 7da57a118c..067dfa82ae 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -187,18 +187,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.get()); - // Get params from SDF - sdf::ElementPtr sdfElem = ptr->GetElement("left_joint"); + sdf::ElementConstPtr sdfElem = _sdf->FindElement("left_joint"); while (sdfElem) { this->dataPtr->leftJointNames.push_back(sdfElem->Get()); sdfElem = sdfElem->GetNextElement("left_joint"); } - sdfElem = ptr->GetElement("right_joint"); + sdfElem = _sdf->FindElement("right_joint"); while (sdfElem) { this->dataPtr->rightJointNames.push_back(sdfElem->Get()); diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc index f1aa0c59d8..5c2e807ee8 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.cc +++ b/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.get()); - sdf::ElementPtr elem = ptr->GetElement("joint_name"); + sdf::ElementConstPtr elem = _sdf->FindElement("joint_name"); while (elem) { std::string jointName = elem->Get(); diff --git a/src/systems/joint_traj_control/JointTrajectoryController.cc b/src/systems/joint_traj_control/JointTrajectoryController.cc index d6882f9796..214da2ac56 100644 --- a/src/systems/joint_traj_control/JointTrajectoryController.cc +++ b/src/systems/joint_traj_control/JointTrajectoryController.cc @@ -811,8 +811,7 @@ std::vector JointParameters::Parse( if (_sdf->HasElement(_parameterName)) { - sdf::ElementPtr param = const_cast( - _sdf.get())->GetElement(_parameterName); + sdf::ElementConstPtr param = _sdf->FindElement(_parameterName); while (param) { output.push_back(param->Get()); diff --git a/src/systems/log/LogRecord.cc b/src/systems/log/LogRecord.cc index e545ff7baa..a8934b4c41 100644 --- a/src/systems/log/LogRecord.cc +++ b/src/systems/log/LogRecord.cc @@ -338,8 +338,8 @@ bool LogRecordPrivate::Start(const std::string &_logPath, // Get the topics to record, if any. if (this->sdf->HasElement("record_topic")) { - auto ptr = const_cast(this->sdf.get()); - sdf::ElementPtr recordTopicElem = ptr->GetElement("record_topic"); + sdf::ElementConstPtr recordTopicElem = + this->sdf->FindElement("record_topic"); // This is used to determine if a topic is a regular expression. std::regex regexMatch(".*[\\*\\?\\[\\]\\(\\)\\.]+.*"); diff --git a/src/systems/log_video_recorder/LogVideoRecorder.cc b/src/systems/log_video_recorder/LogVideoRecorder.cc index beb0abd655..64fd20928f 100644 --- a/src/systems/log_video_recorder/LogVideoRecorder.cc +++ b/src/systems/log_video_recorder/LogVideoRecorder.cc @@ -171,13 +171,9 @@ void LogVideoRecorder::Configure( this->dataPtr->node.Advertise( "/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.get()); - if (_sdf->HasElement("entity")) { - auto entityElem = ptr->GetElement("entity"); + auto entityElem = _sdf->FindElement("entity"); while (entityElem) { this->dataPtr->modelsToRecord.insert(entityElem->Get()); @@ -187,7 +183,7 @@ void LogVideoRecorder::Configure( if (_sdf->HasElement("region")) { - sdf::ElementPtr regionElem = ptr->GetElement("region"); + sdf::ElementConstPtr regionElem = _sdf->FindElement("region"); while (regionElem) { auto min = regionElem->Get("min"); @@ -201,14 +197,14 @@ void LogVideoRecorder::Configure( if (_sdf->HasElement("start_time")) { - auto t = ptr->Get("start_time"); + auto t = _sdf->Get("start_time"); this->dataPtr->startTime = std::chrono::milliseconds(static_cast(t * 1000.0)); } if (_sdf->HasElement("end_time")) { - auto t = ptr->Get("end_time"); + auto t = _sdf->Get("end_time"); std::chrono::milliseconds ms(static_cast(t * 1000.0)); if (this->dataPtr->startTime > ms) { @@ -222,7 +218,7 @@ void LogVideoRecorder::Configure( if (_sdf->HasElement("exit_on_finish")) { - this->dataPtr->exitOnFinish = ptr->Get("exit_on_finish"); + this->dataPtr->exitOnFinish = _sdf->Get("exit_on_finish"); } this->dataPtr->loadTime = std::chrono::system_clock::now(); diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index c1b084bce3..1530a47116 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -194,31 +194,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.get()); - // Get params from SDF - sdf::ElementPtr sdfElem = ptr->GetElement("front_left_joint"); + sdf::ElementConstPtr sdfElem = _sdf->FindElement("front_left_joint"); while (sdfElem) { this->dataPtr->frontLeftJointNames.push_back(sdfElem->Get()); 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()); 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()); 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()); diff --git a/src/systems/shader_param/ShaderParam.cc b/src/systems/shader_param/ShaderParam.cc index 7c12f0ded2..cc73be5a16 100644 --- a/src/systems/shader_param/ShaderParam.cc +++ b/src/systems/shader_param/ShaderParam.cc @@ -144,14 +144,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.get()); - if (sdf->HasElement("param")) + if (_sdf->HasElement("param")) { // loop and parse all shader params - sdf::ElementPtr paramElem = sdf->GetElement("param"); + sdf::ElementConstPtr paramElem = _sdf->FindElement("param"); while (paramElem) { if (!paramElem->HasElement("shader") || @@ -176,7 +173,7 @@ void ShaderParam::Configure(const Entity &_entity, if (paramElem->HasElement("arg")) { - sdf::ElementPtr argElem = paramElem->GetElement("arg"); + sdf::ElementConstPtr argElem = paramElem->FindElement("arg"); while (argElem) { spv.args.push_back(argElem->Get()); @@ -197,14 +194,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 SDF element." << std::endl; return; } // allow mulitple shader SDF element for different shader languages - sdf::ElementPtr shaderElem = sdf->GetElement("shader"); + sdf::ElementConstPtr shaderElem = _sdf->FindElement("shader"); while (shaderElem) { if (!shaderElem->HasElement("vertex") || @@ -223,11 +220,11 @@ void ShaderParam::Configure(const Entity &_entity, ShaderParamPrivate::ShaderUri shader; shader.language = api; - sdf::ElementPtr vertexElem = shaderElem->GetElement("vertex"); + sdf::ElementConstPtr vertexElem = shaderElem->FindElement("vertex"); shader.vertexShaderUri = common::findFile( asFullPath(vertexElem->Get(), this->dataPtr->modelPath)); - sdf::ElementPtr fragmentElem = shaderElem->GetElement("fragment"); + sdf::ElementConstPtr fragmentElem = shaderElem->FindElement("fragment"); shader.fragmentShaderUri = common::findFile( asFullPath(fragmentElem->Get(), this->dataPtr->modelPath)); diff --git a/src/systems/tracked_vehicle/TrackedVehicle.cc b/src/systems/tracked_vehicle/TrackedVehicle.cc index 4afc223033..323989a325 100644 --- a/src/systems/tracked_vehicle/TrackedVehicle.cc +++ b/src/systems/tracked_vehicle/TrackedVehicle.cc @@ -233,17 +233,13 @@ 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.get()); - - std::unordered_map tracks; + std::unordered_map tracks; if (_sdf->HasElement("body_link")) this->dataPtr->bodyLinkName = _sdf->Get("body_link"); // Get params from SDF - sdf::ElementPtr sdfElem = ptr->GetElement("left_track"); + sdf::ElementConstPtr sdfElem = _sdf->FindElement("left_track"); while (sdfElem) { const auto& linkName = sdfElem->Get("link"); @@ -251,7 +247,7 @@ void TrackedVehicle::Configure(const Entity &_entity, 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("link"); @@ -294,7 +290,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; diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 92728a11b4..f83ed02921 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/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.get()); - - if (!ptr->HasElement("link_name")) + if (!_sdf->HasElement("link_name")) return; - sdf::ElementPtr sdfElem = ptr->GetElement("link_name"); + sdf::ElementConstPtr sdfElem = _sdf->FindElement("link_name"); while (sdfElem) { this->dataPtr->linkNames.push_back(sdfElem->Get()); From 7b6e966246d3307326e7af367cc6e37073e2ec96 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 7 Nov 2023 15:38:30 -0800 Subject: [PATCH 2/3] Use more auto Signed-off-by: Steve Peters --- src/systems/diff_drive/DiffDrive.cc | 2 +- .../joint_state_publisher/JointStatePublisher.cc | 2 +- .../joint_traj_control/JointTrajectoryController.cc | 2 +- src/systems/log/LogRecord.cc | 3 +-- src/systems/log_video_recorder/LogVideoRecorder.cc | 2 +- src/systems/mecanum_drive/MecanumDrive.cc | 2 +- src/systems/shader_param/ShaderParam.cc | 10 +++++----- src/systems/tracked_vehicle/TrackedVehicle.cc | 2 +- src/systems/velocity_control/VelocityControl.cc | 2 +- 9 files changed, 13 insertions(+), 14 deletions(-) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 067dfa82ae..700e342743 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -188,7 +188,7 @@ void DiffDrive::Configure(const Entity &_entity, } // Get params from SDF - sdf::ElementConstPtr sdfElem = _sdf->FindElement("left_joint"); + auto sdfElem = _sdf->FindElement("left_joint"); while (sdfElem) { this->dataPtr->leftJointNames.push_back(sdfElem->Get()); diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc index 5c2e807ee8..c8085198f4 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.cc +++ b/src/systems/joint_state_publisher/JointStatePublisher.cc @@ -64,7 +64,7 @@ void JointStatePublisher::Configure( // specified joints. Otherwise, publish all the joints. if (_sdf->HasElement("joint_name")) { - sdf::ElementConstPtr elem = _sdf->FindElement("joint_name"); + auto elem = _sdf->FindElement("joint_name"); while (elem) { std::string jointName = elem->Get(); diff --git a/src/systems/joint_traj_control/JointTrajectoryController.cc b/src/systems/joint_traj_control/JointTrajectoryController.cc index 214da2ac56..58647463b4 100644 --- a/src/systems/joint_traj_control/JointTrajectoryController.cc +++ b/src/systems/joint_traj_control/JointTrajectoryController.cc @@ -811,7 +811,7 @@ std::vector JointParameters::Parse( if (_sdf->HasElement(_parameterName)) { - sdf::ElementConstPtr param = _sdf->FindElement(_parameterName); + auto param = _sdf->FindElement(_parameterName); while (param) { output.push_back(param->Get()); diff --git a/src/systems/log/LogRecord.cc b/src/systems/log/LogRecord.cc index a8934b4c41..8cbeadbfb1 100644 --- a/src/systems/log/LogRecord.cc +++ b/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")) { - sdf::ElementConstPtr recordTopicElem = - this->sdf->FindElement("record_topic"); + auto recordTopicElem = this->sdf->FindElement("record_topic"); // This is used to determine if a topic is a regular expression. std::regex regexMatch(".*[\\*\\?\\[\\]\\(\\)\\.]+.*"); diff --git a/src/systems/log_video_recorder/LogVideoRecorder.cc b/src/systems/log_video_recorder/LogVideoRecorder.cc index 64fd20928f..7be53f67f5 100644 --- a/src/systems/log_video_recorder/LogVideoRecorder.cc +++ b/src/systems/log_video_recorder/LogVideoRecorder.cc @@ -183,7 +183,7 @@ void LogVideoRecorder::Configure( if (_sdf->HasElement("region")) { - sdf::ElementConstPtr regionElem = _sdf->FindElement("region"); + auto regionElem = _sdf->FindElement("region"); while (regionElem) { auto min = regionElem->Get("min"); diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index 1530a47116..6d77de4c77 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -195,7 +195,7 @@ void MecanumDrive::Configure(const Entity &_entity, } // Get params from SDF - sdf::ElementConstPtr sdfElem = _sdf->FindElement("front_left_joint"); + auto sdfElem = _sdf->FindElement("front_left_joint"); while (sdfElem) { this->dataPtr->frontLeftJointNames.push_back(sdfElem->Get()); diff --git a/src/systems/shader_param/ShaderParam.cc b/src/systems/shader_param/ShaderParam.cc index cc73be5a16..d89cd57ddb 100644 --- a/src/systems/shader_param/ShaderParam.cc +++ b/src/systems/shader_param/ShaderParam.cc @@ -148,7 +148,7 @@ void ShaderParam::Configure(const Entity &_entity, if (_sdf->HasElement("param")) { // loop and parse all shader params - sdf::ElementConstPtr paramElem = _sdf->FindElement("param"); + auto paramElem = _sdf->FindElement("param"); while (paramElem) { if (!paramElem->HasElement("shader") || @@ -173,7 +173,7 @@ void ShaderParam::Configure(const Entity &_entity, if (paramElem->HasElement("arg")) { - sdf::ElementConstPtr argElem = paramElem->FindElement("arg"); + auto argElem = paramElem->FindElement("arg"); while (argElem) { spv.args.push_back(argElem->Get()); @@ -201,7 +201,7 @@ void ShaderParam::Configure(const Entity &_entity, return; } // allow mulitple shader SDF element for different shader languages - sdf::ElementConstPtr shaderElem = _sdf->FindElement("shader"); + auto shaderElem = _sdf->FindElement("shader"); while (shaderElem) { if (!shaderElem->HasElement("vertex") || @@ -220,11 +220,11 @@ void ShaderParam::Configure(const Entity &_entity, ShaderParamPrivate::ShaderUri shader; shader.language = api; - sdf::ElementConstPtr vertexElem = shaderElem->FindElement("vertex"); + auto vertexElem = shaderElem->FindElement("vertex"); shader.vertexShaderUri = common::findFile( asFullPath(vertexElem->Get(), this->dataPtr->modelPath)); - sdf::ElementConstPtr fragmentElem = shaderElem->FindElement("fragment"); + auto fragmentElem = shaderElem->FindElement("fragment"); shader.fragmentShaderUri = common::findFile( asFullPath(fragmentElem->Get(), this->dataPtr->modelPath)); diff --git a/src/systems/tracked_vehicle/TrackedVehicle.cc b/src/systems/tracked_vehicle/TrackedVehicle.cc index 323989a325..848c672f43 100644 --- a/src/systems/tracked_vehicle/TrackedVehicle.cc +++ b/src/systems/tracked_vehicle/TrackedVehicle.cc @@ -239,7 +239,7 @@ void TrackedVehicle::Configure(const Entity &_entity, this->dataPtr->bodyLinkName = _sdf->Get("body_link"); // Get params from SDF - sdf::ElementConstPtr sdfElem = _sdf->FindElement("left_track"); + auto sdfElem = _sdf->FindElement("left_track"); while (sdfElem) { const auto& linkName = sdfElem->Get("link"); diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index f83ed02921..68224b3bab 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -157,7 +157,7 @@ void VelocityControl::Configure(const Entity &_entity, if (!_sdf->HasElement("link_name")) return; - sdf::ElementConstPtr sdfElem = _sdf->FindElement("link_name"); + auto sdfElem = _sdf->FindElement("link_name"); while (sdfElem) { this->dataPtr->linkNames.push_back(sdfElem->Get()); From 9cf082432ba8dc9a9f072bb5d90f589a764b4bd1 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 5 Nov 2023 08:14:43 -0800 Subject: [PATCH 3/3] Fix gz-sensors branch in custom sensor example Signed-off-by: Steve Peters --- examples/plugin/custom_sensor_system/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/plugin/custom_sensor_system/CMakeLists.txt b/examples/plugin/custom_sensor_system/CMakeLists.txt index 60bc0d3c63..6682fed462 100644 --- a/examples/plugin/custom_sensor_system/CMakeLists.txt +++ b/examples/plugin/custom_sensor_system/CMakeLists.txt @@ -20,7 +20,7 @@ include(FetchContent) FetchContent_Declare( sensors_clone GIT_REPOSITORY https://github.com/gazebosim/gz-sensors - GIT_TAG gz-sensors9 + GIT_TAG main ) FetchContent_Populate(sensors_clone) add_subdirectory(${sensors_clone_SOURCE_DIR}/examples/custom_sensor ${sensors_clone_BINARY_DIR})