diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index c1b2d5cb..8169462d 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -25,9 +25,11 @@ find_package(gz-sim REQUIRED) find_package(gz_transport_vendor REQUIRED) find_package(gz-transport REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rcpputils REQUIRED) find_package(ros_gz_interfaces REQUIRED) +find_package(simulation_interfaces REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -70,16 +72,42 @@ target_link_libraries(delete_entity ${PROJECT_NAME}_delete_entity ${builtin_interfaces_TARGETS} ${geometry_msgs_TARGETS} ${ros_gz_interfaces_TARGETS} + ${simulation_intefaces_TARGETS} rclcpp::rclcpp) target_include_directories(delete_entity PUBLIC ${CLI11_INCLUDE_DIRS}) +set(gz_simulation_interfaces_sources + src/gz_simulation_interfaces/gazebo_proxy.cpp + src/gz_simulation_interfaces/gz_entity_filters.cpp + src/gz_simulation_interfaces/gz_simulation_interfaces.cpp + src/gz_simulation_interfaces/utils.cpp + + # Actions + src/gz_simulation_interfaces/actions/simulate_steps.cpp + + # Services + src/gz_simulation_interfaces/services/delete_entity.cpp + src/gz_simulation_interfaces/services/get_entities.cpp + src/gz_simulation_interfaces/services/get_entities_states.cpp + src/gz_simulation_interfaces/services/get_entity_info.cpp + src/gz_simulation_interfaces/services/get_entity_state.cpp + src/gz_simulation_interfaces/services/get_simulation_state.cpp + src/gz_simulation_interfaces/services/get_simulator_features.cpp + src/gz_simulation_interfaces/services/reset_simulation.cpp + src/gz_simulation_interfaces/services/set_entity_state.cpp + src/gz_simulation_interfaces/services/set_simulation_state.cpp + src/gz_simulation_interfaces/services/spawn_entity.cpp + src/gz_simulation_interfaces/services/step_simulation.cpp +) + # gzserver_component -add_library(gzserver_component SHARED src/gzserver.cpp) -rclcpp_components_register_nodes(gzserver_component "ros_gz_sim::GzServer") +add_library(gzserver_component SHARED src/gzserver.cpp ${gz_simulation_interfaces_sources}) target_link_libraries(gzserver_component PUBLIC ${std_msgs_TARGETS} + ${simulation_interfaces_TARGETS} gz-sim::core rclcpp::rclcpp + rclcpp_action::rclcpp_action rclcpp_components::component rclcpp_components::component_manager ) @@ -294,6 +322,7 @@ if(BUILD_TESTING) add_launch_test(test/test_create_node.launch.py TIMEOUT 200) add_launch_test(test/test_remove_node.launch.py TIMEOUT 200) + add_launch_test(test/test_gz_simulation_interfaces.launch.py TIMEOUT 300) endif() ament_package() diff --git a/ros_gz_sim/include/ros_gz_sim/gz_simulation_interfaces.hpp b/ros_gz_sim/include/ros_gz_sim/gz_simulation_interfaces.hpp new file mode 100644 index 00000000..fbd03d58 --- /dev/null +++ b/ros_gz_sim/include/ros_gz_sim/gz_simulation_interfaces.hpp @@ -0,0 +1,40 @@ +// Copyright 2025 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS_GZ_SIM__GZ_SIMULATION_INTERFACES_HPP_ +#define ROS_GZ_SIM__GZ_SIMULATION_INTERFACES_HPP_ + +#include + +#include +#include + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +class GzSimulationInterfaces +{ +public: + // Class constructor. + explicit GzSimulationInterfaces(std::shared_ptr node); + +private: + /// \internal + /// \brief Private data pointer. + GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) +}; +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // ROS_GZ_SIM__GZ_SIMULATION_INTERFACES_HPP_ diff --git a/ros_gz_sim/package.xml b/ros_gz_sim/package.xml index 48e068bb..6b4d4d53 100644 --- a/ros_gz_sim/package.xml +++ b/ros_gz_sim/package.xml @@ -35,9 +35,11 @@ launch_ros libgflags-dev rclcpp + rclcpp_action rclcpp_components rcpputils ros_gz_interfaces + simulation_interfaces std_msgs tf2 tf2_ros @@ -48,6 +50,7 @@ launch_ros launch_testing launch_testing_ament_cmake + ros_gz_bridge ament_cmake diff --git a/ros_gz_sim/src/gz_simulation_interfaces/actions/simulate_steps.cpp b/ros_gz_sim/src/gz_simulation_interfaces/actions/simulate_steps.cpp new file mode 100644 index 00000000..73b86701 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/actions/simulate_steps.cpp @@ -0,0 +1,183 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "simulate_steps.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "../gazebo_proxy.hpp" +#include "simulation_interfaces/action/simulate_steps.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace actions +{ +using SimulateStepsAction = simulation_interfaces::action::SimulateSteps; +using GoalHandleSimulateSteps = rclcpp_action::ServerGoalHandle; + +SimulateSteps::SimulateSteps( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + const auto control_service = this->gz_proxy_->PrefixTopic("control"); + if (!this->gz_proxy_->WaitForGzService(control_service)) { + RCLCPP_ERROR_STREAM( + this->ros_node_->get_logger(), + "Gazebo service [" << control_service << "] is not available. " + << "The [SimulateSteps] interface will not function properly."); + } + auto goal_callback = + [](const rclcpp_action::GoalUUID &, std::shared_ptr) { + // TODO(azeey) Add console message that we've received the goal + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }; + + auto cancel_cb = + [](const std::shared_ptr) { + // TODO(azeey) Add console message that we've received the cancellation + return rclcpp_action::CancelResponse::ACCEPT; + }; + + auto accept_cb = + [this, control_service](const std::shared_ptr goal_handle) { + // Note that this is not the same as SimulateStepsAction::Result, which is the typename + // associated with the Result of the action, which in turn contains a + // simulation_interfaces::msg::Result + using Result = simulation_interfaces::msg::Result; + + // Adapted from https://github.com/ros-navigation/navigation2/blob/main/nav2_ros_common/include/nav2_ros_common/simple_action_server.hpp#L154 + if (this->worker_future_.valid() && ( + this->worker_future_.wait_for(std::chrono::milliseconds(0)) == + std::future_status::timeout)) + { + auto action_result = std::make_shared(); + action_result->result.result = Result::RESULT_OPERATION_FAILED; + action_result->result.error_message = "Another goal is already running"; + goal_handle->abort(action_result); + return; + } + + // Execute the task in a separate thread and return immediately. + this->worker_future_ = std::async(std::launch::async, [this, goal_handle, control_service] + { + const auto goal = goal_handle->get_goal(); + auto action_result = std::make_shared(); + + if (goal->steps > std::numeric_limits::max()) { + action_result->result.result = Result::RESULT_OPERATION_FAILED; + action_result->result.error_message = + "The requested number of steps exceeds the maximum supported value (max uint32)"; + goal_handle->abort(action_result); + return; + } + + if (!this->gz_proxy_->Paused()) { + action_result->result.result = Result::RESULT_OPERATION_FAILED; + action_result->result.error_message = "Simulation has to be paused before stepping"; + goal_handle->abort(action_result); + return; + } + + uint64_t num_iters_start = this->gz_proxy_->Iterations(); + + // TODO(azeey) Refactor this code since it's also used in the StepSimulation service. + gz::msgs::WorldControl gz_request; + gz_request.set_pause(true); + gz_request.set_step(true); + gz_request.set_multi_step(goal->steps); + bool gz_result; + gz::msgs::Boolean reply; + bool executed = this->gz_proxy_->GzNode()->Request( + control_service, gz_request, GazeboProxy::kGzServiceTimeoutMs, reply, gz_result); + if (!executed) { + action_result->result.result = Result::RESULT_OPERATION_FAILED; + action_result->result.error_message = "Timed out while trying to reset simulation"; + goal_handle->abort(action_result); + } else if (action_result && reply.data()) { + // The request has succeeded, so now we listen to the stats and provide feedback. + action_result->result.result = Result::RESULT_OK; + auto feedback = std::make_shared(); + + while (rclcpp::ok()) { + if (!this->gz_proxy_->AssertUpdatedWorldStats(action_result->result)) { + goal_handle->abort(action_result); + } + + auto iterations = this->gz_proxy_->Iterations(); + feedback->completed_steps = iterations - num_iters_start; + feedback->remaining_steps = goal->steps - feedback->completed_steps; + goal_handle->publish_feedback(feedback); + // TODO(azeey) There is a bug in Gazebo where the stepping field is set to true only + // once immediately after the request to step instead of being true for the whole + // duration of steps. So we can't use this right now to determine if we need to + // break out early + + // if (!this->world_stats_.stepping()) { + // break; + // } + if (feedback->remaining_steps == 0) { + break; + } + + if (goal_handle->is_canceling()) { + goal_handle->canceled(action_result); + return; + } + } + + if (rclcpp::ok() && goal_handle->is_active()) { + if (feedback->remaining_steps == 0) { + goal_handle->succeed(action_result); + } else { + action_result->result.result = Result::RESULT_OPERATION_FAILED; + action_result->result.error_message = "SimulateSteps was interrupted"; + goal_handle->abort(action_result); + } + } + } else { + action_result->result.result = Result::RESULT_OPERATION_FAILED; + action_result->result.error_message = + "Unknown error while trying to reset simulation"; + goal_handle->abort(action_result); + } + }); + }; + + // For some reason, create_server doesn't respect the sub_namespace of the node. + const auto action_name = ros_node->get_effective_namespace() + "/simulate_steps"; + this->action_handle_ = rclcpp_action::create_server( + ros_node, action_name, goal_callback, cancel_cb, accept_cb); + + RCLCPP_INFO_STREAM(ros_node->get_logger(), "Created action " << action_name); +} + +SimulateSteps::~SimulateSteps() +{ + if (this->worker_future_.valid()) { + this->worker_future_.wait(); + } +} +} // namespace actions +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/actions/simulate_steps.hpp b/ros_gz_sim/src/gz_simulation_interfaces/actions/simulate_steps.hpp new file mode 100644 index 00000000..1431d96c --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/actions/simulate_steps.hpp @@ -0,0 +1,52 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__ACTIONS__SIMULATE_STEPS_HPP_ +#define GZ_SIMULATION_INTERFACES__ACTIONS__SIMULATE_STEPS_HPP_ + +#include +#include + +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ + +namespace gz_simulation_interfaces +{ +class GazeboProxy; +namespace actions +{ + +/// \class SimulateSteps +/// \brief Implements the `simulation_interfaces/SimulateSteps` interface. +class SimulateSteps : public HandlerBase +{ +public: + // Documentation inherited + SimulateSteps(std::shared_ptr node, std::shared_ptr gz_proxy); + ~SimulateSteps(); + +private: + // TODO(azeey) Refactor into base class + std::shared_ptr action_handle_; + std::future worker_future_; +}; +} // namespace actions +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__ACTIONS__SIMULATE_STEPS_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/gazebo_proxy.cpp b/ros_gz_sim/src/gz_simulation_interfaces/gazebo_proxy.cpp new file mode 100644 index 00000000..54cc4521 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/gazebo_proxy.cpp @@ -0,0 +1,353 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gazebo_proxy.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace components = gz::sim::components; + +GazeboProxy::GazeboProxy(const std::string world_name, std::shared_ptr ros_node) +: world_name_(world_name), ros_node_(ros_node), gz_node_(std::make_shared()) +{ + if (!this->InitializeGazeboConnection()) { + throw std::runtime_error("Could not initialize Gazebo connection"); + } + + if (!this->WaitForCriticalServices()) { + RCLCPP_ERROR( + ros_node->get_logger(), + "Critical services from Gazebo are not available. ROS Simulation Interfaces will not " + "function properly."); + } + // TODO(azeey) Consider adding the UserCommands system if not already present. + // TODO(azeey) Wait for critical services to be available (e.g. /world/*/create, + // /world/*/control) Request the initial state of the world. This will block until Gazebo is + // initialized + gz::msgs::SerializedStepMap reply; + bool result; + if (!this->gz_node_->Request(this->PrefixTopic("state"), kGzServiceTimeoutMs, reply, result)) { + RCLCPP_ERROR( + ros_node->get_logger(), + "Simulation interface timed out while waiting for Gazebo to initialize"); + return; + } else { + if (!result) { + RCLCPP_ERROR( + ros_node->get_logger(), + "Simulation interface encountered an error while synchronizing state with Gazebo"); + return; + } else { + this->UpdateStateFromMsg(reply); + this->state_intialized_ = true; + + // Listen to the "state" topic to get periodic updates. + this->SubscribeToGzTopic(this->PrefixTopic("state"), &GazeboProxy::UpdateStateFromMsg, this); + // Listen to the "scene/info" to detect a reset + // TODO(azeey): This is a hack. We currently don't have a nice way of determining when + // simulation has been reset if it's currently paused. Checking if time has been rewound or + // the number of iterations was reset back to zero doesn't work if the simulation was started + // in the paused state and hasn't been played before it was reset. The SceneBroadacaster + // publishes on the scene/info topic every time it's reset. So we'll use that as our signal + // until we come up with a better way to communicate this from the server. + std::function resetHandler = [this](const auto &) { + { + std::lock_guard lk(this->reset_detected_mutex_); + this->reset_detected_ = true; + this->reset_detected_cv_.notify_all(); + } + // Use std::async since InitializeAllCanonicalLinks eventually makes a service call, which + // we don't want to do from this callback thread. + this->initialize_canonical_links_ = + std::async(std::launch::async, [this] {this->InitializeAllCanonicalLinks();}); + }; + + this->SubscribeToGzTopic(this->PrefixTopic("scene/info"), resetHandler); + } + + std::function updateStats = + [this](const auto & stats) + { + std::lock_guard lk(this->world_stats_sync_mutex_); + this->world_stats_ = stats; + this->world_stats_updated_ = true; + this->world_stats_cv_.notify_all(); + }; + + // Listen to the stats topic to get more frequently updates world statistics. + this->SubscribeToGzTopic(this->PrefixTopic("stats"), updateStats); + } + + // Before creating the services, we need to add the `[Angular/Linear]Velocity` components to all + // the entities available. Currently, we're treating entities are models, but Gazebo doesn't + // update velocity components of models. Therefore, we have to set the component on the canonical + // link and compute the velocity of the model entity manually here. + // TODO(azeey): Compute model velocities on the Gazebo server. + this->InitializeAllCanonicalLinks(); +} + +std::string GazeboProxy::PrefixTopic(const char * topic) const +{ + return "world/" + this->world_name_ + "/" + topic; +} + +bool GazeboProxy::WaitForGzService( + const std::string & service, const std::chrono::milliseconds & timeout) +{ + std::vector publishers; + const auto start_time = std::chrono::system_clock::now(); + bool found_service = false; + while (true) { + this->GzNode()->ServiceInfo(service, publishers); + if (!publishers.empty()) { + found_service = true; + break; + } + const auto now = std::chrono::system_clock::now(); + if (now > start_time + timeout) {break;} + + using namespace std::chrono_literals; // NOLINT + std::this_thread::sleep_for(500ms); + } + return found_service; +} +uint64_t GazeboProxy::Iterations() const +{ + std::lock_guard lk(this->world_stats_sync_mutex_); + return this->world_stats_.iterations(); +} + +bool GazeboProxy::Paused() const +{ + std::lock_guard lk(this->world_stats_sync_mutex_); + return this->world_stats_.paused(); +} + +gz::msgs::WorldStatistics GazeboProxy::Stats() const +{ + std::lock_guard lk(this->world_stats_sync_mutex_); + return this->world_stats_; +} + +void GazeboProxy::WithEcm(std::function f) +{ + std::lock_guard lk(this->state_sync_mutex_); + f(this->ecm_); +} + +std::shared_ptr GazeboProxy::GzNode() {return this->gz_node_;} + +bool GazeboProxy::StateInitialized() const {return this->state_intialized_;} + +bool GazeboProxy::WaitForUpdatedState(const std::chrono::milliseconds & timeout) +{ + // If the state has not been initialized, it will not be continuously updated, so return early. + if (!state_intialized_) { + return false; + } + if (this->initialize_canonical_links_.valid()) { + // Wait if canonical links need to be initialized. This should only happen on reset. + this->initialize_canonical_links_.wait_for(timeout); + } + // TODO(azeey): Technically we should subtract the amount of time the `wait_for` above used up + // from the timeout when we use it for the second `wait_for` below. + std::unique_lock lk(this->state_sync_mutex_); + this->state_updated_ = false; + return this->state_cv_.wait_for(lk, timeout, [this] {return this->state_updated_;}); +} + +bool GazeboProxy::AssertUpdatedState(simulation_interfaces::msg::Result & result) +{ + using simulation_interfaces::msg::Result; + if (!this->StateInitialized()) { + result.result = Result::RESULT_OPERATION_FAILED; + result.error_message = "Required Gazebo system is missing"; + return false; + } + if (!this->WaitForUpdatedState()) { + result.result = Result::RESULT_OPERATION_FAILED; + result.error_message = "Timed out while waiting for updated Gazebo state"; + return false; + } + return true; +} + +bool GazeboProxy::AssertUpdatedWorldStats(simulation_interfaces::msg::Result & result) +{ + using simulation_interfaces::msg::Result; + std::unique_lock lk(this->world_stats_sync_mutex_); + this->world_stats_updated_ = false; + auto rc = this->world_stats_cv_.wait_for( + lk, std::chrono::milliseconds(kGzStateUpdatedTimeoutMs), + [this] {return this->world_stats_updated_;}); + if (!rc) { + result.result = Result::RESULT_OPERATION_FAILED; + result.error_message = "Timed out while waiting for updated Gazebo world stats"; + return false; + } + return true; +} + +bool GazeboProxy::WaitForResetDetected() +{ + std::unique_lock lk(this->reset_detected_mutex_); + this->reset_detected_ = false; + if(!this->reset_detected_cv_.wait_for( + lk, std::chrono::milliseconds(kGzServiceTimeoutMs), [this] {return this->reset_detected_;})) + { + return false; + } + return this->reset_detected_; +} + +bool GazeboProxy::InitializeGazeboConnection() +{ + gz::msgs::StringMsg_V worlds_msg; + bool result; + if (this->gz_node_->Request( + "gazebo/worlds", GazeboProxy::kGzServiceTimeoutMs, worlds_msg, result)) + { + if (result && !worlds_msg.data().empty()) { + this->world_name_ = worlds_msg.data(0); + return true; + } + } + return false; +} + +bool GazeboProxy::WaitForCriticalServices() +{ + bool have_all_services = true; + // Check that services from SceneBroadacaster are available + { + const auto state_service = this->PrefixTopic("state"); + if (!this->WaitForGzService(state_service)) { + RCLCPP_ERROR_STREAM( + this->ros_node_->get_logger(), + "Required Gazebo service [" + << state_service << "] is not available. " + << "Make sure the [SceneBroadacaster] system is loaded in your Gazebo world"); + have_all_services = false; + } + } + + { + const auto control_service = this->PrefixTopic("control/state"); + if (!this->WaitForGzService(control_service)) { + RCLCPP_ERROR_STREAM( + this->ros_node_->get_logger(), + "Gazebo service [" << control_service << "] is not available."); + + have_all_services = false; + } + } + + return have_all_services; +} + +void GazeboProxy::UpdateStateFromMsg(const gz::msgs::SerializedStepMap & msg) +{ + { + std::lock_guard lk(this->state_sync_mutex_); + this->ecm_.SetState(msg.state()); + + this->HandleNewEntities(); + this->ecm_.ClearRemovedComponents(); + this->ecm_.ClearNewlyCreatedEntities(); + this->ecm_.ProcessRemoveEntityRequests(); + this->state_updated_ = true; + } + this->state_cv_.notify_all(); +} +void GazeboProxy::HandleNewEntities() +{ + std::unordered_set canonicalLinkEntities; + this->ecm_.EachNew( + [&canonicalLinkEntities](const gz::sim::Entity & entity, const components::CanonicalLink *) { + canonicalLinkEntities.insert(entity); + return true; + }); + + if (canonicalLinkEntities.empty()) { + return; + } + + this->InitializeCanonicalLinks(canonicalLinkEntities); +} + +void GazeboProxy::InitializeAllCanonicalLinks() +{ + RCLCPP_INFO(this->ros_node_->get_logger(), "InitializeCanonicalLinks"); + std::vector c_links; + { + std::lock_guard lk(this->state_sync_mutex_); + c_links = this->ecm_.EntitiesByComponents(components::CanonicalLink()); + } + this->InitializeCanonicalLinks({c_links.begin(), c_links.end()}); +} + +void GazeboProxy::InitializeCanonicalLinks( + const std::unordered_set & canonicalLinkEntities) +{ + // TODO(azeey) Computing velocities at every timestep might have a performance impact + for (const auto & link : canonicalLinkEntities) { + this->ecm_.CreateComponent(link, components::WorldPose()); + this->ecm_.CreateComponent(link, components::WorldLinearVelocity()); + this->ecm_.CreateComponent(link, components::WorldAngularVelocity()); + } + + gz::msgs::WorldControlState control_msg; + control_msg.mutable_state()->CopyFrom(this->ecm_.State( + canonicalLinkEntities, {components::WorldPose::typeId, components::WorldLinearVelocity::typeId, + components::WorldAngularVelocity::typeId})); + + bool result{false}; + gz::msgs::Boolean controlReply; + this->gz_node_->Request( + this->PrefixTopic("control/state"), control_msg, GazeboProxy::kGzServiceTimeoutMs, controlReply, + result); + if (!result || !controlReply.data()) { + RCLCPP_ERROR_THROTTLE( + this->ros_node_->get_logger(), *this->ros_node_->get_clock(), 1000, + "Simulation interface encountered an error while handling newly created entities"); + return; + } +} +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/gazebo_proxy.hpp b/ros_gz_sim/src/gz_simulation_interfaces/gazebo_proxy.hpp new file mode 100644 index 00000000..6cb64f47 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/gazebo_proxy.hpp @@ -0,0 +1,249 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__GAZEBO_PROXY_HPP_ +#define GZ_SIMULATION_INTERFACES__GAZEBO_PROXY_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +/// \class GazeboProxy +/// \brief Class that serves as a proxy to the Gazebo server by providing local caches of essential +/// data or state. It provides: +/// - A local EntityComponentManager (ECM) that is synchronized with the server +/// - A WorldStatistics object that is updated regularly +/// The class provides access to these objects in a thread-safe manner. +/// Additionally, the class has convenience functions that are used by most of the Simulation +/// Interface services/actions. +class GazeboProxy +{ +public: + /// \brief Constructor. + /// \param[in] world_name The name of the Gazebo world that is being simulated. + /// \param[in] ros_node ROS node. + GazeboProxy(const std::string world_name, std::shared_ptr ros_node); + + /// \brief Prefixes a given topic with the world name. + /// \param[in] topic The topic to be prefixed. + /// \return The prefixed topic. e.g. Given "create" as the topic and the world name is "shapes", + /// this will return "/world/shapes/create". + std::string PrefixTopic(const char * topic) const; + + /// \brief Wait for a Gazebo service. + /// \param[in] service Full name of the service. + /// \param[in] timeout Amount of time to wait before giving up. + /// \return True if the service was found before a timeout occurred. + bool WaitForGzService( + const std::string & service, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds(kGzServiceTimeoutMs)); + + /// \brief Get the number of iterations so far. + /// \return Number of iterations executed by Gazebo. + /// \WARNING Calling this function within a callback of the WithEcm will cause a deadlock. + uint64_t Iterations() const; + + /// \brief Get whether simulation is in a paused state. + /// \return Paused state. + /// \WARNING Calling this function within a callback of the WithEcm will cause a deadlock. + bool Paused() const; + + /// \brief Get a copy of the World statistics message. + /// \return WorldStatistics message. + gz::msgs::WorldStatistics Stats() const; + + /// \brief Call a provided function with the EntityComponentManager as the only argument. + /// + /// This function locks a mutex before calling the provided function. + /// + /// \param[in] f Callback function with the EntityComponentManager as the argument. + /// + /// \note The callback function should not call \ref Stats as that function is also thread + /// synchronized with the same mutex. + void WithEcm(std::function f); + + /// \brief Returns the gz::transport Node + std::shared_ptr GzNode(); + + /// \brief Check whether the internal state (the local copy of the ECM) has been initialized. + /// + /// The internal state is supposed to be initialized the first time the class is instantiated, + /// however, if the necessary services are not available, it will fail and this function will + /// return false. + /// + /// \return True if the state has been initialized. + bool StateInitialized() const; + + /// \brief Asserts that the local state (ECM, WorldStatistics) has been updated. + /// + /// This waits until a new state message is received with a short timeout period + /// (kGzServiceTimeoutMs) + /// \param[out] result Populates the result object with an error code and message if a timeout + /// occurred. + /// \return True if the state has been updated before a timeout occurred. + bool AssertUpdatedState(simulation_interfaces::msg::Result & result); + + /// \brief Asserts that the local state (ECM, WorldStatistics) has been updated. + /// + /// This waits until a new world stats message is received with a short timeout period + /// (kGzServiceTimeoutMs) + /// \param[out] result Populates the result object with an error code and message if a timeout + /// occurred. + /// \return True if the world stats has been updated before a timeout occurred. + bool AssertUpdatedWorldStats(simulation_interfaces::msg::Result & result); + + /// \brief Wait until simulation reset is detected + /// \return True if reset was detected. + bool WaitForResetDetected(); + + /// \brief Amount of time to wait for a Gazebo service to become available. + static constexpr unsigned int kGzServiceTimeoutMs{5000}; + + /// \brief Amount of time to wait to receive the latest state update. + static constexpr unsigned int kGzStateUpdatedTimeoutMs{1000}; + +private: + /// \brief Establish initial connection to the Gazebo server. + /// + /// This makes the initial service request to Gazebo to receive the name of the world being + /// simulated. It assumes that there is only one world being simulated. If there are multiple + /// worlds, it will pick the first. + /// + /// \return True if the service request succeeded. + bool InitializeGazeboConnection(); + + /// \brief Wait for critical Gazebo services to become available. + /// \return True if all the required Gazebo services become available before the service timeout + /// occurred. + bool WaitForCriticalServices(); + + /// \brief Wait for updated state (ECM, WorldStatistics). + /// \param[in] timeout Amount of time to wait before giving up. + /// \return True if the state was updated before a timeout occurred. + bool WaitForUpdatedState( + const std::chrono::milliseconds & timeout = std::chrono::milliseconds( + kGzStateUpdatedTimeoutMs)); + + /// \brief Callback for receiving SerializedStepMap + /// + /// This updates the local ECM and WorldStatistics objects. + /// \param[in] msg SerializedStepMap message + void UpdateStateFromMsg(const gz::msgs::SerializedStepMap & msg); + + /// \brief Handle newly created entities + /// For example, this is responsible for creating necessary ECM Components on newly created + /// entities, so that the Physics system can populate them. + void HandleNewEntities(); + + /// \brief Initialize all canonical link entities by creating necessary ECM components on them. + void InitializeAllCanonicalLinks(); + + /// \brief Initialize canonical link entities by creating necessary ECM components on them. + /// \param[in] canonicalLinkEntities A set of canonical links to initialize. + void InitializeCanonicalLinks(const std::unordered_set & canonicalLinkEntities); + + /// \brief Subscribe to a Gazebo topic and store the subscription handler so that topics are + /// automatically unsubscribed during destruction. + /// \tparam[in] Args Arguments to be passed to gz::transport::Node::CreateSubscriber + /// \param[in] topic Subscription topic + template + void SubscribeToGzTopic(const std::string & topic, Args &&... args) + { + auto subscription = this->gz_node_->CreateSubscriber(topic, std::forward(args)...); + if (!subscription) { + RCLCPP_ERROR_STREAM( + this->ros_node_->get_logger(), "Subscribing to the topic [" << topic << "] failed"); + } else { + this->gz_subscribers[topic] = std::move(subscription); + } + } + + /// \brief Name of Gazebo world being simulated + std::string world_name_; + + /// \brief ROS node + std::shared_ptr ros_node_; + + /// \brief Gazebo Node + std::shared_ptr gz_node_; + + /// \brief Mutex used to synchronize access to ecm_ and state_updated_. + mutable std::mutex state_sync_mutex_; + + /// \brief Local copy of the EntityComponentManager synchronized with the server. + gz::sim::EntityComponentManager ecm_; + + /// \brief Local copy of the latest WorldStatistics message + gz::msgs::WorldStatistics world_stats_; + + /// \brief Conditional variable used for waiting on the updated world_stats message. + std::condition_variable world_stats_cv_; + + /// \brief Used as predicate for waiting on the latest state update with a conditional variable. + bool state_updated_{false}; + + /// \brief Mutex used to synchronize access to world_stats_, and world_stats_updated_. + mutable std::mutex world_stats_sync_mutex_; + + /// \brief Used as predicate for waiting on the latest world stats update with a conditional + /// variable. + bool world_stats_updated_{false}; + + /// \brief Conditional variable used for waiting on the latest state update. + std::condition_variable state_cv_; + + /// \brief Records whether the state has been initialized when this class was first instantiated. + bool state_intialized_{false}; + + /// \brief Holds the future returned by a std::async call made when a sim reset occurs. + std::future initialize_canonical_links_; + + /// \brief Whether simulation reset was detected + bool reset_detected_{false}; + + /// \brief Mutex to synchronize access to reset_detected_ + mutable std::mutex reset_detected_mutex_; + + /// \brief Conditional variable used for waiting on reset detected. + std::condition_variable reset_detected_cv_; + + /// \brief gz-transport subscription handles + /// \TODO(azeey): Using a std::map here instead of a std::vector due to a bug in gz-transport + /// See https://github.com/gazebosim/gz-transport/issues/717 + std::map gz_subscribers; +}; +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__GAZEBO_PROXY_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/gz_entity_filters.cpp b/ros_gz_sim/src/gz_simulation_interfaces/gz_entity_filters.cpp new file mode 100644 index 00000000..dbdc3d6b --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/gz_entity_filters.cpp @@ -0,0 +1,101 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gz_entity_filters.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "simulation_interfaces/msg/entity_category.hpp" +#include "simulation_interfaces/msg/entity_filters.hpp" +#include "simulation_interfaces/msg/result.hpp" +#include "simulation_interfaces/msg/tags_filter.hpp" +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +using simulation_interfaces::msg::EntityCategory; +using simulation_interfaces::msg::Result; +using simulation_interfaces::msg::TagsFilter; + +GzEntityFilters::GzEntityFilters( + const simulation_interfaces::msg::EntityFilters & filters, + const gz::sim::EntityComponentManager & ecm) +: filters_(filters), ecm_(ecm), regex_filter_(filters.filter, std::regex::extended) +{ + const auto & tags_filter_mode = filters_.tags.filter_mode; + if (tags_filter_mode != TagsFilter::FILTER_MODE_ANY && + tags_filter_mode != TagsFilter::FILTER_MODE_ALL) + { + throw std::runtime_error( + "The tag filter mode needs to be one of [FILTER_MODE_ANY, FILTER_MODE_ALL]"); + } +} + +std::tuple GzEntityFilters::ApplyFilter( + const gz::sim::Entity & entity, const std::string entity_name) +{ + namespace components = gz::sim::components; + Result result; + + result.result = Result::RESULT_OK; + if (!filters_.filter.empty() && !std::regex_match(entity_name, regex_filter_)) { + return {false, result}; + } + + const std::vector & test_categories = filters_.categories; + if (!test_categories.empty()) { + EntityCategory entity_category; + entity_category.category = ecm_.ComponentData(entity).value_or( + EntityCategory::CATEGORY_OBJECT); + auto it = std::find(test_categories.begin(), test_categories.end(), entity_category); + if (it == test_categories.end()) { + return {false, result}; + } + } + const auto & test_tags = filters_.tags.tags; + if (!test_tags.empty()) { + const auto entity_tags = ecm_.ComponentData(entity).value_or( + components::SemanticTags::Type{}); + auto are_in_entity_tags = + [&entity_tags](auto tag) { + return std::find(entity_tags.begin(), entity_tags.end(), tag) != entity_tags.end(); + }; + + if (filters_.tags.filter_mode == TagsFilter::FILTER_MODE_ANY) { + if (!std::any_of(test_tags.begin(), test_tags.end(), are_in_entity_tags)) { + return {false, result}; + } + } else if (filters_.tags.filter_mode == TagsFilter::FILTER_MODE_ALL) { + if (!std::all_of(test_tags.begin(), test_tags.end(), are_in_entity_tags)) { + return {false, result}; + } + } + } + // TODO(azeey) Implement filtering by bounds + return {true, result}; +} +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/gz_entity_filters.hpp b/ros_gz_sim/src/gz_simulation_interfaces/gz_entity_filters.hpp new file mode 100644 index 00000000..26c14005 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/gz_entity_filters.hpp @@ -0,0 +1,67 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__GZ_ENTITY_FILTERS_HPP_ +#define GZ_SIMULATION_INTERFACES__GZ_ENTITY_FILTERS_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "simulation_interfaces/msg/entity_filters.hpp" +#include "simulation_interfaces/msg/result.hpp" +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +/// \class GzEntityFilters +/// \brief Implements the various ways in which entities can be filtered in services such as +/// `GetEntities`. +class GzEntityFilters +{ +public: + /// \brief Constructor. + /// \param[in] filters Filters to apply. + /// \param[in] ecm Reference to the EntityComponentManager. + GzEntityFilters( + const simulation_interfaces::msg::EntityFilters & filters, + const gz::sim::EntityComponentManager & ecm); + + /// \brief Checks whether the given entity matches the filter. + /// \param[in] entity ID of the entity in the EntityComponentManager. + /// \param[in] entity_name Name of the entity. + /// + /// \note The fact that both `entity` and `entity_name` are needed is an optimization. Having both + /// avoid duplicate calls to fetch the ID or name. + std::tuple ApplyFilter( + const gz::sim::Entity & entity, const std::string entity_name); + +private: + /// \brief Filters to apply. + simulation_interfaces::msg::EntityFilters filters_; + /// \brief Reference to the EntityComponentManager. + const gz::sim::EntityComponentManager & ecm_; + /// \brief Regular expression object created from the contents of `filters_`. + std::regex regex_filter_; +}; +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__GZ_ENTITY_FILTERS_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/gz_simulation_interfaces.cpp b/ros_gz_sim/src/gz_simulation_interfaces/gz_simulation_interfaces.cpp new file mode 100644 index 00000000..dd35ba1f --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/gz_simulation_interfaces.cpp @@ -0,0 +1,134 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ros_gz_sim/gz_simulation_interfaces.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "actions/simulate_steps.hpp" +#include "gazebo_proxy.hpp" +#include "handler_base.hpp" +#include "services/delete_entity.hpp" +#include "services/get_entities.hpp" +#include "services/get_entities_states.hpp" +#include "services/get_entity_info.hpp" +#include "services/get_entity_state.hpp" +#include "services/get_simulation_state.hpp" +#include "services/get_simulator_features.hpp" +#include "services/reset_simulation.hpp" +#include "services/set_entity_state.hpp" +#include "services/set_simulation_state.hpp" +#include "services/spawn_entity.hpp" +#include "services/step_simulation.hpp" + +namespace components = gz::sim::components; + +namespace ros_gz_sim +{ + +namespace gz_simulation_interfaces +{ +class GzSimulationInterfaces::Implementation +{ +public: + explicit Implementation(std::shared_ptr node); + + void Run(); + void UpdateStateFromMsg(const gz::msgs::SerializedStepMap & msg); + void CreateInterfaces(); + + template + void AddInterface(); + + bool InitializeGazeboParameters(); + +private: + std::shared_ptr ros_node_; + std::string world_name_; + std::shared_ptr gz_proxy_; + std::vector> sim_interface_handles_; +}; + +GzSimulationInterfaces::Implementation::Implementation(std::shared_ptr node) +: ros_node_(node) +{ +} + +void GzSimulationInterfaces::Implementation::Run() +{ + auto thread = std::thread([&] { + try { + this->gz_proxy_ = std::make_shared(this->world_name_, this->ros_node_); + this->CreateInterfaces(); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(this->ros_node_->get_logger(), e.what()); + } + }); + + thread.detach(); +} + +void GzSimulationInterfaces::Implementation::CreateInterfaces() +{ + RCLCPP_INFO_STREAM( + this->ros_node_->get_logger(), "Creating services on " << this->ros_node_->get_name()); + + this->AddInterface(); + this->AddInterface(); + this->AddInterface(); + this->AddInterface(); + this->AddInterface(); + this->AddInterface(); + this->AddInterface(); + this->AddInterface(); + this->AddInterface(); + this->AddInterface(); + this->AddInterface(); + this->AddInterface(); + this->AddInterface(); +} + +template +void GzSimulationInterfaces::Implementation::AddInterface() +{ + this->sim_interface_handles_.push_back( + std::make_unique(this->ros_node_, this->gz_proxy_)); +} + +GzSimulationInterfaces::GzSimulationInterfaces(std::shared_ptr node) +: dataPtr(gz::utils::MakeUniqueImpl(node)) +{ + this->dataPtr->Run(); +} +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/handler_base.hpp b/ros_gz_sim/src/gz_simulation_interfaces/handler_base.hpp new file mode 100644 index 00000000..433266a2 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/handler_base.hpp @@ -0,0 +1,58 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__HANDLER_BASE_HPP_ +#define GZ_SIMULATION_INTERFACES__HANDLER_BASE_HPP_ + +#include + +#include +#include + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + + +/// \class HandlerBase +/// \brief Base class for Service Interface handlers +/// +/// This just holds the ROS and Gazebo nodes as well as the service handles +class HandlerBase +{ +public: + /// \brief Constructor + /// \param[in] ros_node ROS Node + /// \param[in] gz_proxy Gazebo Node + HandlerBase(std::shared_ptr ros_node, std::shared_ptr gz_proxy) + : ros_node_(ros_node), gz_proxy_(gz_proxy) + { + } + +protected: + /// \brief ROS Node + std::shared_ptr ros_node_; + + /// \brief Gazebo Node + std::shared_ptr gz_proxy_; + + /// \brief ROS Service handle for the service provided by the derived class. + std::shared_ptr services_handle_; +}; +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__HANDLER_BASE_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/delete_entity.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/delete_entity.cpp new file mode 100644 index 00000000..0b838d76 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/delete_entity.cpp @@ -0,0 +1,73 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "delete_entity.hpp" + +#include + +#include + +#include "../gazebo_proxy.hpp" +#include "simulation_interfaces/srv/delete_entity.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ +using DeleteEntitySrv = simulation_interfaces::srv::DeleteEntity; +using RequestPtr = DeleteEntitySrv::Request::ConstSharedPtr; +using ResponsePtr = DeleteEntitySrv::Response::SharedPtr; + +DeleteEntity::DeleteEntity( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + const auto remove_service = this->gz_proxy_->PrefixTopic("remove/blocking"); + if (!this->gz_proxy_->WaitForGzService(remove_service)) { + RCLCPP_ERROR_STREAM( + this->ros_node_->get_logger(), + "Gazebo service [" + << remove_service << "] is not available. " + << "The [DeleteEntity] interface will not function properly. To fix this, make " + "sure the [UserCommands] system is loaded in your Gazebo world"); + } + this->services_handle_ = ros_node->create_service( + "delete_entity", [this, remove_service](RequestPtr request, ResponsePtr response) { + gz::msgs::Entity gz_request; + gz_request.set_name(request->entity); + gz_request.set_type(gz::msgs::Entity::MODEL); + gz::msgs::Boolean gz_reply; + bool result; + if (this->gz_proxy_->GzNode()->Request( + remove_service, gz_request, GazeboProxy::kGzServiceTimeoutMs, gz_reply, result)) + { + if (result && gz_reply.data()) { + response->result.result = simulation_interfaces::msg::Result::RESULT_OK; + return; + } + } + // TODO(azeey) Add specific error codes depending on what went wrong and add more thorough + // error messages. + response->result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Error while trying to remove entity"; + }); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/delete_entity.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/delete_entity.hpp new file mode 100644 index 00000000..8652b54a --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/delete_entity.hpp @@ -0,0 +1,45 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__DELETE_ENTITY_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__DELETE_ENTITY_HPP_ + +#include + +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ +/// \class DeleteEntity +/// \brief Implements the `simulation_interfaces/DeleteEntity` interface. +class DeleteEntity : public HandlerBase +{ +public: + // Documentation inherited + DeleteEntity(std::shared_ptr node, std::shared_ptr gz_proxy); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__DELETE_ENTITY_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities.cpp new file mode 100644 index 00000000..8c4bebb5 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities.cpp @@ -0,0 +1,103 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "get_entities.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../gazebo_proxy.hpp" +#include "../gz_entity_filters.hpp" + +namespace components = gz::sim::components; + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ +using GetEntitiesSrv = simulation_interfaces::srv::GetEntities; +using RequestPtr = GetEntitiesSrv::Request::ConstSharedPtr; +using ResponsePtr = GetEntitiesSrv::Response::SharedPtr; +using simulation_interfaces::msg::EntityCategory; +using simulation_interfaces::msg::Result; +using simulation_interfaces::msg::TagsFilter; + +GetEntities::GetEntities( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + auto service_cb = [this](RequestPtr request, ResponsePtr response) { + if (!this->gz_proxy_->AssertUpdatedState(response->result)) { + return; + } + this->gz_proxy_->WithEcm([&](const gz::sim::EntityComponentManager & ecm) { + try { + GzEntityFilters filters(request->filters, ecm); + ecm.Each( + [&]( + const gz::sim::Entity & entity, const components::Name * name, + const components::Model *, const components::ParentEntity * parent) { + // Check that this is a top level model + if (ecm.Component(parent->Data())) { + // This is a nested model which should not be included in the list of entities to + // return. + // TODO(azeey) It might be useful to allow nested models here when we enable + // setting their poses in Gazebo. + return true; + } + auto [isIncluded, filter_result] = filters.ApplyFilter(entity, name->Data()); + if (filter_result.result != Result::RESULT_OK) { + response->result = filter_result; + return false; + } + + if (isIncluded) { + response->entities.push_back(name->Data()); + } + return true; + }); + + response->result.result = Result::RESULT_OK; + } catch (const std::exception & e) { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = e.what(); + } + }); + }; + + this->services_handle_ = ros_node->create_service("get_entities", service_cb); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities.hpp new file mode 100644 index 00000000..7ed03977 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities.hpp @@ -0,0 +1,50 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITIES_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITIES_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ +/// \class GetEntities +/// \brief Implements the `simulation_interfaces/GetEntities` interface. +/// +/// Supports EntityFilters where entities can be filtered by name, category, or tags. +class GetEntities : public HandlerBase +{ +public: + // Documentation inherited + GetEntities(std::shared_ptr node, std::shared_ptr gz_proxy); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITIES_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities_states.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities_states.cpp new file mode 100644 index 00000000..1dd52b12 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities_states.cpp @@ -0,0 +1,109 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "get_entities_states.hpp" + +#include + +#include + +#include +#include +#include + +#include "../gazebo_proxy.hpp" +#include "../gz_entity_filters.hpp" +#include "get_entity_state.hpp" + +namespace components = gz::sim::components; + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ + +using GetEntitiesStatesSrv = simulation_interfaces::srv::GetEntitiesStates; +using RequestPtr = GetEntitiesStatesSrv::Request::ConstSharedPtr; +using ResponsePtr = GetEntitiesStatesSrv::Response::SharedPtr; + +using simulation_interfaces::msg::Result; + +GetEntitiesStates::GetEntitiesStates( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + auto service_cb = [this](RequestPtr request, ResponsePtr response) { + if (!this->gz_proxy_->AssertUpdatedState(response->result)) { + return; + } + const auto stats = this->gz_proxy_->Stats(); + this->gz_proxy_->WithEcm([&](const gz::sim::EntityComponentManager & ecm) { + try { + // Since we will be entering the `Each` loop, we start with RESULT_OK and override it + // with any non-okay result in the loop. The loop breaks by return false if any result + // other than RESULT_OK is encountered. + response->result.result = Result::RESULT_OK; + GzEntityFilters filters(request->filters, ecm); + ecm.Each( + [&]( + const gz::sim::Entity & entity, const components::Name * name, + const components::Model *, const components::ParentEntity * parent) { + // Check that this is a top level model + if (ecm.Component(parent->Data())) { + // This is a nested model which should not be included in the list of entities to + // return. + // TODO(azeey) It might be useful to allow nested models here when we enable + // setting their poses in Gazebo. + return true; + } + auto [isIncluded, filter_result] = filters.ApplyFilter(entity, name->Data()); + + if (filter_result.result != Result::RESULT_OK) { + response->result = filter_result; + return false; + } + + if (!isIncluded) { + return true; + } + + response->entities.push_back(name->Data()); + auto & state = response->states.emplace_back(); + auto state_result = GetEntityState::FromEcm(ecm, stats, entity, state); + + if (state_result.result != Result::RESULT_OK) { + response->result = state_result; + return false; + } + + return true; + }); + } catch (const std::exception & e) { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = e.what(); + } + }); + }; + + this->services_handle_ = + ros_node->create_service("get_entities_states", service_cb); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities_states.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities_states.hpp new file mode 100644 index 00000000..ee9834c9 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entities_states.hpp @@ -0,0 +1,46 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITIES_STATES_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITIES_STATES_HPP_ + +#include + +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ + +/// \class GetEntityStates +/// \brief Implements the `simulation_interfaces/GetEntityStates` interface. +class GetEntitiesStates : public HandlerBase +{ +public: + // Documentation inherited + GetEntitiesStates(std::shared_ptr node, std::shared_ptr gz_proxy); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITIES_STATES_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_info.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_info.cpp new file mode 100644 index 00000000..78c09db5 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_info.cpp @@ -0,0 +1,87 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "get_entity_info.hpp" + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include "../gazebo_proxy.hpp" +#include "simulation_interfaces/srv/get_entity_info.hpp" + +namespace components = gz::sim::components; + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ +using GetEntityInfoSrv = simulation_interfaces::srv::GetEntityInfo; +using RequestPtr = GetEntityInfoSrv::Request::ConstSharedPtr; +using ResponsePtr = GetEntityInfoSrv::Response::SharedPtr; +using simulation_interfaces::msg::Result; + +GetEntityInfo::GetEntityInfo( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + this->services_handle_ = ros_node->create_service( + "get_entity_info", [this](RequestPtr request, ResponsePtr response) { + if (!this->gz_proxy_->AssertUpdatedState(response->result)) { + return; + } + this->gz_proxy_->WithEcm([request, response](gz::sim::EntityComponentManager & ecm) { + auto entity = ecm.EntityByName(request->entity); + if (entity) { + auto category = ecm.ComponentData(*entity); + if (category) { + response->info.category.set__category(*category); + } + + auto description = ecm.ComponentData(*entity); + + if (description) { + response->info.description = *description; + } + + auto tags = ecm.ComponentData(*entity); + if (tags) { + response->info.tags = *tags; + } + + response->result.result = Result::RESULT_OK; + } else { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Specified entity was not found"; + } + }); + }); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_info.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_info.hpp new file mode 100644 index 00000000..6651d72d --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_info.hpp @@ -0,0 +1,45 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITY_INFO_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITY_INFO_HPP_ + +#include + +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ +/// \class GetEntityInfo +/// \brief Implements the `simulation_interfaces/GetEntityInfo` interface. +class GetEntityInfo : public HandlerBase +{ +public: + // Documentation inherited + GetEntityInfo(std::shared_ptr node, std::shared_ptr gz_proxy); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITY_INFO_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_state.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_state.cpp new file mode 100644 index 00000000..257561c1 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_state.cpp @@ -0,0 +1,135 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "get_entity_state.hpp" + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../gazebo_proxy.hpp" +#include "../utils.hpp" +#include "simulation_interfaces/srv/get_entity_state.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ +using GetEntityStateSrv = simulation_interfaces::srv::GetEntityState; +using RequestPtr = GetEntityStateSrv::Request::ConstSharedPtr; +using ResponsePtr = GetEntityStateSrv::Response::SharedPtr; + +using simulation_interfaces::msg::Result; +namespace components = gz::sim::components; + +GetEntityState::GetEntityState( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + auto service_cb = [this](RequestPtr request, ResponsePtr response) { + if (!this->gz_proxy_->AssertUpdatedState(response->result)) { + return; + } + auto stats = this->gz_proxy_->Stats(); + this->gz_proxy_->WithEcm([&](const auto & ecm) { + response->result = GetEntityState::FromEcm(ecm, stats, request->entity, response->state); + }); + }; + this->services_handle_ = + ros_node->create_service("get_entity_state", service_cb); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} + +Result GetEntityState::FromEcm( + const gz::sim::EntityComponentManager & ecm, const gz::msgs::WorldStatistics & stats, + const std::string & name, simulation_interfaces::msg::EntityState & state) +{ + Result result; + auto entity = ecm.EntityByName(name); + if (entity) { + return GetEntityState::FromEcm(ecm, stats, *entity, state); + } else { + result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED; + result.error_message = "Requested entity not found"; + return result; + } +} + +simulation_interfaces::msg::Result GetEntityState::FromEcm( + const gz::sim::EntityComponentManager & ecm, const gz::msgs::WorldStatistics & stats, + const gz::sim::Entity & entity, simulation_interfaces::msg::EntityState & state) +{ + Result result; + state.header.frame_id = "world"; + state.header.stamp = ConvertTime(stats.sim_time()); + // If the entity is a static model, we will only fill in the pose + gz::sim::Model model(entity); + if (model.Static(ecm)) { + auto pose = gz::sim::worldPose(entity, ecm); + ConvertPose(pose, state.pose); + result.result = simulation_interfaces::msg::Result::RESULT_OK; + return result; + } + // Find the canonical link that corresponds to this model + auto canonicalLinkEntity = model.CanonicalLink(ecm); + if (canonicalLinkEntity == gz::sim::kNullEntity) { + // TODO(azeey) Handle this error condition. Maybe a static model + result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED; + result.error_message = "Could not find canonical link"; + return result; + } + gz::sim::Link canonicalLink(canonicalLinkEntity); + // See https://drake.mit.edu/doxygen_cxx/group__multibody__quantities.html for notations + std::optional X_WL = canonicalLink.WorldPose(ecm); + auto X_LM = ecm.ComponentData(canonicalLinkEntity)->Inverse(); + std::optional v_WM = canonicalLink.WorldLinearVelocity(ecm, X_LM.Pos()); + std::optional w_WL = canonicalLink.WorldAngularVelocity(ecm); + + if (X_WL && v_WM && w_WL) { + // TODO(azeey) Add comments + // Find the pose of the model based on the pose of the pose of the canonical link. + // X_ML: pose of the canonical link w.r.t the model + auto X_WM = (*X_WL) * X_LM; + ConvertPose(X_WM, state.pose); + + ConvertVector3(*v_WM, state.twist.linear); + ConvertVector3(*w_WL, state.twist.angular); + result.result = simulation_interfaces::msg::Result::RESULT_OK; + } else { + result.result = simulation_interfaces::msg::Result::RESULT_OPERATION_FAILED; + // TODO(azeey) Fix error message + result.error_message = "WorldPose component not set on entity"; + } + return result; +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_state.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_state.hpp new file mode 100644 index 00000000..c90b42fd --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_entity_state.hpp @@ -0,0 +1,69 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITY_STATE_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITY_STATE_HPP_ + +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ +/// \class GetEntityState +/// \brief Implements the `simulation_interfaces/GetEntityState` interface. +class GetEntityState : public HandlerBase +{ +public: + // Documentation inherited + GetEntityState(std::shared_ptr node, std::shared_ptr gz_proxy); + + /// \brief Populate an EntityState object based on our internal state (ECM, WorldStatistics). + /// \param[in] ecm Reference to the EntityComponentManager. + /// \param[in] stats Reference to the local copy of the WorldStatistics message. + /// \param[in] name Name of the entity + /// \param[out] state State object to be populated. + static simulation_interfaces::msg::Result FromEcm( + const gz::sim::EntityComponentManager & ecm, const gz::msgs::WorldStatistics & stats, + const std::string & name, simulation_interfaces::msg::EntityState & state); + + /// \brief Populate an EntityState object based on our internal state (ECM, WorldStatistics). + /// \param[in] ecm Reference to the EntityComponentManager. + /// \param[in] stats Reference to the local copy of the WorldStatistics message. + /// \param[in] entity Entity ID of the entity whose state is fetched. + /// \param[out] state State object to be populated. + static simulation_interfaces::msg::Result FromEcm( + const gz::sim::EntityComponentManager & ecm, const gz::msgs::WorldStatistics & stats, + const gz::sim::Entity & entity, simulation_interfaces::msg::EntityState & state); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__GET_ENTITY_STATE_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulation_state.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulation_state.cpp new file mode 100644 index 00000000..42356809 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulation_state.cpp @@ -0,0 +1,63 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "get_simulation_state.hpp" + +#include + +#include + +#include "../gazebo_proxy.hpp" +#include "simulation_interfaces/srv/get_simulation_state.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ +using GetSimulationStateSrv = simulation_interfaces::srv::GetSimulationState; +using RequestPtr = GetSimulationStateSrv::Request::ConstSharedPtr; +using ResponsePtr = GetSimulationStateSrv::Response::SharedPtr; +using simulation_interfaces::msg::Result; + +GetSimulationState::GetSimulationState( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + this->services_handle_ = ros_node->create_service( + "get_simulation_state", [this](RequestPtr, ResponsePtr response) { + if (!this->gz_proxy_->AssertUpdatedState(response->result)) { + return; + } + if (this->gz_proxy_->Paused()) { + response->state.state = simulation_interfaces::msg::SimulationState::STATE_PAUSED; + if (this->gz_proxy_->Iterations() == 0) { + // The simulation is in its initial state after loading a world or being reset, which will + // assign to the STATE_STOPPED state + response->state.state = simulation_interfaces::msg::SimulationState::STATE_STOPPED; + } + } else { + response->state.state = simulation_interfaces::msg::SimulationState::STATE_PLAYING; + } + + response->result.result = Result::RESULT_OK; + }); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulation_state.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulation_state.hpp new file mode 100644 index 00000000..2de493ef --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulation_state.hpp @@ -0,0 +1,45 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__GET_SIMULATION_STATE_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__GET_SIMULATION_STATE_HPP_ + +#include + +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ +/// \class GetSimulationState +/// \brief Implements the `simulation_interfaces/GetSimulationState` interface. +class GetSimulationState : public HandlerBase +{ +public: + // Documentation inherited + GetSimulationState(std::shared_ptr node, std::shared_ptr gz_proxy); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__GET_SIMULATION_STATE_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulator_features.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulator_features.cpp new file mode 100644 index 00000000..298c65c9 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulator_features.cpp @@ -0,0 +1,76 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "get_simulator_features.hpp" + +#include + +#include + +#include "../gazebo_proxy.hpp" +#include "simulation_interfaces/srv/get_simulator_features.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ +using GetSimulatorFeaturesSrv = simulation_interfaces::srv::GetSimulatorFeatures; +using RequestPtr = GetSimulatorFeaturesSrv::Request::ConstSharedPtr; +using ResponsePtr = GetSimulatorFeaturesSrv::Response::SharedPtr; + +GetSimulatorFeatures::GetSimulatorFeatures( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + this->services_handle_ = ros_node->create_service( + "get_simulator_features", [](RequestPtr, ResponsePtr response) { + using SimulatorFeatures = simulation_interfaces::msg::SimulatorFeatures; + // clang-format off + response->features.features.assign({ + SimulatorFeatures::SPAWNING, + SimulatorFeatures::DELETING, + SimulatorFeatures::ENTITY_TAGS, + // SimulatorFeatures::ENTITY_BOUNDS, // TODO(azeey) + // SimulatorFeatures::ENTITY_BOUNDS_BOX, // TODO(azeey) + SimulatorFeatures::ENTITY_CATEGORIES, + SimulatorFeatures::SPAWNING_RESOURCE_STRING, + SimulatorFeatures::ENTITY_STATE_GETTING, + SimulatorFeatures::ENTITY_STATE_SETTING, + SimulatorFeatures::ENTITY_INFO_GETTING, + // SimulatorFeatures::ENTITY_INFO_SETTING, // TODO(azeey) + SimulatorFeatures::SIMULATION_RESET, + // SimulatorFeatures::SIMULATION_RESET_TIME, // TODO(azeey) + // SimulatorFeatures::SIMULATION_RESET_STATE, // TODO(azeey) + // SimulatorFeatures::SIMULATION_RESET_SPAWNED, // TODO(azeey) + SimulatorFeatures::SIMULATION_STATE_GETTING, + SimulatorFeatures::SIMULATION_STATE_SETTING, + SimulatorFeatures::SIMULATION_STATE_PAUSE, + SimulatorFeatures::STEP_SIMULATION_SINGLE, + SimulatorFeatures::STEP_SIMULATION_MULTIPLE, + SimulatorFeatures::STEP_SIMULATION_ACTION, + // clang-format on + }); + + response->features.spawn_formats.assign({"sdf", "urdf"}); + // TODO(azeey) Fill in custom_info + }); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulator_features.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulator_features.hpp new file mode 100644 index 00000000..cc4facac --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/get_simulator_features.hpp @@ -0,0 +1,45 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__GET_SIMULATOR_FEATURES_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__GET_SIMULATOR_FEATURES_HPP_ + +#include + +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ +/// \class GetSimulatorFeatures +/// \brief Implements the `simulation_interfaces/GetSimulatorFeatures` interface. +class GetSimulatorFeatures : public HandlerBase +{ +public: + // Documentation inherited + GetSimulatorFeatures(std::shared_ptr node, std::shared_ptr gz_proxy); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__GET_SIMULATOR_FEATURES_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/reset_simulation.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/reset_simulation.cpp new file mode 100644 index 00000000..2879f7ce --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/reset_simulation.cpp @@ -0,0 +1,97 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "reset_simulation.hpp" + +#include +#include + +#include +#include + +#include "../gazebo_proxy.hpp" +#include "simulation_interfaces/srv/reset_simulation.hpp" +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ +using ResetSimulationSrv = simulation_interfaces::srv::ResetSimulation; +using RequestPtr = ResetSimulationSrv::Request::ConstSharedPtr; +using ResponsePtr = ResetSimulationSrv::Response::SharedPtr; + +ResetSimulation::ResetSimulation( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + const auto control_service = this->gz_proxy_->PrefixTopic("control"); + if (!this->gz_proxy_->WaitForGzService(control_service)) { + RCLCPP_ERROR_STREAM( + this->ros_node_->get_logger(), + "Gazebo service [" << control_service << "] is not available. " + << "The [ResetSimulation] interface will not function properly."); + } + this->services_handle_ = ros_node->create_service( + "reset_simulation", [this, control_service](RequestPtr request, ResponsePtr response) { + auto reset_detected_future = std::async(std::launch::async, [this] + { + return this->gz_proxy_->WaitForResetDetected(); + }); + + using Result = simulation_interfaces::msg::Result; + if ( + request->scope != ResetSimulationSrv::Request::SCOPE_DEFAULT && + request->scope != ResetSimulationSrv::Request::SCOPE_ALL) + { + response->result.result = Result::RESULT_FEATURE_UNSUPPORTED; + response->result.error_message = + "Only reset scopes SCOPE_DEFAULT and SCOPE_ALL are supported"; + return; + } + + gz::msgs::WorldControl gz_request; + gz_request.set_pause(this->gz_proxy_->Paused()); + gz_request.mutable_reset()->set_all(true); + // TODO(azeey) Resetting only the time, state or spawned models is not supported yet in Gazebo + + bool result; + gz::msgs::Boolean reply; + bool executed = this->gz_proxy_->GzNode()->Request( + control_service, gz_request, GazeboProxy::kGzServiceTimeoutMs, reply, result); + if (!executed) { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Timed out while trying to reset simulation"; + } else if (result && reply.data()) { + response->result.result = Result::RESULT_OK; + } else { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Unknown error while trying to reset simulation"; + } + + // Since the "control" service is asynchronous, getting results from the service doesn't mean + // the request has taken effect. Therefore, we wait here until the desired state is reached or + // a timeout occurs. + if (!reset_detected_future.get()) { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Timed out while trying to reset simulation"; + } + }); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/reset_simulation.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/reset_simulation.hpp new file mode 100644 index 00000000..80a68704 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/reset_simulation.hpp @@ -0,0 +1,45 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__RESET_SIMULATION_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__RESET_SIMULATION_HPP_ + +#include + +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ +/// \class ResetSimulation +/// \brief Implements the `simulation_interfaces/ResetSimulation` interface. +class ResetSimulation : public HandlerBase +{ +public: + // Documentation inherited + ResetSimulation(std::shared_ptr node, std::shared_ptr gz_proxy); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__RESET_SIMULATION_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/set_entity_state.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/set_entity_state.cpp new file mode 100644 index 00000000..d78756fc --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/set_entity_state.cpp @@ -0,0 +1,119 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "set_entity_state.hpp" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "../gazebo_proxy.hpp" +#include "../utils.hpp" +#include "simulation_interfaces/srv/set_entity_state.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ +using SetEntityStateSrv = simulation_interfaces::srv::SetEntityState; +using RequestPtr = SetEntityStateSrv::Request::ConstSharedPtr; +using ResponsePtr = SetEntityStateSrv::Response::SharedPtr; + +using simulation_interfaces::msg::Result; +namespace components = gz::sim::components; + +SetEntityState::SetEntityState( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + const auto control_state_service = this->gz_proxy_->PrefixTopic("control/state"); + if (!this->gz_proxy_->WaitForGzService(control_state_service)) { + RCLCPP_ERROR_STREAM( + this->ros_node_->get_logger(), + "Gazebo service [" << control_state_service << "] is not available. " + << "The [SetEntityState] interface will not function properly."); + } + auto service_cb = [this, control_state_service](RequestPtr request, ResponsePtr response) { + if (!this->gz_proxy_->AssertUpdatedState(response->result)) { + return; + } + + gz::msgs::WorldControlState control_msg; + + this->gz_proxy_->WithEcm( + [&](gz::sim::EntityComponentManager & ecm) { + const auto entity = ecm.EntityByName(request->entity); + + // TODO(azeey) Handle frame semantics. For now we assume all commands are in the world + // frame. + + // Note that since there is no way to tell if a field has been set by the user, there's no + // setting just the pose or just the twist. They will both be set according to what's in + // the message. If not set by the user, the default values will be used. + if (entity) { + gz::sim::Model model(*entity); + model.SetWorldPoseCmd(ecm, ConvertPose(request->state.pose)); + if (!model.Static(ecm)) { + // Velocity components are expected to be in the body frame, so we'll need to + // transform them. + // TODO(azeey) Clarify whether the velocities are set in the new pose of the entity + auto entityWorldPose = gz::sim::worldPose(*entity, ecm); + auto linearVelCmdBody = + entityWorldPose.Rot().RotateVectorReverse(ConvertVector3( + request->state.twist.linear)); + auto angularVelCmdBody = + entityWorldPose.Rot().RotateVectorReverse(ConvertVector3( + request->state.twist.angular)); + + ecm.SetComponentData(*entity, linearVelCmdBody); + ecm.SetComponentData(*entity, angularVelCmdBody); + } + } else { + // TODO(azeey) Error + } + control_msg.mutable_state()->CopyFrom(ecm.State( + {*entity}, {components::WorldPoseCmd::typeId, components::LinearVelocityCmd::typeId, + components::AngularVelocityCmd::typeId})); + }); + + control_msg.mutable_world_control()->set_pause(this->gz_proxy_->Paused()); + bool result; + gz::msgs::Boolean reply; + this->gz_proxy_->GzNode()->Request(control_state_service, control_msg, + GazeboProxy::kGzServiceTimeoutMs, reply, result); + // TODO(azeey) Handle Error + response->result.result = simulation_interfaces::msg::Result::RESULT_OK; + // TODO(azeey) Wait for result? + }; + this->services_handle_ = + ros_node->create_service("set_entity_state", service_cb); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/set_entity_state.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/set_entity_state.hpp new file mode 100644 index 00000000..c0dbd6db --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/set_entity_state.hpp @@ -0,0 +1,49 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__SET_ENTITY_STATE_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__SET_ENTITY_STATE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ +/// \class SetEntityState +/// \brief Implements the `simulation_interfaces/SetEntityState` interface. +class SetEntityState : public HandlerBase +{ +public: + // Documentation inherited + SetEntityState(std::shared_ptr node, std::shared_ptr gz_proxy); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__SET_ENTITY_STATE_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/set_simulation_state.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/set_simulation_state.cpp new file mode 100644 index 00000000..39fd2eca --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/set_simulation_state.cpp @@ -0,0 +1,122 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "set_simulation_state.hpp" + +#include +#include + +#include +#include + +#include "../gazebo_proxy.hpp" +#include "simulation_interfaces/srv/set_simulation_state.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ +using SetSimulationStateSrv = simulation_interfaces::srv::SetSimulationState; +using RequestPtr = SetSimulationStateSrv::Request::ConstSharedPtr; +using ResponsePtr = SetSimulationStateSrv::Response::SharedPtr; + +SetSimulationState::SetSimulationState( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + const auto control_service = this->gz_proxy_->PrefixTopic("control"); + if (!this->gz_proxy_->WaitForGzService(control_service)) { + RCLCPP_ERROR_STREAM( + this->ros_node_->get_logger(), + "Gazebo service [" << control_service << "] is not available. " + << "The [SetSimulationState] interface will not function properly."); + } + this->services_handle_ = ros_node->create_service( + "set_simulation_state", [this, control_service](RequestPtr request, ResponsePtr response) { + using Result = simulation_interfaces::msg::Result; + using SimulationState = simulation_interfaces::msg::SimulationState; + + gz::msgs::WorldControl gz_request; + switch (request->state.state) { + case SimulationState::STATE_STOPPED: + gz_request.set_pause(true); + gz_request.mutable_reset()->set_all(true); + break; + case SimulationState::STATE_PAUSED: + gz_request.set_pause(true); + break; + case SimulationState::STATE_PLAYING: + gz_request.set_pause(false); + break; + default: + response->result.result = Result::RESULT_FEATURE_UNSUPPORTED; + response->result.error_message = + "Only the states [STATE_STOPPED, STATE_PAUSED, STATE_PLAYING] are supported"; + return; + } + + bool result; + gz::msgs::Boolean reply; + bool executed = this->gz_proxy_->GzNode()->Request( + control_service, gz_request, GazeboProxy::kGzServiceTimeoutMs, reply, result); + if (!executed) { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Timed out while trying to set simulation state"; + } else if (result && reply.data()) { + response->result.result = Result::RESULT_OK; + } else { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Unknown error while trying to reset simulation"; + } + + // Since the "control" service is asynchronous, getting results from the service doesn't mean + // the request has taken effect. Therefore, we wait here until the desired state is reached or + // a timeout occurs. + bool state_reached{false}; + auto t_init = std::chrono::steady_clock::now(); + auto timeout = std::chrono::milliseconds(GazeboProxy::kGzStateUpdatedTimeoutMs); + while (!state_reached && (std::chrono::steady_clock::now() - t_init) < timeout) { + this->gz_proxy_->AssertUpdatedState(response->result); + switch (request->state.state) { + case SimulationState::STATE_STOPPED: + if (this->gz_proxy_->Paused() && (this->gz_proxy_->Iterations() == 0)) { + state_reached = true; + } + break; + case SimulationState::STATE_PAUSED: + if (this->gz_proxy_->Paused()) { + state_reached = true; + } + break; + case SimulationState::STATE_PLAYING: + if (!this->gz_proxy_->Paused()) { + state_reached = true; + } + break; + } + } + if (!state_reached) { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Timed out while trying to reset simulation"; + } + }); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/set_simulation_state.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/set_simulation_state.hpp new file mode 100644 index 00000000..8a8fdf9f --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/set_simulation_state.hpp @@ -0,0 +1,45 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__SET_SIMULATION_STATE_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__SET_SIMULATION_STATE_HPP_ + +#include + +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ +/// \class SetSimulationState +/// \brief Implements the `simulation_interfaces/SetSimulationState` interface. +class SetSimulationState : public HandlerBase +{ +public: + // Documentation inherited + SetSimulationState(std::shared_ptr node, std::shared_ptr gz_proxy); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__SET_SIMULATION_STATE_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/spawn_entity.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/spawn_entity.cpp new file mode 100644 index 00000000..44298cba --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/spawn_entity.cpp @@ -0,0 +1,108 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "spawn_entity.hpp" + +#include +#include + +#include + +#include "../gazebo_proxy.hpp" +#include "simulation_interfaces/srv/spawn_entity.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ +using SpawnEntitySrv = simulation_interfaces::srv::SpawnEntity; +using RequestPtr = SpawnEntitySrv::Request::ConstSharedPtr; +using ResponsePtr = SpawnEntitySrv::Response::SharedPtr; + +SpawnEntity::SpawnEntity( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + const auto create_service = this->gz_proxy_->PrefixTopic("create/blocking"); + if (!this->gz_proxy_->WaitForGzService(create_service)) { + RCLCPP_ERROR_STREAM( + this->ros_node_->get_logger(), + "Gazebo service [" << create_service << "] is not available. " + << "The [SpawnEntity] interface will not function properly. To fix this, " + "make sure the [UserCommands] system is loaded in your Gazebo world"); + } + this->services_handle_ = ros_node->create_service( + "spawn_entity", [this, create_service](RequestPtr request, ResponsePtr response) { + using Result = simulation_interfaces::msg::Result; + gz::msgs::EntityFactory gz_request; + if (!request->name.empty()) { + gz_request.set_name(request->name); + } + gz_request.set_allow_renaming(request->allow_renaming); + const auto & resource = request->entity_resource; + + if (!resource.uri.empty()) { + // TODO(azeey) The `sdf_filename` field requires absolute paths to the file. + // Consider resolving the uri using the `/gazebo/resource_paths/resolve` + gz_request.set_sdf_filename(resource.uri); + } else if (!resource.resource_string.empty()) { + gz_request.set_sdf(resource.resource_string); + } else { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = + "One of the fields [uri] or [resource_string] must be specified"; + return; + } + + // TODO(azeey) Add support for entity_namespace + // TODO(azeey) Reuse code in ros_gz_bridge/convert/geometry_msgs + auto * pose = gz_request.mutable_pose(); + pose->mutable_position()->set_x(request->initial_pose.pose.position.x); + pose->mutable_position()->set_y(request->initial_pose.pose.position.y); + pose->mutable_position()->set_z(request->initial_pose.pose.position.z); + + pose->mutable_orientation()->set_x(request->initial_pose.pose.orientation.x); + pose->mutable_orientation()->set_y(request->initial_pose.pose.orientation.y); + pose->mutable_orientation()->set_z(request->initial_pose.pose.orientation.z); + pose->mutable_orientation()->set_w(request->initial_pose.pose.orientation.w); + + bool result; + gz::msgs::Boolean reply; + bool executed = this->gz_proxy_->GzNode()->Request( + create_service, gz_request, GazeboProxy::kGzServiceTimeoutMs, reply, result); + if (!executed) { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Timed out while trying to spawn entity"; + } else if (result && reply.data()) { + response->result.result = Result::RESULT_OK; + // TODO(azeey) Fetch the new name of the entity from our local ECM using `EachNew`. + // We'd have to make sure that the ECM has been updated at least once after the `create` + // request + response->entity_name = request->name; + } else { + // TODO(azeey) SpawnEntity has additional error codes to allow surfacing more informative + // error messages. However, the `create` service in UserCommands only returns a boolean. + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Unknown error while trying to spawn entity"; + } + }); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/spawn_entity.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/spawn_entity.hpp new file mode 100644 index 00000000..519d5ff5 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/spawn_entity.hpp @@ -0,0 +1,46 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__SPAWN_ENTITY_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__SPAWN_ENTITY_HPP_ + +#include + +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ +/// \class SpawnEntity +/// \brief Implements the `simulation_interfaces/SpawnEntity` interface. +/// Supports spawning entities from a given URI or string. +class SpawnEntity : public HandlerBase +{ +public: + // Documentation inherited + SpawnEntity(std::shared_ptr node, std::shared_ptr gz_proxy); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__SPAWN_ENTITY_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/step_simulation.cpp b/ros_gz_sim/src/gz_simulation_interfaces/services/step_simulation.cpp new file mode 100644 index 00000000..7505cd28 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/step_simulation.cpp @@ -0,0 +1,85 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "step_simulation.hpp" + +#include +#include + +#include +#include + +#include "../gazebo_proxy.hpp" +#include "simulation_interfaces/srv/step_simulation.hpp" +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +namespace services +{ +using StepSimulationSrv = simulation_interfaces::srv::StepSimulation; +using RequestPtr = StepSimulationSrv::Request::ConstSharedPtr; +using ResponsePtr = StepSimulationSrv::Response::SharedPtr; +using simulation_interfaces::msg::Result; + +StepSimulation::StepSimulation( + std::shared_ptr ros_node, std::shared_ptr gz_proxy) +: HandlerBase(ros_node, gz_proxy) +{ + const auto control_service = this->gz_proxy_->PrefixTopic("control"); + if (!this->gz_proxy_->WaitForGzService(control_service)) { + RCLCPP_ERROR_STREAM( + this->ros_node_->get_logger(), + "Gazebo service [" << control_service << "] is not available. " + << "The [StepSimulation] interface will not function properly."); + } + this->services_handle_ = ros_node->create_service( + "step_simulation", [this, control_service](RequestPtr request, ResponsePtr response) { + if (!this->gz_proxy_->AssertUpdatedState(response->result)) { + return; + } + // The spec uses a uint64, but the service provided by Gazebo uses a uint32 so we bail out if + // the requested number of steps cannot be represented properly. + if (request->steps > std::numeric_limits::max()) { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = + "The requested number of steps exceeds the maximum supported value (max uint32)"; + return; + } + + gz::msgs::WorldControl gz_request; + gz_request.set_pause(true); + gz_request.set_step(true); + gz_request.set_multi_step(request->steps); + bool result; + gz::msgs::Boolean reply; + bool executed = this->gz_proxy_->GzNode()->Request( + control_service, gz_request, GazeboProxy::kGzServiceTimeoutMs, reply, result); + if (!executed) { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Timed out while trying to step simulation"; + } else if (result && reply.data()) { + response->result.result = Result::RESULT_OK; + } else { + response->result.result = Result::RESULT_OPERATION_FAILED; + response->result.error_message = "Unknown error while trying to step simulation"; + } + }); + + RCLCPP_INFO_STREAM( + ros_node->get_logger(), "Created service " << this->services_handle_->get_service_name()); +} +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/services/step_simulation.hpp b/ros_gz_sim/src/gz_simulation_interfaces/services/step_simulation.hpp new file mode 100644 index 00000000..9b379543 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/services/step_simulation.hpp @@ -0,0 +1,45 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__SERVICES__STEP_SIMULATION_HPP_ +#define GZ_SIMULATION_INTERFACES__SERVICES__STEP_SIMULATION_HPP_ + +#include + +#include +#include + +#include "../handler_base.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +class GazeboProxy; + +namespace services +{ +/// \class StepSimulation +/// \brief Implements the `simulation_interfaces/StepSimulation` interface. +class StepSimulation : public HandlerBase +{ +public: + // Documentation inherited + StepSimulation(std::shared_ptr node, std::shared_ptr gz_proxy); +}; +} // namespace services +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__SERVICES__STEP_SIMULATION_HPP_ diff --git a/ros_gz_sim/src/gz_simulation_interfaces/utils.cpp b/ros_gz_sim/src/gz_simulation_interfaces/utils.cpp new file mode 100644 index 00000000..4a001720 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/utils.cpp @@ -0,0 +1,102 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "utils.hpp" + +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/vector3.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ + +void ConvertPose(const gz::math::Pose3d & gz_pose, geometry_msgs::msg::Pose & ros_pose) +{ + ros_pose.position.x = gz_pose.X(); + ros_pose.position.y = gz_pose.Y(); + ros_pose.position.z = gz_pose.Z(); + + ros_pose.orientation.x = gz_pose.Rot().X(); + ros_pose.orientation.y = gz_pose.Rot().Y(); + ros_pose.orientation.z = gz_pose.Rot().Z(); + ros_pose.orientation.w = gz_pose.Rot().W(); +} + +geometry_msgs::msg::Pose ConvertPose(const gz::math::Pose3d & gz_pose) +{ + geometry_msgs::msg::Pose ros_pose; + ConvertPose(gz_pose, ros_pose); + return ros_pose; +} + +void ConvertPose(const geometry_msgs::msg::Pose & ros_pose, gz::math::Pose3d & gz_pose) +{ + gz_pose.Pos().X() = ros_pose.position.x; + gz_pose.Pos().Y() = ros_pose.position.y; + gz_pose.Pos().Z() = ros_pose.position.z; + + gz_pose.Rot().X() = ros_pose.orientation.x; + gz_pose.Rot().Y() = ros_pose.orientation.y; + gz_pose.Rot().Z() = ros_pose.orientation.z; + gz_pose.Rot().W() = ros_pose.orientation.w; +} + +gz::math::Pose3d ConvertPose(const geometry_msgs::msg::Pose & ros_pose) +{ + gz::math::Pose3d gz_pose; + ConvertPose(ros_pose, gz_pose); + return gz_pose; +} + +void ConvertVector3(const gz::math::Vector3d & gz_v, geometry_msgs::msg::Vector3 & ros_v) +{ + ros_v.x = gz_v.X(); + ros_v.y = gz_v.Y(); + ros_v.z = gz_v.Z(); +} + +geometry_msgs::msg::Vector3 ConvertVector3(const gz::math::Vector3d & gz_v) +{ + geometry_msgs::msg::Vector3 ros_v; + ConvertVector3(gz_v, ros_v); + return ros_v; +} + +void ConvertVector3(const geometry_msgs::msg::Vector3 & ros_v, gz::math::Vector3d & gz_v) +{ + gz_v.X() = ros_v.x; + gz_v.Y() = ros_v.y; + gz_v.Z() = ros_v.z; +} + +gz::math::Vector3d ConvertVector3(const geometry_msgs::msg::Vector3 & ros_v) +{ + gz::math::Vector3d gz_v; + ConvertVector3(ros_v, gz_v); + return gz_v; +} + +builtin_interfaces::msg::Time ConvertTime(const gz::msgs::Time gz_time) +{ + builtin_interfaces::msg::Time ros_time; + ros_time.sec = gz_time.sec(); + ros_time.nanosec = gz_time.nsec(); + return ros_time; +} +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim diff --git a/ros_gz_sim/src/gz_simulation_interfaces/utils.hpp b/ros_gz_sim/src/gz_simulation_interfaces/utils.hpp new file mode 100644 index 00000000..14f2f055 --- /dev/null +++ b/ros_gz_sim/src/gz_simulation_interfaces/utils.hpp @@ -0,0 +1,77 @@ +// Copyright 2025 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GZ_SIMULATION_INTERFACES__UTILS_HPP_ +#define GZ_SIMULATION_INTERFACES__UTILS_HPP_ + +#include + +#include +#include + +#include "builtin_interfaces/msg/time.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/vector3.hpp" + +namespace ros_gz_sim +{ +namespace gz_simulation_interfaces +{ +/// \brief Convert a gz::math Pose to a geometry_msgs Pose +/// \param[in] gz_pose gz::math::Pose object +/// \param[out] ros_pose geometry_msgs::msg::Pose object to be filled in +void ConvertPose(const gz::math::Pose3d & gz_pose, geometry_msgs::msg::Pose & ros_pose); + +/// \brief Convert a gz::math Pose to a geometry_msgs Pose +/// \param[in] gz_pose gz::math::Pose object +/// \return Converted geometry_msgs::msg::Pose object +geometry_msgs::msg::Pose ConvertPose(const gz::math::Pose3d & gz_pose); + +/// \brief Convert a geometry_msgs Pose to a gz::math Pose +/// \param[in] ros_pose geometry_msgs Pose object +/// \param[out] gz_pose gz::math::Pose object to be filled in +void ConvertPose(const geometry_msgs::msg::Pose & ros_pose, gz::math::Pose3d & gz_pose); + +/// \brief Convert a geometry_msgs Pose to a gz::math Pose +/// \param[in] ros_pose geometry_msgs Pose object +/// \return Converted gz::math::Pose object +gz::math::Pose3d ConvertPose(const geometry_msgs::msg::Pose & ros_pose); + +/// \brief Convert a gz::math Vector3d to a geometry_msgs Vector3 +/// \param[in] gz_v gz::math::Vector3d object +/// \param[out] ros_v geometry_msgs::msg::Vector3 object to be filled in +void ConvertVector3(const gz::math::Vector3d & gz_v, geometry_msgs::msg::Vector3 & ros_v); + +/// \brief Convert a gz::math Vector3d to a geometry_msgs Vector3 +/// \param[in] gz_v gz::math::Vector3d object +/// \return Converted geometry_msgs::msg::Vector3 +geometry_msgs::msg::Vector3 ConvertVector3(const gz::math::Vector3d & gz_v); + +/// \brief Convert a geometry_msgs Vector3 to a gz::math Vector3 +/// \param[in] ros_v geometry_msgs Vector3 object +/// \param[out] gz_v gz::math::Vector3d object to be filled in +void ConvertVector3(const geometry_msgs::msg::Vector3 & ros_v, gz::math::Vector3d & gz_v); + +/// \brief Convert a ROS Vector3 to a Gazebo Vector3 +/// \param[in] ros_v geometry_msgs Vector3 object +/// \return Converted gz::math::Vector3d object +gz::math::Vector3d ConvertVector3(const geometry_msgs::msg::Vector3 & ros_v); + +/// \brief Convert Gazebo Time to ROS time +/// \param[in] gz_time gz::msgs::Time time +/// \return Converted ROS time +builtin_interfaces::msg::Time ConvertTime(const gz::msgs::Time gz_time); +} // namespace gz_simulation_interfaces +} // namespace ros_gz_sim +#endif // GZ_SIMULATION_INTERFACES__UTILS_HPP_ diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp index 31b1091d..c18cd5ab 100644 --- a/ros_gz_sim/src/gzserver.cpp +++ b/ros_gz_sim/src/gzserver.cpp @@ -15,14 +15,19 @@ #include "ros_gz_sim/gzserver.hpp" #include +#include +#include #include + #include #include -#include #include +#include #include +#include #include +#include "ros_gz_sim/gz_simulation_interfaces.hpp" namespace ros_gz_sim { @@ -33,11 +38,13 @@ class GzServer::Implementation public: std::thread thread; + std::unique_ptr sim_interfaces; }; GzServer::GzServer(const rclcpp::NodeOptions & options) : Node("gzserver", options), dataPtr(gz::utils::MakeUniqueImpl()) { + rclcpp::uninstall_signal_handlers(); this->dataPtr->thread = std::thread(std::bind(&GzServer::OnStart, this)); } @@ -55,7 +62,6 @@ void GzServer::OnStart() auto world_sdf_string = this->declare_parameter("world_sdf_string", ""); auto initial_sim_time = this->declare_parameter("initial_sim_time", 0.0); - gz::common::Console::SetVerbosity(4); gz::sim::ServerConfig server_config; @@ -64,17 +70,27 @@ void GzServer::OnStart() } else if (!world_sdf_string.empty()) { server_config.SetSdfString(world_sdf_string); } else { - RCLCPP_ERROR( - this->get_logger(), - "Must specify either 'world_sdf_file' or 'world_sdf_string'"); + RCLCPP_ERROR(this->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); rclcpp::shutdown(); return; } server_config.SetInitialSimTime(initial_sim_time); - gz::sim::Server server(server_config); - server.Run(true /*blocking*/, 0, false /*paused*/); + auto server = std::make_unique(server_config); + // TODO(azeey) Think about whether it makes sense to RunOnce paused here or wait for some + // critical services from Gazebo to become available before starting the ROS services + server->RunOnce(true); + // TODO(azeey) Allow disabling simulation interfaces + + this->dataPtr->sim_interfaces = + std::make_unique( + this->create_sub_node(this->get_name())); + server->Run(true /*blocking*/, 0, false /*paused*/); + server.reset(); + // Call shutdown before resetting sim_interfaces so that threads in sim_interfaces can gracefully + // exit before being destructed. rclcpp::shutdown(); + this->dataPtr->sim_interfaces.reset(); } } // namespace ros_gz_sim diff --git a/ros_gz_sim/test/sdf/gz_simulation_interfaces.sdf b/ros_gz_sim/test/sdf/gz_simulation_interfaces.sdf new file mode 100644 index 00000000..845c281d --- /dev/null +++ b/ros_gz_sim/test/sdf/gz_simulation_interfaces.sdf @@ -0,0 +1,378 @@ + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + STATIC_OBJECT + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + ROBOT + Food delivery mobile robot + mobile + diff_drive + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + left_wheel_joint + right_wheel_joint + 1.25 + 0.3 + 1 + -1 + 2 + -2 + 0.5 + -0.5 + 1 + -1 + + + + + + DYNAMIC_OBJECT + + 5 5 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + + 0 5 1 0 0 1 + + DYNAMIC_OBJECT + + + + 1.0 + + + + + 1 + + + + + + + + 1 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + + STATIC_OBJECT + + true + 15 -10 0.5 0 0 0 + + + + + 1 10 5 + + + + + + + + 1 10 5 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + + STATIC_OBJECT + + true + 5 -20 0.5 0 30 0 + + + + + 5 5 1 + + + + + + + + 5 5 1 + + + + 0 0.6 0 1 + + + + + + diff --git a/ros_gz_sim/test/test_gz_simulation_interfaces.launch.py b/ros_gz_sim/test/test_gz_simulation_interfaces.launch.py new file mode 100644 index 00000000..ae91db26 --- /dev/null +++ b/ros_gz_sim/test/test_gz_simulation_interfaces.launch.py @@ -0,0 +1,439 @@ +# Copyright 2025 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from pathlib import Path +import re +import time +from typing import Any +import unittest + +from geometry_msgs.msg import Twist +from launch import LaunchDescription +from launch.actions import SetEnvironmentVariable +from launch_ros.actions import Node +import launch_testing +from launch_testing.actions import ReadyToTest +from launch_testing.asserts import assertExitCodes +import rclpy +import rclpy.action +from simulation_interfaces.action import SimulateSteps +from simulation_interfaces.msg import EntityCategory, Result, SimulationState, SimulatorFeatures +import simulation_interfaces.srv as si + +# Match name used in launch files +GZ_SERVER_NODE_NAME = 'gz_server' + + +def generate_test_description(): + test_dir = Path(__file__).parent + test_sdf_file = str(test_dir / 'sdf' / 'gz_simulation_interfaces.sdf') + + server_node = Node( + package='ros_gz_sim', + executable='gzserver', + name=GZ_SERVER_NODE_NAME, + output='screen', + parameters=[{'world_sdf_file': test_sdf_file}], + ) + + bridge_node = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + emulate_tty=True, + parameters=[ + {'bridge_names': ['cmd_vel']}, + { + 'bridges': { + 'cmd_vel': { + 'ros_topic_name': '/cmd_vel', + 'gz_topic_name': '/model/vehicle/cmd_vel', + 'ros_type_name': 'geometry_msgs/msg/Twist', + 'gz_type_name': 'gz.msgs.Twist', + 'direction': 'ROS_TO_GZ', + } + } + }, + ], + ) + + return ( + LaunchDescription( + [ + # Keep gz-transport contained to localhost to make the test more deterministic + SetEnvironmentVariable(name='GZ_IP', value='127.0.0.1'), + server_node, + bridge_node, + ReadyToTest(), + ] + ), + locals(), + ) + + +class TestGzSimulationInterfaces(unittest.TestCase): + + @classmethod + def setUpClass(cls): + expected_gz_ip = '127.0.0.1' + gz_ip = os.environ['GZ_IP'] + if gz_ip != expected_gz_ip: + raise RuntimeError(f'GZ_IP is expected to be {expected_gz_ip}, but is set to {gz_ip}') + rclpy.init() + + @classmethod + def tearDownClass(cls): + if rclpy.ok(): + rclpy.shutdown() + + def setUp(self) -> None: + self.node = rclpy.create_node('test_interfaces') + + def tearDown(self) -> None: + self.node.destroy_node() + + # helpers + + def setup_client(self, srv_type, srv_name): + client = self.node.create_client( + srv_type, + f'{GZ_SERVER_NODE_NAME}/{srv_name}') + self.assertTrue(client.wait_for_service(timeout_sec=5)) + return client, srv_type.Request() + + def call_and_spin(self, client, request, timeout_sec=5) -> Any: + future = client.call_async(request) + rclpy.spin_until_future_complete( + self.node, future, timeout_sec=timeout_sec) + self.assertTrue(future.done()) + return future.result() + + def assert_result_ok(self, response) -> None: + self.assertIsNotNone(response) + self.assertEqual( + response.result.result, + Result.RESULT_OK, + msg=response.result.error_message) + self.assertEqual(response.result.error_message, '') + + def get_simulation_state(self) -> si.GetSimulationState.Response: + get_simulation_state, request = self.setup_client( + si.GetSimulationState, 'get_simulation_state') + response = self.call_and_spin(get_simulation_state, request) + self.assert_result_ok(response) + return response + + def set_simulation_state(self, simulation_state) -> None: + set_simulation_state, request = self.setup_client( + si.SetSimulationState, 'set_simulation_state') + request.state.state = simulation_state + response = self.call_and_spin(set_simulation_state, request) + self.assert_result_ok(response) + + def delete_entity(self, entity_name) -> None: + delete_entity, request = self.setup_client( + si.DeleteEntity, 'delete_entity') + request.entity = entity_name + response = self.call_and_spin(delete_entity, request) + self.assert_result_ok(response) + + def get_entity_state(self, entity_name) -> si.GetEntityState.Response: + get_entity_state, request = self.setup_client( + si.GetEntityState, 'get_entity_state') + request.entity = entity_name + response = self.call_and_spin(get_entity_state, request) + self.assert_result_ok(response) + return response + + def reset_simulation(self) -> si.ResetSimulation.Response: + reset_simulation, request = self.setup_client( + si.ResetSimulation, 'reset_simulation') + request.scope = si.ResetSimulation.Request.SCOPE_DEFAULT + return self.call_and_spin(reset_simulation, request) + + # tests + + def test_get_entities_with_no_filters(self) -> None: + get_entities, request = self.setup_client( + si.GetEntities, 'get_entities') + response = self.call_and_spin(get_entities, request) + self.assert_result_ok(response) + + def test_get_entity_state(self, proc_output, bridge_node) -> None: + state = self.get_entity_state('vehicle').state + self.assertAlmostEqual(state.twist.linear.x, 0, delta=1e-4) + + # Send a velocity command to vehicle + publisher = self.node.create_publisher(Twist, 'cmd_vel', 10) + msg = Twist() + msg.linear.x = 0.25 + pattern = re.compile('Passing message.*geometry_msgs/msg/Twist') + for _ in range(10): + publisher.publish(msg) + if proc_output.waitFor(pattern, process=bridge_node, timeout=1): + break + rclpy.spin_once(self.node) + + time.sleep(2) + state = self.get_entity_state('vehicle').state + self.assertAlmostEqual(state.twist.linear.x, 0.25, delta=1e-1) + + def test_get_entity_state_on_spawned_entity(self) -> None: + sdf_string = """ + + + + 1.0 + + + 1 + + + + + 1 + + + + + + """ + spawn_entity, request = self.setup_client( + si.SpawnEntity, 'spawn_entity') + request.name = 'test_sphere' + request.entity_resource.resource_string = sdf_string + # spawn at 4, -20, 2 so that when the ball falls on the incline and start + # rolling without any external commands + request.initial_pose.pose.position.x = 4.0 + request.initial_pose.pose.position.y = -20.0 + request.initial_pose.pose.position.z = 4.0 + + self.assertTrue(self.call_and_spin(spawn_entity, request)) + time.sleep(2) + state = self.get_entity_state('test_sphere').state + self.assertGreater(state.twist.linear.x, 0.1) + + def test_set_entity_state(self) -> None: + self.assert_result_ok(self.reset_simulation()) + set_entity_state, request = self.setup_client( + si.SetEntityState, 'set_entity_state') + self.set_simulation_state(SimulationState.STATE_PLAYING) + test_entity = 'sphere' + request.entity = test_entity + request.state.pose.position.z = 100.0 + request.state.twist.linear.x = 5.0 + self.assert_result_ok(self.call_and_spin(set_entity_state, request)) + state = self.get_entity_state(test_entity).state + self.assertAlmostEqual(state.twist.linear.x, 5.0, delta=1e-1) + + def test_set_entity_state_preserves_sim_state(self) -> None: + set_entity_state, request = self.setup_client( + si.SetEntityState, 'set_entity_state') + request.entity = 'sphere' + request.state.pose.position.z = 10.0 + for test_state in [SimulationState.STATE_PLAYING, SimulationState.STATE_PAUSED]: + self.set_simulation_state(test_state) + self.assertEqual(self.get_simulation_state().state.state, test_state) + self.assert_result_ok(self.call_and_spin(set_entity_state, request)) + self.assertEqual(self.get_simulation_state().state.state, test_state) + + def test_spawn_entity_duplicate_name(self) -> None: + sdf_string = """ + + + + + + + """ + spawn_entity, request = self.setup_client( + si.SpawnEntity, 'spawn_entity') + request.name = 'test_duplicate' + request.entity_resource.resource_string = sdf_string + + self.assert_result_ok(self.call_and_spin(spawn_entity, request)) + time.sleep(2) + + # Try to spawn the same entity again + result = self.call_and_spin(spawn_entity, request).result.result + self.assertTrue(result, Result.RESULT_OPERATION_FAILED) + + def test_delete_entity(self) -> None: + self.delete_entity('box') + + def test_delete_on_spawned_entity(self) -> None: + sdf_string = """ + + + + + + + """ + spawn_entity, request = self.setup_client( + si.SpawnEntity, 'spawn_entity') + request.name = 'test_empty' + request.entity_resource.resource_string = sdf_string + + self.assert_result_ok(self.call_and_spin(spawn_entity, request)) + time.sleep(2) + + self.delete_entity('test_empty') + + def test_step_simulation(self) -> None: + step_simulation, request = self.setup_client( + si.StepSimulation, 'step_simulation') + request.steps = 5 + response = self.call_and_spin(step_simulation, request) + self.assert_result_ok(response) + + def test_reset_simulation(self) -> None: + self.assert_result_ok(self.reset_simulation()) + + def test_toggle_simulation_state(self) -> None: + initial_state = self.get_simulation_state().state.state + if initial_state == SimulationState.STATE_PAUSED: + target_state = SimulationState.STATE_PLAYING + else: + target_state = SimulationState.STATE_PAUSED + + self.set_simulation_state(target_state) + + final_state = self.get_simulation_state().state.state + self.assertEqual(final_state, target_state) + + # Return the simulation to its initial state + self.set_simulation_state(initial_state) + restored_state = self.get_simulation_state().state.state + self.assertEqual(restored_state, initial_state) + + def test_playing_when_already_playing(self) -> None: + # Try to set it to the same state twice + self.set_simulation_state(SimulationState.STATE_PLAYING) + self.set_simulation_state(SimulationState.STATE_PLAYING) + + def test_simulate_steps_action(self) -> None: + + goal_msg = SimulateSteps.Goal() + goal_msg.steps = 200 + + self.feedback_cb_count = 0 + + def feedback_cb(feedback_msg) -> None: + self.feedback_cb_count += 1 + feedback = feedback_msg.feedback + self.assertGreater(feedback.completed_steps, 0) + + simulation_state = self.get_simulation_state().state.state + if simulation_state != SimulationState.STATE_PAUSED: + self.set_simulation_state(SimulationState.STATE_PAUSED) + + # Create a new node for testing the action. + # A warning is printed when the existing node is used. + test_node = rclpy.create_node('test_action') + action_client = rclpy.action.ActionClient( + test_node, + SimulateSteps, + f'{GZ_SERVER_NODE_NAME}/simulate_steps') + self.assertTrue(action_client.wait_for_server(timeout_sec=30)) + + # TODO(azeey): Need to wait on the result future here so the test is ran to completion. + def send_goal_and_check_results(): + self.feedback_cb_count = 0 + send_goal_future = action_client.send_goal_async( + goal_msg, feedback_callback=feedback_cb) + rclpy.spin_until_future_complete(test_node, send_goal_future) + goal_handle = send_goal_future.result() + assert goal_handle is not None + result_future = goal_handle.get_result_async() + rclpy.spin_until_future_complete(test_node, result_future) + result = result_future.result() + assert result is not None + self.assert_result_ok(result.result) + + send_goal_and_check_results() + # Assert that the feedback callback is called at least once. + self.assertGreater(self.feedback_cb_count, 1) + # Do it again to make sure subsequent actions are handled properly + send_goal_and_check_results() + self.assertGreater(self.feedback_cb_count, 1) + + def test_get_entity_info(self) -> None: + get_entity_info, request = self.setup_client( + si.GetEntityInfo, 'get_entity_info') + request.entity = 'vehicle' + + response = self.call_and_spin(get_entity_info, request) + self.assert_result_ok(response) + + self.assertEqual(response.info.category.category, + EntityCategory.CATEGORY_ROBOT) + + def test_get_entities_states(self) -> None: + get_entities_state, request = self.setup_client( + si.GetEntitiesStates, 'get_entities_states') + request.filters.filter = 'vehicle' + + response = self.call_and_spin(get_entities_state, request) + self.assert_result_ok(response) + + self.assertEqual(len(response.entities), 1) + self.assertEqual(response.entities[0], 'vehicle') + + def test_get_simulator_features(self) -> None: + get_simulator_features, request = self.setup_client( + si.GetSimulatorFeatures, 'get_simulator_features') + + response = self.call_and_spin(get_simulator_features, request) + returned_features = response.features.features + + existing_features = [ + SimulatorFeatures.SPAWNING, + SimulatorFeatures.DELETING, + SimulatorFeatures.ENTITY_TAGS, + # SimulatorFeatures.ENTITY_BOUNDS, + # SimulatorFeatures.ENTITY_BOUNDS_BOX, + SimulatorFeatures.ENTITY_CATEGORIES, + SimulatorFeatures.SPAWNING_RESOURCE_STRING, + SimulatorFeatures.ENTITY_STATE_GETTING, + SimulatorFeatures.ENTITY_STATE_SETTING, + SimulatorFeatures.ENTITY_INFO_GETTING, + # SimulatorFeatures.ENTITY_INFO_SETTING, + SimulatorFeatures.SIMULATION_RESET, + # SimulatorFeatures.SIMULATION_RESET_TIME, + # SimulatorFeatures.SIMULATION_RESET_STATE, + # SimulatorFeatures.SIMULATION_RESET_SPAWNED, + SimulatorFeatures.SIMULATION_STATE_GETTING, + SimulatorFeatures.SIMULATION_STATE_SETTING, + SimulatorFeatures.SIMULATION_STATE_PAUSE, + SimulatorFeatures.STEP_SIMULATION_SINGLE, + SimulatorFeatures.STEP_SIMULATION_MULTIPLE, + SimulatorFeatures.STEP_SIMULATION_ACTION + ] + + # Check if each feature exists in the response + for feature in existing_features: + self.assertIn(feature, returned_features) + + # Check sizes of both feature lists + self.assertEqual(len(returned_features), len(existing_features)) + + +# NOTE: If we don't have this test, unittest will report "NO TESTS RAN" at the end of the test +# See https://github.com/colcon/colcon-core/issues/678 +@launch_testing.post_shutdown_test() +class TestGzserverShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + assertExitCodes(proc_info)