Skip to content

Commit 301832d

Browse files
scpetersscpeters
authored
Merge pull request #2363 from gazebosim/scpeters/merge_8_main
Merge gz-sim8 ➡️ main
2 parents 250c939 + 00f2676 commit 301832d

File tree

16 files changed

+96
-230
lines changed

16 files changed

+96
-230
lines changed

‎Changelog.md

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,23 @@
44

55
## Gazebo Sim 8.x
66

7+
### Gazebo Sim 8.2.0 (2024-03-14)
8+
9+
1. Add reference to joint_controller.md tutorial.
10+
* [Pull request #2333](https://github.com/gazebosim/gz-sim/pull/2333)
11+
12+
1. Fix wget in maritime tutorials
13+
* [Pull request #2330](https://github.com/gazebosim/gz-sim/pull/2330)
14+
15+
1. Add entity and sdf parameters to Server's AddSystem interface
16+
* [Pull request #2324](https://github.com/gazebosim/gz-sim/pull/2324)
17+
18+
1. Add entity validation to OdometryPublisher
19+
* [Pull request #2326](https://github.com/gazebosim/gz-sim/pull/2326)
20+
21+
1. Fix typo in Joint.hh
22+
* [Pull request #2310](https://github.com/gazebosim/gz-sim/pull/2310)
23+
724
### Gazebo Sim 8.1.0 (2024-02-06)
825

926
1. Add tutorial for using components in systems

‎src/MeshInertiaCalculator.cc

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -218,6 +218,11 @@ std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
218218
// Load the Mesh
219219
gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance();
220220
mesh = meshManager->Load(fullPath);
221+
if (!mesh)
222+
{
223+
gzerr << "Failed to load mesh: " << fullPath << std::endl;
224+
return std::nullopt;
225+
}
221226
std::vector<Triangle> meshTriangles;
222227
gz::math::MassMatrix3d meshMassMatrix;
223228
gz::math::Pose3d centreOfMass;

‎src/SimulationRunner.cc

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -463,6 +463,8 @@ void SimulationRunner::PublishStats()
463463

464464
msg.set_paused(this->currentInfo.paused);
465465

466+
msgs::Set(msg.mutable_step_size(), this->currentInfo.dt);
467+
466468
if (this->Stepping())
467469
{
468470
// (deprecated) Remove this header in Gazebo H

‎src/gui/plugins/apply_force_torque/ApplyForceTorque.cc

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,6 @@
1919
#include <mutex>
2020
#include <string>
2121

22-
#include <gz/common/MeshManager.hh>
2322
#include <gz/common/MouseEvent.hh>
2423
#include <gz/gui/Application.hh>
2524
#include <gz/gui/GuiEvents.hh>

‎src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1245,7 +1245,14 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry(
12451245
gz::common::MeshManager *meshManager =
12461246
gz::common::MeshManager::Instance();
12471247
descriptor.mesh = meshManager->Load(descriptor.meshName);
1248-
geom = this->scene->CreateMesh(descriptor);
1248+
if (descriptor.mesh)
1249+
{
1250+
geom = this->scene->CreateMesh(descriptor);
1251+
}
1252+
else
1253+
{
1254+
gzerr << "Failed to load mesh: " << descriptor.meshName << std::endl;
1255+
}
12491256
scale = _geom.MeshShape()->Scale();
12501257
}
12511258
else if (_geom.Type() == sdf::GeometryType::HEIGHTMAP)

‎src/rendering/SceneManager.cc

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -813,8 +813,14 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom,
813813
rendering::MeshDescriptor descriptor;
814814
descriptor.meshName = name;
815815
descriptor.mesh = meshManager->MeshByName(name);
816-
817-
geom = this->dataPtr->scene->CreateMesh(descriptor);
816+
if (descriptor.mesh)
817+
{
818+
geom = this->dataPtr->scene->CreateMesh(descriptor);
819+
}
820+
else
821+
{
822+
gzerr << "Unable to find the polyline mesh: " << name << std::endl;
823+
}
818824
}
819825
else
820826
{

‎src/systems/ackermann_steering/AckermannSteering.cc

Lines changed: 10 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -608,70 +608,29 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info,
608608
for (Entity joint : this->dataPtr->leftJoints)
609609
{
610610
// Update wheel velocity
611-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
612-
613-
if (vel == nullptr)
614-
{
615-
_ecm.CreateComponent(
616-
joint, components::JointVelocityCmd(
617-
{this->dataPtr->leftJointSpeed}));
618-
}
619-
else
620-
{
621-
*vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed});
622-
}
611+
_ecm.SetComponentData<components::JointVelocityCmd>(
612+
joint, {this->dataPtr->leftJointSpeed});
623613
}
624614

625615
for (Entity joint : this->dataPtr->rightJoints)
626616
{
627617
// Update wheel velocity
628-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
629-
630-
if (vel == nullptr)
631-
{
632-
_ecm.CreateComponent(joint,
633-
components::JointVelocityCmd({this->dataPtr->rightJointSpeed}));
634-
}
635-
else
636-
{
637-
*vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed});
638-
}
618+
_ecm.SetComponentData<components::JointVelocityCmd>(
619+
joint, {this->dataPtr->rightJointSpeed});
639620
}
640621
}
641622

642623
// Update steering
643624
for (Entity joint : this->dataPtr->leftSteeringJoints)
644625
{
645-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
646-
647-
if (vel == nullptr)
648-
{
649-
_ecm.CreateComponent(
650-
joint, components::JointVelocityCmd(
651-
{this->dataPtr->leftSteeringJointSpeed}));
652-
}
653-
else
654-
{
655-
*vel = components::JointVelocityCmd(
656-
{this->dataPtr->leftSteeringJointSpeed});
657-
}
626+
_ecm.SetComponentData<components::JointVelocityCmd>(
627+
joint, {this->dataPtr->leftSteeringJointSpeed});
658628
}
659629

660630
for (Entity joint : this->dataPtr->rightSteeringJoints)
661631
{
662-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
663-
664-
if (vel == nullptr)
665-
{
666-
_ecm.CreateComponent(joint,
667-
components::JointVelocityCmd(
668-
{this->dataPtr->rightSteeringJointSpeed}));
669-
}
670-
else
671-
{
672-
*vel = components::JointVelocityCmd(
673-
{this->dataPtr->rightSteeringJointSpeed});
674-
}
632+
_ecm.SetComponentData<components::JointVelocityCmd>(
633+
joint, {this->dataPtr->rightSteeringJointSpeed});
675634
}
676635
if (!this->dataPtr->steeringOnly)
677636
{
@@ -968,11 +927,11 @@ void AckermannSteeringPrivate::UpdateAngle(
968927

969928
double leftSteeringJointAngle =
970929
atan((2.0 * this->wheelBase * sin(ang)) / \
971-
((2.0 * this->wheelBase * cos(ang)) + \
930+
((2.0 * this->wheelBase * cos(ang)) - \
972931
(1.0 * this->wheelSeparation * sin(ang))));
973932
double rightSteeringJointAngle =
974933
atan((2.0 * this->wheelBase * sin(ang)) / \
975-
((2.0 * this->wheelBase * cos(ang)) - \
934+
((2.0 * this->wheelBase * cos(ang)) + \
976935
(1.0 * this->wheelSeparation * sin(ang))));
977936

978937
auto leftSteeringPos = _ecm.Component<components::JointPosition>(

‎src/systems/diff_drive/DiffDrive.cc

Lines changed: 4 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -459,17 +459,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info,
459459
continue;
460460

461461
// Update wheel velocity
462-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
463-
464-
if (vel == nullptr)
465-
{
466-
_ecm.CreateComponent(
467-
joint, components::JointVelocityCmd({this->dataPtr->leftJointSpeed}));
468-
}
469-
else
470-
{
471-
*vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed});
472-
}
462+
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
463+
{this->dataPtr->leftJointSpeed});
473464
}
474465

475466
for (Entity joint : this->dataPtr->rightJoints)
@@ -479,17 +470,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info,
479470
continue;
480471

481472
// Update wheel velocity
482-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
483-
484-
if (vel == nullptr)
485-
{
486-
_ecm.CreateComponent(joint,
487-
components::JointVelocityCmd({this->dataPtr->rightJointSpeed}));
488-
}
489-
else
490-
{
491-
*vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed});
492-
}
473+
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
474+
{this->dataPtr->rightJointSpeed});
493475
}
494476

495477
// Create the left and right side joint position components if they

‎src/systems/dvl/DopplerVelocityLogSystem.cc

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -220,6 +220,9 @@ class gz::sim::systems::DopplerVelocityLogSystem::Implementation
220220
/// \brief Current simulation time.
221221
public: std::chrono::steady_clock::duration nextUpdateTime{
222222
std::chrono::steady_clock::duration::max()};
223+
224+
/// \brief Mutex to protect current simulation times
225+
public: std::mutex timeMutex;
223226
};
224227

225228
using namespace gz;
@@ -351,6 +354,8 @@ void DopplerVelocityLogSystem::Implementation::DoPostUpdate(
351354
return true;
352355
});
353356

357+
std::lock_guard<std::mutex> timeLock(this->timeMutex);
358+
354359
if (!this->perStepRequests.empty() || (
355360
!_info.paused && this->nextUpdateTime <= _info.simTime))
356361
{
@@ -611,6 +616,9 @@ void DopplerVelocityLogSystem::Implementation::OnRender()
611616
}
612617

613618
auto closestUpdateTime = std::chrono::steady_clock::duration::max();
619+
620+
std::lock_guard<std::mutex> timeLock(this->timeMutex);
621+
614622
for (const auto & [_, sensorId] : this->sensorIdPerEntity)
615623
{
616624
gz::sensors::Sensor *sensor =
@@ -635,6 +643,9 @@ void DopplerVelocityLogSystem::Implementation::OnRender()
635643
void DopplerVelocityLogSystem::Implementation::OnPostRender()
636644
{
637645
GZ_PROFILE("DopplerVelocityLogSystem::Implementation::OnPostRender");
646+
647+
std::lock_guard<std::mutex> timeLock(this->timeMutex);
648+
638649
for (const auto & sensorId : this->updatedSensorIds)
639650
{
640651
auto *sensor =

‎src/systems/joint_controller/JointController.cc

Lines changed: 1 addition & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -344,17 +344,7 @@ void JointController::PreUpdate(const UpdateInfo &_info,
344344
// Update joint velocity
345345
for (Entity joint : this->dataPtr->jointEntities)
346346
{
347-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
348-
349-
if (vel == nullptr)
350-
{
351-
_ecm.CreateComponent(
352-
joint, components::JointVelocityCmd({targetVel}));
353-
}
354-
else
355-
{
356-
*vel = components::JointVelocityCmd({targetVel});
357-
}
347+
_ecm.SetComponentData<components::JointVelocityCmd>(joint, {targetVel});
358348
}
359349
}
360350
}

‎src/systems/joint_position_controller/JointPositionController.cc

Lines changed: 1 addition & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -461,17 +461,7 @@ void JointPositionController::PreUpdate(
461461
for (Entity joint : this->dataPtr->jointEntities)
462462
{
463463
// Update velocity command.
464-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
465-
466-
if (vel == nullptr)
467-
{
468-
_ecm.CreateComponent(
469-
joint, components::JointVelocityCmd({targetVel}));
470-
}
471-
else
472-
{
473-
*vel = components::JointVelocityCmd({targetVel});
474-
}
464+
_ecm.SetComponentData<components::JointVelocityCmd>(joint, {targetVel});
475465
}
476466
return;
477467
}

‎src/systems/mecanum_drive/MecanumDrive.cc

Lines changed: 8 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -426,66 +426,29 @@ void MecanumDrive::PreUpdate(const gz::sim::UpdateInfo &_info,
426426
for (Entity joint : this->dataPtr->frontLeftJoints)
427427
{
428428
// Update wheel velocity
429-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
430-
431-
if (vel == nullptr)
432-
{
433-
_ecm.CreateComponent(joint,
434-
components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed}));
435-
}
436-
else
437-
{
438-
*vel = components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed});
439-
}
429+
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
430+
{this->dataPtr->frontLeftJointSpeed});
440431
}
441432

442433
for (Entity joint : this->dataPtr->frontRightJoints)
443434
{
444435
// Update wheel velocity
445-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
446-
447-
if (vel == nullptr)
448-
{
449-
_ecm.CreateComponent(joint,
450-
components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed}));
451-
}
452-
else
453-
{
454-
*vel =
455-
components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed});
456-
}
436+
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
437+
{this->dataPtr->frontRightJointSpeed});
457438
}
458439

459440
for (Entity joint : this->dataPtr->backLeftJoints)
460441
{
461442
// Update wheel velocity
462-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
463-
464-
if (vel == nullptr)
465-
{
466-
_ecm.CreateComponent(joint,
467-
components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed}));
468-
}
469-
else
470-
{
471-
*vel = components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed});
472-
}
443+
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
444+
{this->dataPtr->backLeftJointSpeed});
473445
}
474446

475447
for (Entity joint : this->dataPtr->backRightJoints)
476448
{
477449
// Update wheel velocity
478-
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);
479-
480-
if (vel == nullptr)
481-
{
482-
_ecm.CreateComponent(joint,
483-
components::JointVelocityCmd({this->dataPtr->backRightJointSpeed}));
484-
}
485-
else
486-
{
487-
*vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed});
488-
}
450+
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
451+
{this->dataPtr->backRightJointSpeed});
489452
}
490453
}
491454

‎src/systems/thruster/Thruster.cc

Lines changed: 2 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -740,18 +740,8 @@ void Thruster::PreUpdate(
740740
// Velocity control
741741
else
742742
{
743-
auto velocityComp =
744-
_ecm.Component<gz::sim::components::JointVelocityCmd>(
745-
this->dataPtr->jointEntity);
746-
if (velocityComp == nullptr)
747-
{
748-
_ecm.CreateComponent(this->dataPtr->jointEntity,
749-
components::JointVelocityCmd({desiredPropellerAngVel}));
750-
}
751-
else
752-
{
753-
velocityComp->Data()[0] = desiredPropellerAngVel;
754-
}
743+
_ecm.SetComponentData<gz::sim::components::JointVelocityCmd>(
744+
this->dataPtr->jointEntity, {desiredPropellerAngVel});
755745
angvel.set_data(desiredPropellerAngVel);
756746
}
757747

0 commit comments

Comments
 (0)