Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Empty file.
Original file line number Diff line number Diff line change
Expand Up @@ -8,24 +8,25 @@

#pragma once

#include <SimulationInterfaces/ROS2SimulationInterfacesTypeIds.h>
#include "ROS2SimulationInterfacesTypeIds.h"

#include <AzCore/EBus/EBus.h>
#include <AzCore/Interface/Interface.h>
#include <AzCore/std/containers/unordered_set.h>

namespace ROS2SimulationInterfaces
{
using SimulationFeatureType = uint8_t;
class ROS2SimulationInterfacesRequests
{
public:
AZ_RTTI(ROS2SimulationInterfacesRequests, ROS2SimulationInterfacesRequestBusTypeId);
virtual ~ROS2SimulationInterfacesRequests() = default;

//! Returns set of simulation features available in ROS2SimulationInterfaces Gem
//! SimulationFeatures follows definition available at:
//! SimulationFeatureType follows definition available at:
//! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg
virtual AZStd::unordered_set<AZ::u8> GetSimulationFeatures() = 0;
virtual AZStd::unordered_set<SimulationFeatureType> GetSimulationFeatures() = 0;
};

class ROS2SimulationInterfacesRequestBusTraits : public AZ::EBusTraits
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,18 +13,18 @@ namespace SimulationInterfaces
{
//! Result codes to be used in the Result message
//! @see <a href="https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/Result.msg">Result.msg</a>
using ErrorCodeValue = uint8_t;
using ErrorCodeType = uint8_t;

//! A message type to represent the result of a failed operation
struct FailedResult
{
FailedResult() = default;
FailedResult(ErrorCodeValue error_code, const AZStd::string& error_string)
: error_code(error_code)
, error_string(error_string)
FailedResult(ErrorCodeType errorCode, const AZStd::string& errorString)
: m_errorCode(errorCode)
, m_errorString(errorString)
{
}
ErrorCodeValue error_code;
AZStd::string error_string;
ErrorCodeType m_errorCode;
AZStd::string m_errorString;
};
} // namespace SimulationInterfaces
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@

#pragma once

#include <SimulationInterfaces/SimulationInterfacesTypeIds.h>
#include "SimulationInterfacesTypeIds.h"

#include "Result.h"
#include "TagFilter.h"
Expand All @@ -19,37 +19,41 @@

namespace SimulationInterfaces
{
//! # A set of filters to apply to entity queries. See GetEntities, GetEntitiesStates.
//! A set of filters to apply to entity queries. See GetEntities, GetEntitiesStates.
//! @see <a href="https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/EntityFilters.msg">EntityFilters.msg</a>
struct EntityFilters
{
AZStd::string m_filter; //! A posix regular expression to match against entity names
TagFilter m_tags_filter; //! A filter to match against entity tags
//! A posix regular expression to match against entity names,
//! The regular expression syntax is POSIX Extended,
//! @see <a href="https://pubs.opengroup.org/onlinepubs/9799919799">POSIX_Extended</a> definitions
AZStd::string m_nameFilter;
TagFilter m_tagsFilter; //! A filter to match against entity tags
AZStd::shared_ptr<Physics::ShapeConfiguration>
m_bounds_shape; //! A shape to use for filtering entities, null means no bounds filtering
AZ::Transform m_bounds_pose{ AZ::Transform::CreateIdentity() };
m_boundsShape; //! A shape to use for filtering entities, null means no bounds filtering
AZ::Transform m_boundsPose{ AZ::Transform::CreateIdentity() };
};

//! @see <a href="https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/EntityState.msg">EntityState.msg</a>
struct EntityState
{
AZ::Transform m_pose; //! The pose of the entity
AZ::Vector3 m_twist_linear; //! The linear velocity of the entity (in the entity frame)
AZ::Vector3 m_twist_angular; //! The angular velocity of the entity (in the entity frame)
AZ::Vector3 m_twistLinear; //! The linear velocity of the entity (in the entity frame)
AZ::Vector3 m_twistAngular; //! The angular velocity of the entity (in the entity frame)
};

struct Spawnable
{
AZStd::string m_uri;
AZStd::string m_description;
AZStd::string m_bounds_sphere;
AZStd::string m_boundsSphere;
};

using EntityNameList = AZStd::vector<AZStd::string>;
using MultipleEntitiesStates = AZStd::unordered_map<AZStd::string, EntityState>;
using SpawnableList = AZStd::vector<Spawnable>;
using DeletionCompletedCb = AZStd::function<void(const AZ::Outcome<void, FailedResult>&)>;
using SpawnCompletedCb = AZStd::function<void(const AZ::Outcome<AZStd::string, FailedResult>&)>;

class SimulationEntityManagerRequests
{
public:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,15 +8,15 @@

#pragma once

#include <SimulationInterfaces/SimulationInterfacesTypeIds.h>
#include "SimulationInterfacesTypeIds.h"

#include <AzCore/EBus/EBus.h>
#include <AzCore/Interface/Interface.h>
#include <AzCore/std/containers/unordered_set.h>

namespace SimulationInterfaces
{
using SimulationFeatures = uint8_t;
using SimulationFeatureType = uint8_t;

class SimulationFeaturesAggregatorRequests
{
Expand All @@ -25,15 +25,15 @@ namespace SimulationInterfaces
virtual ~SimulationFeaturesAggregatorRequests() = default;

//! Registers simulation features defined by caller
virtual void AddSimulationFeatures(const AZStd::unordered_set<SimulationFeatures>& features) = 0;
virtual void AddSimulationFeatures(const AZStd::unordered_set<SimulationFeatureType>& features) = 0;

//! Returns features available in the simulator, list follows definitions at
//! @see https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/SimulatorFeatures.msg
virtual const AZStd::unordered_set<SimulationFeatures> GetSimulationFeatures() const = 0;
virtual AZStd::unordered_set<SimulationFeatureType> GetSimulationFeatures() = 0;

//! Method checks if feature with given id is available in the simulation
//! Method is extension to standard defined in simulation_interfaces
virtual bool HasFeature(SimulationFeatures feature) const = 0;
virtual bool HasFeature(SimulationFeatureType feature) = 0;
};

class SimulationFeaturesAggregatorRequestBusTraits : public AZ::EBusTraits
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,25 +8,27 @@

#pragma once

#include <SimulationInterfaces/SimulationInterfacesTypeIds.h>
#include "SimulationInterfacesTypeIds.h"

#include "Result.h"
#include <AzCore/EBus/EBus.h>
#include <AzCore/Interface/Interface.h>
#include <AzCore/std/smart_ptr/shared_ptr.h>
#include <AzFramework/Physics/ShapeConfiguration.h>
#include <SimulationInterfaces/Result.h>

namespace SimulationInterfaces
{
using SimulationState = uint8_t;

class SimulationManagerRequests
{
public:
AZ_RTTI(SimulationManagerRequests, SimulationManagerRequestsTypeId);
virtual ~SimulationManagerRequests() = default;

//! Reload level and removal of all spawned simulation entities.
using ReloadLevelCallback = AZStd::function<void(void)>;

//! Reload level and removal of all spawned simulation entities.
virtual void ReloadLevel(ReloadLevelCallback completionCallback) = 0;

//! Set the simulation to paused or unpaused,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,14 +15,10 @@ namespace SimulationInterfaces
{
//! Structure to design a filter for tags
//! @see <a href="https://github.com/ros-simulation/simulation_interfaces/blob/main/msg/TagsFilter.msg">TagsFilter.msg</a>
using TagFilterMode = uint8_t;

struct TagFilter
{
enum class TagFilterMode
{
FILTER_MODE_ANY,
FILTER_MODE_ALL
};
AZStd::unordered_set<AZStd::string> m_tags;
TagFilterMode m_mode;
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@
#include <AzCore/std/functional.h>
#include <Interfaces/IROS2HandlerBase.h>
#include <Utils/RegistryUtils.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <simulation_interfaces/msg/simulator_features.hpp>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -28,4 +28,4 @@ namespace SimulationInterfaces::Utils
}
return {};
}
} // namespace SimulationInterfaces::Utils
} // namespace SimulationInterfaces::Utils
Original file line number Diff line number Diff line change
Expand Up @@ -17,4 +17,4 @@ namespace SimulationInterfaces::Utils
AZStd::string RelPathToUri(AZStd::string_view relPath);
AZStd::string UriToRelPath(AZStd::string_view relPath);

} // namespace SimulationInterfaces::Utils
} // namespace SimulationInterfaces::Utils
64 changes: 26 additions & 38 deletions Gems/SimulationInterfaces/Code/Source/Clients/ConsoleCommands.inl
Original file line number Diff line number Diff line change
Expand Up @@ -20,10 +20,11 @@ namespace SimulationInterfacesCommands
static void simulationinterfaces_GetEntities(const AZ::ConsoleCommandContainer& arguments)
{
AZ::Outcome<EntityNameList, FailedResult> entities;
SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters());
SimulationEntityManagerRequestBus::BroadcastResult(
entities, &SimulationEntityManagerRequestBus::Events::GetEntities, EntityFilters());
if (!entities.IsSuccess())
{
AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().error_string.c_str());
AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().m_errorString.c_str());
return;
}

Expand Down Expand Up @@ -67,13 +68,13 @@ namespace SimulationInterfacesCommands
AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntities in radius %f \n", sphereShape);
AZ_Printf("SimulationInterfacesConsole", "position %f %f %f \n", position.GetX(), position.GetY(), position.GetZ());
EntityFilters filter;
filter.m_bounds_shape = AZStd::make_shared<Physics::SphereShapeConfiguration>(sphereShape);
filter.m_boundsShape = AZStd::make_shared<Physics::SphereShapeConfiguration>(sphereShape);

AZ::Outcome<EntityNameList, FailedResult> entities;
SimulationEntityManagerRequestBus::BroadcastResult(entities, &SimulationEntityManagerRequestBus::Events::GetEntities, filter);
if (!entities.IsSuccess())
{
AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().error_string.c_str());
AZ_Printf("SimulationInterfacesConsole", "Failed to get entities: %s\n", entities.GetError().m_errorString.c_str());
return;
}

Expand All @@ -93,33 +94,23 @@ namespace SimulationInterfacesCommands
const AZStd::string entityName = arguments[0];
AZ_Printf("SimulationInterfacesConsole", "simulationinterfaces_GetEntityState %s\n", entityName.c_str());
AZ::Outcome<EntityState, FailedResult> entityStateResult;
SimulationEntityManagerRequestBus::BroadcastResult(entityStateResult, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName);
SimulationEntityManagerRequestBus::BroadcastResult(
entityStateResult, &SimulationEntityManagerRequestBus::Events::GetEntityState, entityName);
if (!entityStateResult.IsSuccess())
{
AZ_Printf("SimulationInterfacesConsole", "Failed to get entity state: %s\n", entityStateResult.GetError().error_string.c_str());
AZ_Printf(
"SimulationInterfacesConsole", "Failed to get entity state: %s\n", entityStateResult.GetError().m_errorString.c_str());
return;
}
const auto &entityState = entityStateResult.GetValue();
const auto& entityState = entityStateResult.GetValue();
AZ_Printf("SimulationInterfacesConsole", "Entity %s\n", entityName.c_str());
AZ_Printf(
"SimulationInterfacesConsole",
"Pose %s\n",
AZ::Vector3ToString(entityState.m_pose.GetTranslation()).c_str());
AZ_Printf(
"SimulationInterfacesConsole",
"Rotation %s \n",
AZ::QuaternionToString(entityState.m_pose.GetRotation()).c_str());
AZ_Printf("SimulationInterfacesConsole", "Pose %s\n", AZ::Vector3ToString(entityState.m_pose.GetTranslation()).c_str());
AZ_Printf("SimulationInterfacesConsole", "Rotation %s \n", AZ::QuaternionToString(entityState.m_pose.GetRotation()).c_str());

const AZ::Vector3 euler = entityState.m_pose.GetRotation().GetEulerDegrees();
AZ_Printf("SimulationInterfacesConsole", "Rotation (euler) %s\n", AZ::Vector3ToString(euler).c_str());
AZ_Printf(
"SimulationInterfacesConsole",
"Twist Linear %s\n",
AZ::Vector3ToString(entityState.m_twist_linear).c_str());
AZ_Printf(
"SimulationInterfacesConsole",
"Twist Angular %s\n",
AZ::Vector3ToString(entityState.m_twist_angular).c_str());
AZ_Printf("SimulationInterfacesConsole", "Twist Linear %s\n", AZ::Vector3ToString(entityState.m_twistLinear).c_str());
AZ_Printf("SimulationInterfacesConsole", "Twist Angular %s\n", AZ::Vector3ToString(entityState.m_twistAngular).c_str());
}

static void simulationinterfaces_SetStateXYZ(const AZ::ConsoleCommandContainer& arguments)
Expand All @@ -143,11 +134,10 @@ namespace SimulationInterfacesCommands

if (!result.IsSuccess())
{
AZ_Printf("SimulationInterfacesConsole", "Failed to set entity state: %s\n", result.GetError().error_string.c_str());
AZ_Printf("SimulationInterfacesConsole", "Failed to set entity state: %s\n", result.GetError().m_errorString.c_str());
return;
}
AZ_Printf("SimulationInterfacesConsole", "Entity %s state set\n", entityName.c_str());

}

static void simulationinterfaces_DeleteEntity(const AZ::ConsoleCommandContainer& arguments)
Expand All @@ -166,11 +156,10 @@ namespace SimulationInterfacesCommands
}
else
{
AZ_Printf("SimulationInterfacesConsole", "Failed to delete entity: %s\n", result.GetError().error_string.c_str());
AZ_Printf("SimulationInterfacesConsole", "Failed to delete entity: %s\n", result.GetError().m_errorString.c_str());
}
};
SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteEntity, entityName, cb);

}

static void simulationinterfaces_GetSpawnables(const AZ::ConsoleCommandContainer& arguments)
Expand All @@ -180,7 +169,7 @@ namespace SimulationInterfacesCommands
SimulationEntityManagerRequestBus::BroadcastResult(spawnables, &SimulationEntityManagerRequestBus::Events::GetSpawnables);
if (!spawnables.IsSuccess())
{
AZ_Printf("SimulationInterfacesConsole", "Failed to get spawnables: %s\n", spawnables.GetError().error_string.c_str());
AZ_Printf("SimulationInterfacesConsole", "Failed to get spawnables: %s\n", spawnables.GetError().m_errorString.c_str());
return;
}
for (const auto& spawnable : spawnables.GetValue())
Expand All @@ -205,24 +194,23 @@ namespace SimulationInterfacesCommands
AZ::Transform initialPose = AZ::Transform::CreateIdentity();
if (arguments.size() > 5)
{
initialPose.SetTranslation(
AZ::Vector3(
AZStd::stof(AZStd::string(arguments[3])),
AZStd::stof(AZStd::string(arguments[4])),
AZStd::stof(AZStd::string(arguments[5]))));
initialPose.SetTranslation(AZ::Vector3(
AZStd::stof(AZStd::string(arguments[3])),
AZStd::stof(AZStd::string(arguments[4])),
AZStd::stof(AZStd::string(arguments[5]))));
}
SpawnCompletedCb completedCb = [](const AZ::Outcome<AZStd::string, FailedResult>& result)
{
if (!result.IsSuccess())
{
AZ_Printf("SimulationInterfacesConsole", "Failed to spawn entity: %s\n", result.GetError().error_string.c_str());
AZ_Printf("SimulationInterfacesConsole", "Failed to spawn entity: %s\n", result.GetError().m_errorString.c_str());
return;
}
AZ_Printf("SimulationInterfacesConsole", "Entity spawned and registered : %s\n", result.GetValue().c_str());

};
constexpr bool allowRename = true;
SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::SpawnEntity, name, uri, entityNamespace, initialPose, allowRename, completedCb);
SimulationEntityManagerRequestBus::Broadcast(
&SimulationEntityManagerRequestBus::Events::SpawnEntity, name, uri, entityNamespace, initialPose, allowRename, completedCb);
AZ_Printf("SimulationInterfacesConsole", "simulationinterface_Spawn %s %s\n", name.c_str(), uri.c_str());
}

Expand All @@ -245,7 +233,8 @@ namespace SimulationInterfacesCommands
}
else
{
AZ_Printf("SimulationInterfacesConsole", "Failed to delete all spawned entities: %s\n", result.GetError().error_string.c_str());
AZ_Printf(
"SimulationInterfacesConsole", "Failed to delete all spawned entities: %s\n", result.GetError().m_errorString.c_str());
}
};
SimulationEntityManagerRequestBus::Broadcast(&SimulationEntityManagerRequestBus::Events::DeleteAllEntities, cb);
Expand All @@ -258,7 +247,6 @@ namespace SimulationInterfacesCommands
AZ_CONSOLEFREEFUNC(simulationinterfaces_Resume, AZ::ConsoleFunctorFlags::DontReplicate, "Resume simulation.");
AZ_CONSOLEFREEFUNC(simulationinterfaces_Step, AZ::ConsoleFunctorFlags::DontReplicate, "Step simulation.");


AZ_CONSOLEFREEFUNC(
simulationinterfaces_GetEntitiesSphere, AZ::ConsoleFunctorFlags::DontReplicate, "Get all simulated entities in the radius.");
AZ_CONSOLEFREEFUNC(simulationinterfaces_GetEntityState, AZ::ConsoleFunctorFlags::DontReplicate, "Get state of the entity.");
Expand Down
Loading