Skip to content

Commit

Permalink
Merge pull request #2363 from gazebosim/scpeters/merge_8_main
Browse files Browse the repository at this point in the history
Merge gz-sim8 ➡️  main
  • Loading branch information
scpeters authored Apr 5, 2024
2 parents 250c939 + 00f2676 commit 301832d
Show file tree
Hide file tree
Showing 16 changed files with 96 additions and 230 deletions.
17 changes: 17 additions & 0 deletions Changelog.md
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,23 @@

## Gazebo Sim 8.x

### Gazebo Sim 8.2.0 (2024-03-14)

1. Add reference to joint_controller.md tutorial.
* [Pull request #2333](https://github.com/gazebosim/gz-sim/pull/2333)

1. Fix wget in maritime tutorials
* [Pull request #2330](https://github.com/gazebosim/gz-sim/pull/2330)

1. Add entity and sdf parameters to Server's AddSystem interface
* [Pull request #2324](https://github.com/gazebosim/gz-sim/pull/2324)

1. Add entity validation to OdometryPublisher
* [Pull request #2326](https://github.com/gazebosim/gz-sim/pull/2326)

1. Fix typo in Joint.hh
* [Pull request #2310](https://github.com/gazebosim/gz-sim/pull/2310)

### Gazebo Sim 8.1.0 (2024-02-06)

1. Add tutorial for using components in systems
Expand Down
5 changes: 5 additions & 0 deletions src/MeshInertiaCalculator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -218,6 +218,11 @@ std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
// Load the Mesh
gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance();
mesh = meshManager->Load(fullPath);
if (!mesh)
{
gzerr << "Failed to load mesh: " << fullPath << std::endl;
return std::nullopt;
}
std::vector<Triangle> meshTriangles;
gz::math::MassMatrix3d meshMassMatrix;
gz::math::Pose3d centreOfMass;
Expand Down
2 changes: 2 additions & 0 deletions src/SimulationRunner.cc
Original file line number Diff line number Diff line change
Expand Up @@ -463,6 +463,8 @@ void SimulationRunner::PublishStats()

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

msgs::Set(msg.mutable_step_size(), this->currentInfo.dt);

if (this->Stepping())
{
// (deprecated) Remove this header in Gazebo H
Expand Down
1 change: 0 additions & 1 deletion src/gui/plugins/apply_force_torque/ApplyForceTorque.cc
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@
#include <mutex>
#include <string>

#include <gz/common/MeshManager.hh>
#include <gz/common/MouseEvent.hh>
#include <gz/gui/Application.hh>
#include <gz/gui/GuiEvents.hh>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1245,7 +1245,14 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry(
gz::common::MeshManager *meshManager =
gz::common::MeshManager::Instance();
descriptor.mesh = meshManager->Load(descriptor.meshName);
geom = this->scene->CreateMesh(descriptor);
if (descriptor.mesh)
{
geom = this->scene->CreateMesh(descriptor);
}
else
{
gzerr << "Failed to load mesh: " << descriptor.meshName << std::endl;
}
scale = _geom.MeshShape()->Scale();
}
else if (_geom.Type() == sdf::GeometryType::HEIGHTMAP)
Expand Down
10 changes: 8 additions & 2 deletions src/rendering/SceneManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -813,8 +813,14 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom,
rendering::MeshDescriptor descriptor;
descriptor.meshName = name;
descriptor.mesh = meshManager->MeshByName(name);

geom = this->dataPtr->scene->CreateMesh(descriptor);
if (descriptor.mesh)
{
geom = this->dataPtr->scene->CreateMesh(descriptor);
}
else
{
gzerr << "Unable to find the polyline mesh: " << name << std::endl;
}
}
else
{
Expand Down
61 changes: 10 additions & 51 deletions src/systems/ackermann_steering/AckermannSteering.cc
Original file line number Diff line number Diff line change
Expand Up @@ -608,70 +608,29 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info,
for (Entity joint : this->dataPtr->leftJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(
joint, components::JointVelocityCmd(
{this->dataPtr->leftJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(
joint, {this->dataPtr->leftJointSpeed});
}

for (Entity joint : this->dataPtr->rightJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->rightJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(
joint, {this->dataPtr->rightJointSpeed});
}
}

// Update steering
for (Entity joint : this->dataPtr->leftSteeringJoints)
{
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(
joint, components::JointVelocityCmd(
{this->dataPtr->leftSteeringJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd(
{this->dataPtr->leftSteeringJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(
joint, {this->dataPtr->leftSteeringJointSpeed});
}

for (Entity joint : this->dataPtr->rightSteeringJoints)
{
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd(
{this->dataPtr->rightSteeringJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd(
{this->dataPtr->rightSteeringJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(
joint, {this->dataPtr->rightSteeringJointSpeed});
}
if (!this->dataPtr->steeringOnly)
{
Expand Down Expand Up @@ -968,11 +927,11 @@ void AckermannSteeringPrivate::UpdateAngle(

double leftSteeringJointAngle =
atan((2.0 * this->wheelBase * sin(ang)) / \
((2.0 * this->wheelBase * cos(ang)) + \
((2.0 * this->wheelBase * cos(ang)) - \
(1.0 * this->wheelSeparation * sin(ang))));
double rightSteeringJointAngle =
atan((2.0 * this->wheelBase * sin(ang)) / \
((2.0 * this->wheelBase * cos(ang)) - \
((2.0 * this->wheelBase * cos(ang)) + \
(1.0 * this->wheelSeparation * sin(ang))));

auto leftSteeringPos = _ecm.Component<components::JointPosition>(
Expand Down
26 changes: 4 additions & 22 deletions src/systems/diff_drive/DiffDrive.cc
Original file line number Diff line number Diff line change
Expand Up @@ -459,17 +459,8 @@ void DiffDrive::PreUpdate(const UpdateInfo &_info,
continue;

// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(
joint, components::JointVelocityCmd({this->dataPtr->leftJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->leftJointSpeed});
}

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

// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->rightJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->rightJointSpeed});
}

// Create the left and right side joint position components if they
Expand Down
11 changes: 11 additions & 0 deletions src/systems/dvl/DopplerVelocityLogSystem.cc
Original file line number Diff line number Diff line change
Expand Up @@ -220,6 +220,9 @@ class gz::sim::systems::DopplerVelocityLogSystem::Implementation
/// \brief Current simulation time.
public: std::chrono::steady_clock::duration nextUpdateTime{
std::chrono::steady_clock::duration::max()};

/// \brief Mutex to protect current simulation times
public: std::mutex timeMutex;
};

using namespace gz;
Expand Down Expand Up @@ -351,6 +354,8 @@ void DopplerVelocityLogSystem::Implementation::DoPostUpdate(
return true;
});

std::lock_guard<std::mutex> timeLock(this->timeMutex);

if (!this->perStepRequests.empty() || (
!_info.paused && this->nextUpdateTime <= _info.simTime))
{
Expand Down Expand Up @@ -611,6 +616,9 @@ void DopplerVelocityLogSystem::Implementation::OnRender()
}

auto closestUpdateTime = std::chrono::steady_clock::duration::max();

std::lock_guard<std::mutex> timeLock(this->timeMutex);

for (const auto & [_, sensorId] : this->sensorIdPerEntity)
{
gz::sensors::Sensor *sensor =
Expand All @@ -635,6 +643,9 @@ void DopplerVelocityLogSystem::Implementation::OnRender()
void DopplerVelocityLogSystem::Implementation::OnPostRender()
{
GZ_PROFILE("DopplerVelocityLogSystem::Implementation::OnPostRender");

std::lock_guard<std::mutex> timeLock(this->timeMutex);

for (const auto & sensorId : this->updatedSensorIds)
{
auto *sensor =
Expand Down
12 changes: 1 addition & 11 deletions src/systems/joint_controller/JointController.cc
Original file line number Diff line number Diff line change
Expand Up @@ -344,17 +344,7 @@ void JointController::PreUpdate(const UpdateInfo &_info,
// Update joint velocity
for (Entity joint : this->dataPtr->jointEntities)
{
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(
joint, components::JointVelocityCmd({targetVel}));
}
else
{
*vel = components::JointVelocityCmd({targetVel});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint, {targetVel});
}
}
}
Expand Down
12 changes: 1 addition & 11 deletions src/systems/joint_position_controller/JointPositionController.cc
Original file line number Diff line number Diff line change
Expand Up @@ -461,17 +461,7 @@ void JointPositionController::PreUpdate(
for (Entity joint : this->dataPtr->jointEntities)
{
// Update velocity command.
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(
joint, components::JointVelocityCmd({targetVel}));
}
else
{
*vel = components::JointVelocityCmd({targetVel});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint, {targetVel});
}
return;
}
Expand Down
53 changes: 8 additions & 45 deletions src/systems/mecanum_drive/MecanumDrive.cc
Original file line number Diff line number Diff line change
Expand Up @@ -426,66 +426,29 @@ void MecanumDrive::PreUpdate(const gz::sim::UpdateInfo &_info,
for (Entity joint : this->dataPtr->frontLeftJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->frontLeftJointSpeed});
}

for (Entity joint : this->dataPtr->frontRightJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed}));
}
else
{
*vel =
components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->frontRightJointSpeed});
}

for (Entity joint : this->dataPtr->backLeftJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->backLeftJointSpeed});
}

for (Entity joint : this->dataPtr->backRightJoints)
{
// Update wheel velocity
auto vel = _ecm.Component<components::JointVelocityCmd>(joint);

if (vel == nullptr)
{
_ecm.CreateComponent(joint,
components::JointVelocityCmd({this->dataPtr->backRightJointSpeed}));
}
else
{
*vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed});
}
_ecm.SetComponentData<components::JointVelocityCmd>(joint,
{this->dataPtr->backRightJointSpeed});
}
}

Expand Down
14 changes: 2 additions & 12 deletions src/systems/thruster/Thruster.cc
Original file line number Diff line number Diff line change
Expand Up @@ -740,18 +740,8 @@ void Thruster::PreUpdate(
// Velocity control
else
{
auto velocityComp =
_ecm.Component<gz::sim::components::JointVelocityCmd>(
this->dataPtr->jointEntity);
if (velocityComp == nullptr)
{
_ecm.CreateComponent(this->dataPtr->jointEntity,
components::JointVelocityCmd({desiredPropellerAngVel}));
}
else
{
velocityComp->Data()[0] = desiredPropellerAngVel;
}
_ecm.SetComponentData<gz::sim::components::JointVelocityCmd>(
this->dataPtr->jointEntity, {desiredPropellerAngVel});
angvel.set_data(desiredPropellerAngVel);
}

Expand Down
Loading

0 comments on commit 301832d

Please sign in to comment.