From 73a12b7c9bbfebd9a0030bc4f2f394f64228c05e Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Wed, 12 Sep 2018 19:29:13 +0200 Subject: [PATCH] [ros2] Port spawn/delete methods (#808) * First port of ROS2 of factory method. Still a work in progress * Install gazebo_ros_factory * Changes proposed by uncrustify * Make cpplint happy * Remove unneded header * fix merge * remove ported ROS 1 code * SpawnEntity service, initialize after world is created, remove XML strip since it's not needed, simplify Is* functions * support robot_namespace inside * a bit more tweaks and cleanup * Use libsdformat to parse the XML, instead of tinyxml, significantly reducing the code * uncrustify * port delete services * linters * spawn and delete tests, must check light test * fix spawning lights, compile error for non implemented conversions, linters --- .../gazebo_ros/gazebo_ros_api_plugin.h | 65 -- .../gazebo_ros/src/gazebo_ros_api_plugin.cpp | 741 ------------------ gazebo_msgs/CMakeLists.txt | 2 + gazebo_msgs/srv/DeleteEntity.srv | 5 + gazebo_msgs/srv/SpawnEntity.srv | 12 + gazebo_ros/CMakeLists.txt | 24 +- .../gazebo_ros/conversions/generic.hpp | 8 +- .../gazebo_ros/conversions/geometry_msgs.hpp | 23 +- .../gazebo_ros/conversions/sensor_msgs.hpp | 2 +- .../include/gazebo_ros/gazebo_ros_factory.hpp | 46 ++ gazebo_ros/package.xml | 2 + gazebo_ros/src/gazebo_ros_factory.cpp | 349 +++++++++ gazebo_ros/test/CMakeLists.txt | 6 +- gazebo_ros/test/test_gazebo_ros_factory.cpp | 211 +++++ 14 files changed, 678 insertions(+), 818 deletions(-) create mode 100644 gazebo_msgs/srv/DeleteEntity.srv create mode 100644 gazebo_msgs/srv/SpawnEntity.srv create mode 100644 gazebo_ros/include/gazebo_ros/gazebo_ros_factory.hpp create mode 100644 gazebo_ros/src/gazebo_ros_factory.cpp create mode 100644 gazebo_ros/test/test_gazebo_ros_factory.cpp diff --git a/.ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h b/.ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h index 9c475da1f..af5a7cfb6 100644 --- a/.ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h +++ b/.ros1_unported/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h @@ -48,10 +48,6 @@ #include "gazebo_msgs/JointRequest.h" #include "gazebo_msgs/BodyRequest.h" -#include "gazebo_msgs/SpawnModel.h" -#include "gazebo_msgs/DeleteModel.h" -#include "gazebo_msgs/DeleteLight.h" - #include "gazebo_msgs/ApplyBodyWrench.h" #include "gazebo_msgs/SetPhysicsProperties.h" @@ -142,20 +138,6 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief void onModelStatesDisconnect(); - /// \brief Function for inserting a URDF into Gazebo from ROS Service Call - bool spawnURDFModel(gazebo_msgs::SpawnModel::Request &req, - gazebo_msgs::SpawnModel::Response &res); - - /// \brief Both SDFs and converted URDFs get sent to this function for further manipulation from a ROS Service call - bool spawnSDFModel(gazebo_msgs::SpawnModel::Request &req, - gazebo_msgs::SpawnModel::Response &res); - - /// \brief delete model given name - bool deleteModel(gazebo_msgs::DeleteModel::Request &req,gazebo_msgs::DeleteModel::Response &res); - - /// \brief delete a given light by name - bool deleteLight(gazebo_msgs::DeleteLight::Request &req,gazebo_msgs::DeleteLight::Response &res); - /// \brief bool getModelState(gazebo_msgs::GetModelState::Request &req,gazebo_msgs::GetModelState::Response &res); @@ -251,30 +233,6 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief void publishModelStates(); - /// \brief - void stripXmlDeclaration(std::string &model_xml); - - /// \brief Update the model name and pose of the SDF file before sending to Gazebo - void updateSDFAttributes(TiXmlDocument &gazebo_model_xml, - const std::string &model_name, - const ignition::math::Vector3d &initial_xyz, - const ignition::math::Quaterniond &initial_q); - - /// \brief Update the model pose of the URDF file before sending to Gazebo - void updateURDFModelPose(TiXmlDocument &gazebo_model_xml, - const ignition::math::Vector3d &initial_xyz, - const ignition::math::Quaterniond &initial_q); - - /// \brief Update the model name of the URDF file before sending to Gazebo - void updateURDFName(TiXmlDocument &gazebo_model_xml, const std::string &model_name); - - /// \brief - void walkChildAddRobotNamespace(TiXmlNode* model_xml); - - /// \brief - bool spawnAndConform(TiXmlDocument &gazebo_model_xml, const std::string &model_name, - gazebo_msgs::SpawnModel::Response &res); - /// \brief helper function for applyBodyWrench /// shift wrench from reference frame to target frame void transformWrench(ignition::math::Vector3d &target_force, ignition::math::Vector3d &target_torque, @@ -288,24 +246,9 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief waits for the rest of Gazebo to be ready before initializing the dynamic reconfigure services void physicsReconfigureThread(); - /// \brief Unused - void onResponse(ConstResponsePtr &response); - - /// \brief utility for checking if string is in URDF format - bool isURDF(std::string model_xml); - - /// \brief utility for checking if string is in SDF format - bool isSDF(std::string model_xml); - /// \brief Connect to Gazebo via its plugin interface, get a pointer to the world, start events void loadGazeboRosApiPlugin(std::string world_name); - /// \brief convert xml to Pose - ignition::math::Pose3d parsePose(const std::string &str); - - /// \brief convert xml to Pose - ignition::math::Vector3d parseVector3(const std::string &str); - // track if the desconstructor event needs to occur bool plugin_loaded_; @@ -317,11 +260,7 @@ class GazeboRosApiPlugin : public SystemPlugin gazebo::transport::NodePtr gazebonode_; gazebo::transport::SubscriberPtr stat_sub_; - gazebo::transport::PublisherPtr factory_pub_; - gazebo::transport::PublisherPtr factory_light_pub_; gazebo::transport::PublisherPtr light_modify_pub_; - gazebo::transport::PublisherPtr request_pub_; - gazebo::transport::SubscriberPtr response_sub_; boost::shared_ptr nh_; ros::CallbackQueue gazebo_queue_; @@ -335,10 +274,6 @@ class GazeboRosApiPlugin : public SystemPlugin gazebo::event::ConnectionPtr pub_model_states_event_; gazebo::event::ConnectionPtr load_gazebo_ros_api_plugin_event_; - ros::ServiceServer spawn_sdf_model_service_; - ros::ServiceServer spawn_urdf_model_service_; - ros::ServiceServer delete_model_service_; - ros::ServiceServer delete_light_service_; ros::ServiceServer get_model_state_service_; ros::ServiceServer get_model_properties_service_; ros::ServiceServer get_world_properties_service_; diff --git a/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp index eac91cbb0..fdfe0ba68 100644 --- a/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -185,11 +185,7 @@ void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name) gazebonode_ = gazebo::transport::NodePtr(new gazebo::transport::Node()); gazebonode_->Init(world_name); //stat_sub_ = gazebonode_->Subscribe("~/world_stats", &GazeboRosApiPlugin::publishSimTime, this); // TODO: does not work in server plugin? - factory_pub_ = gazebonode_->Advertise("~/factory"); - factory_light_pub_ = gazebonode_->Advertise("~/factory/light"); light_modify_pub_ = gazebonode_->Advertise("~/light/modify"); - request_pub_ = gazebonode_->Advertise("~/request"); - response_sub_ = gazebonode_->Subscribe("~/response",&GazeboRosApiPlugin::onResponse, this); // reset topic connection counts pub_link_states_connection_count_ = 0; @@ -218,11 +214,6 @@ void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name) time_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishSimTime,this)); } -void GazeboRosApiPlugin::onResponse(ConstResponsePtr &response) -{ - -} - void GazeboRosApiPlugin::gazeboQueueThread() { static const double timeout = 0.001; @@ -240,45 +231,8 @@ void GazeboRosApiPlugin::advertiseServices() return; } - // publish clock for simulated ros time pub_clock_ = nh_->advertise("/clock",10); - // Advertise spawn services on the custom queue - std::string spawn_sdf_model_service_name("spawn_sdf_model"); - ros::AdvertiseServiceOptions spawn_sdf_model_aso = - ros::AdvertiseServiceOptions::create( - spawn_sdf_model_service_name, - boost::bind(&GazeboRosApiPlugin::spawnSDFModel,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - spawn_sdf_model_service_ = nh_->advertiseService(spawn_sdf_model_aso); - - // Advertise spawn services on the custom queue - std::string spawn_urdf_model_service_name("spawn_urdf_model"); - ros::AdvertiseServiceOptions spawn_urdf_model_aso = - ros::AdvertiseServiceOptions::create( - spawn_urdf_model_service_name, - boost::bind(&GazeboRosApiPlugin::spawnURDFModel,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - spawn_urdf_model_service_ = nh_->advertiseService(spawn_urdf_model_aso); - - // Advertise delete services on the custom queue - std::string delete_model_service_name("delete_model"); - ros::AdvertiseServiceOptions delete_aso = - ros::AdvertiseServiceOptions::create( - delete_model_service_name, - boost::bind(&GazeboRosApiPlugin::deleteModel,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - delete_model_service_ = nh_->advertiseService(delete_aso); - - // Advertise delete service for lights on the custom queue - std::string delete_light_service_name("delete_light"); - ros::AdvertiseServiceOptions delete_light_aso = - ros::AdvertiseServiceOptions::create( - delete_light_service_name, - boost::bind(&GazeboRosApiPlugin::deleteLight,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - delete_light_service_ = nh_->advertiseService(delete_light_aso); - // Advertise more services on the custom queue std::string get_model_properties_service_name("get_model_properties"); ros::AdvertiseServiceOptions get_model_properties_aso = @@ -571,312 +525,6 @@ void GazeboRosApiPlugin::onModelStatesDisconnect() } } -bool GazeboRosApiPlugin::spawnURDFModel(gazebo_msgs::SpawnModel::Request &req, - gazebo_msgs::SpawnModel::Response &res) -{ - // get namespace for the corresponding model plugins - robot_namespace_ = req.robot_namespace; - - // incoming entity string - std::string model_xml = req.model_xml; - - if (!isURDF(model_xml)) - { - ROS_ERROR_NAMED("api_plugin", "SpawnModel: Failure - entity format is invalid."); - res.success = false; - res.status_message = "SpawnModel: Failure - entity format is invalid."; - return false; - } - - /// STRIP DECLARATION from model_xml - /// @todo: does tinyxml have functionality for this? - /// @todo: should gazebo take care of the declaration? - { - std::string open_bracket(""); - size_t pos1 = model_xml.find(open_bracket,0); - size_t pos2 = model_xml.find(close_bracket,0); - if (pos1 != std::string::npos && pos2 != std::string::npos) - model_xml.replace(pos1,pos2-pos1+2,std::string("")); - } - - // Remove comments from URDF - { - std::string open_comment(""); - size_t pos1; - while((pos1 = model_xml.find(open_comment,0)) != std::string::npos){ - size_t pos2 = model_xml.find(close_comment,0); - if (pos2 != std::string::npos) - model_xml.replace(pos1,pos2-pos1+3,std::string("")); - } - } - - // Now, replace package://xxx with the full path to the package - { - std::string package_prefix("package://"); - size_t pos1 = model_xml.find(package_prefix,0); - while (pos1 != std::string::npos) - { - size_t pos2 = model_xml.find("/", pos1+10); - //ROS_DEBUG_NAMED("api_plugin", " pos %d %d",(int)pos1, (int)pos2); - if (pos2 == std::string::npos || pos1 >= pos2) - { - ROS_ERROR_NAMED("api_plugin", "Malformed package name?"); - break; - } - - std::string package_name = model_xml.substr(pos1+10,pos2-pos1-10); - //ROS_DEBUG_NAMED("api_plugin", "package name [%s]", package_name.c_str()); - std::string package_path = ros::package::getPath(package_name); - if (package_path.empty()) - { - ROS_FATAL_NAMED("api_plugin", "Package[%s] does not have a path",package_name.c_str()); - res.success = false; - res.status_message = "urdf reference package name does not exist: " + package_name; - return false; - } - ROS_DEBUG_ONCE_NAMED("api_plugin", "Package name [%s] has path [%s]", package_name.c_str(), package_path.c_str()); - - model_xml.replace(pos1,(pos2-pos1),package_path); - pos1 = model_xml.find(package_prefix, pos1); - } - } - // ROS_DEBUG_NAMED("api_plugin", "Model XML\n\n%s\n\n ",model_xml.c_str()); - - req.model_xml = model_xml; - - // Model is now considered convert to SDF - return spawnSDFModel(req,res); -} - -bool GazeboRosApiPlugin::spawnSDFModel(gazebo_msgs::SpawnModel::Request &req, - gazebo_msgs::SpawnModel::Response &res) -{ - // incoming entity name - std::string model_name = req.model_name; - - // get namespace for the corresponding model plugins - robot_namespace_ = req.robot_namespace; - - // get initial pose of model - ignition::math::Vector3d initial_xyz(req.initial_pose.position.x,req.initial_pose.position.y,req.initial_pose.position.z); - // get initial roll pitch yaw (fixed frame transform) - ignition::math::Quaterniond initial_q(req.initial_pose.orientation.w,req.initial_pose.orientation.x,req.initial_pose.orientation.y,req.initial_pose.orientation.z); - - // refernce frame for initial pose definition, modify initial pose if defined -#if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::EntityPtr frame = world_->EntityByName(req.reference_frame); -#else - gazebo::physics::EntityPtr frame = world_->GetEntity(req.reference_frame); -#endif - if (frame) - { - // convert to relative pose -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d frame_pose = frame->WorldPose(); -#else - ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); -#endif - initial_xyz = frame_pose.Pos() + frame_pose.Rot().RotateVector(initial_xyz); - initial_q = frame_pose.Rot() * initial_q; - } - - /// @todo: map is really wrong, need to use tf here somehow - else if (req.reference_frame == "" || req.reference_frame == "world" || req.reference_frame == "map" || req.reference_frame == "/map") - { - ROS_DEBUG_NAMED("api_plugin", "SpawnModel: reference_frame is empty/world/map, using inertial frame"); - } - else - { - res.success = false; - res.status_message = "SpawnModel: reference reference_frame not found, did you forget to scope the link by model name?"; - return true; - } - - // incoming robot model string - std::string model_xml = req.model_xml; - - // store resulting Gazebo Model XML to be sent to spawn queue - // get incoming string containg either an URDF or a Gazebo Model XML - // grab from parameter server if necessary convert to SDF if necessary - stripXmlDeclaration(model_xml); - - // put string in TiXmlDocument for manipulation - TiXmlDocument gazebo_model_xml; - gazebo_model_xml.Parse(model_xml.c_str()); - - // optional model manipulations: update initial pose && replace model name - if (isSDF(model_xml)) - { - updateSDFAttributes(gazebo_model_xml, model_name, initial_xyz, initial_q); - - // Walk recursively through the entire SDF, locate plugin tags and - // add robotNamespace as a child with the correct namespace - if (!this->robot_namespace_.empty()) - { - // Get root element for SDF - // TODO: implement the spawning also with and - TiXmlNode* model_tixml = gazebo_model_xml.FirstChild("sdf"); - model_tixml = (!model_tixml) ? - gazebo_model_xml.FirstChild("gazebo") : model_tixml; - if (model_tixml) - { - walkChildAddRobotNamespace(model_tixml); - } - else - { - ROS_WARN_NAMED("api_plugin", "Unable to add robot namespace to xml"); - } - } - } - else if (isURDF(model_xml)) - { - updateURDFModelPose(gazebo_model_xml, initial_xyz, initial_q); - updateURDFName(gazebo_model_xml, model_name); - - // Walk recursively through the entire URDF, locate plugin tags and - // add robotNamespace as a child with the correct namespace - if (!this->robot_namespace_.empty()) - { - // Get root element for URDF - TiXmlNode* model_tixml = gazebo_model_xml.FirstChild("robot"); - if (model_tixml) - { - walkChildAddRobotNamespace(model_tixml); - } - else - { - ROS_WARN_NAMED("api_plugin", "Unable to add robot namespace to xml"); - } - } - } - else - { - ROS_ERROR_NAMED("api_plugin", "GazeboRosApiPlugin SpawnModel Failure: input xml format not recognized"); - res.success = false; - res.status_message = "GazeboRosApiPlugin SpawnModel Failure: input model_xml not SDF or URDF, or cannot be converted to Gazebo compatible format."; - return true; - } - - // do spawning check if spawn worked, return response - return spawnAndConform(gazebo_model_xml, model_name, res); -} - -bool GazeboRosApiPlugin::deleteModel(gazebo_msgs::DeleteModel::Request &req, - gazebo_msgs::DeleteModel::Response &res) -{ - // clear forces, etc for the body in question -#if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::ModelPtr model = world_->ModelByName(req.model_name); -#else - gazebo::physics::ModelPtr model = world_->GetModel(req.model_name); -#endif - if (!model) - { - ROS_ERROR_NAMED("api_plugin", "DeleteModel: model [%s] does not exist",req.model_name.c_str()); - res.success = false; - res.status_message = "DeleteModel: model does not exist"; - return true; - } - - // delete wrench jobs on bodies - for (unsigned int i = 0 ; i < model->GetChildCount(); i ++) - { - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(model->GetChild(i)); - if (body) - { - // look for it in jobs, delete body wrench jobs - clearBodyWrenches(body->GetScopedName()); - } - } - - // delete force jobs on joints - gazebo::physics::Joint_V joints = model->GetJoints(); - for (unsigned int i=0;i< joints.size(); i++) - { - // look for it in jobs, delete joint force jobs - clearJointForces(joints[i]->GetName()); - } - - // send delete model request - gazebo::msgs::Request *msg = gazebo::msgs::CreateRequest("entity_delete",req.model_name); - request_pub_->Publish(*msg,true); - - ros::Duration model_spawn_timeout(60.0); - ros::Time timeout = ros::Time::now() + model_spawn_timeout; - // wait and verify that model is deleted - while (true) - { - if (ros::Time::now() > timeout) - { - res.success = false; - res.status_message = "DeleteModel: Model pushed to delete queue, but delete service timed out waiting for model to disappear from simulation"; - return true; - } - { - //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex()); -#if GAZEBO_MAJOR_VERSION >= 8 - if (!world_->ModelByName(req.model_name)) break; -#else - if (!world_->GetModel(req.model_name)) break; -#endif - } - ROS_DEBUG_NAMED("api_plugin", "Waiting for model deletion (%s)",req.model_name.c_str()); - usleep(1000); - } - - // set result - res.success = true; - res.status_message = "DeleteModel: successfully deleted model"; - return true; -} - -bool GazeboRosApiPlugin::deleteLight(gazebo_msgs::DeleteLight::Request &req, - gazebo_msgs::DeleteLight::Response &res) -{ -#if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::LightPtr phy_light = world_->LightByName(req.light_name); -#else - gazebo::physics::LightPtr phy_light = world_->Light(req.light_name); -#endif - - if (phy_light == NULL) - { - res.success = false; - res.status_message = "DeleteLight: Requested light " + req.light_name + " not found!"; - } - else - { - gazebo::msgs::Request* msg = gazebo::msgs::CreateRequest("entity_delete", req.light_name); - request_pub_->Publish(*msg, true); - - res.success = false; - - for (int i = 0; i < 100; i++) - { -#if GAZEBO_MAJOR_VERSION >= 8 - phy_light = world_->LightByName(req.light_name); -#else - phy_light = world_->Light(req.light_name); -#endif - if (phy_light == NULL) - { - res.success = true; - res.status_message = "DeleteLight: " + req.light_name + " successfully deleted"; - return true; - } - // Check every 100ms - usleep(100000); - } - } - - res.status_message = "DeleteLight: Timeout reached while removing light \"" + req.light_name - + "\""; - - return true; -} - bool GazeboRosApiPlugin::getModelState(gazebo_msgs::GetModelState::Request &req, gazebo_msgs::GetModelState::Response &res) { @@ -1998,28 +1646,6 @@ bool GazeboRosApiPlugin::applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request & return true; } -bool GazeboRosApiPlugin::isURDF(std::string model_xml) -{ - TiXmlDocument doc_in; - doc_in.Parse(model_xml.c_str()); - if (doc_in.FirstChild("robot")) - return true; - else - return false; -} - -bool GazeboRosApiPlugin::isSDF(std::string model_xml) -{ - // FIXME: very crude check - TiXmlDocument doc_in; - doc_in.Parse(model_xml.c_str()); - if (doc_in.FirstChild("gazebo") || - doc_in.FirstChild("sdf")) // sdf - return true; - else - return false; -} - void GazeboRosApiPlugin::wrenchBodySchedulerSlot() { // MDMutex locks in case model is getting deleted, don't have to do this if we delete jobs first @@ -2325,373 +1951,6 @@ void GazeboRosApiPlugin::physicsReconfigureThread() ROS_INFO_NAMED("api_plugin", "Physics dynamic reconfigure ready."); } -void GazeboRosApiPlugin::stripXmlDeclaration(std::string &model_xml) -{ - // incoming robot model string is a string containing a Gazebo Model XML - /// STRIP DECLARATION from model_xml - /// @todo: does tinyxml have functionality for this? - /// @todo: should gazebo take care of the declaration? - std::string open_bracket(""); - size_t pos1 = model_xml.find(open_bracket,0); - size_t pos2 = model_xml.find(close_bracket,0); - if (pos1 != std::string::npos && pos2 != std::string::npos) - model_xml.replace(pos1,pos2-pos1+2,std::string("")); -} - -void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, - const std::string &model_name, - const ignition::math::Vector3d &initial_xyz, - const ignition::math::Quaterniond &initial_q) -{ - // This function can handle both regular SDF files and SDFs that are used with the - // Gazebo Model Database - - TiXmlElement* pose_element; // This is used by both reguar and database SDFs - - // Check SDF for requires SDF element - TiXmlElement* gazebo_tixml = gazebo_model_xml.FirstChildElement("sdf"); - if (!gazebo_tixml) - { - ROS_WARN_NAMED("api_plugin", "Could not find element in sdf, so name and initial position cannot be applied"); - return; - } - - // Check SDF for optional model element. May not have one - TiXmlElement* model_tixml = gazebo_tixml->FirstChildElement("model"); - if (model_tixml) - { - // Update entity name - if (model_tixml->Attribute("name") != NULL) - { - // removing old entity name - model_tixml->RemoveAttribute("name"); - } - // replace with user specified name - model_tixml->SetAttribute("name",model_name); - } - else - { - // Check SDF for world element - TiXmlElement* world_tixml = gazebo_tixml->FirstChildElement("world"); - if (!world_tixml) - { - ROS_WARN_NAMED("api_plugin", "Could not find or element in sdf, so name and initial position cannot be applied"); - return; - } - // If not element, check SDF for required include element - model_tixml = world_tixml->FirstChildElement("include"); - if (!model_tixml) - { - ROS_WARN_NAMED("api_plugin", "Could not find element in sdf, so name and initial position cannot be applied"); - return; - } - - // Check for name element - TiXmlElement* name_tixml = model_tixml->FirstChildElement("name"); - if (!name_tixml) - { - // Create the name element - name_tixml = new TiXmlElement("name"); - model_tixml->LinkEndChild(name_tixml); - } - - // Set the text within the name element - TiXmlText* text = new TiXmlText(model_name); - name_tixml->LinkEndChild( text ); - } - - - // Check for the pose element - pose_element = model_tixml->FirstChildElement("pose"); - ignition::math::Pose3d model_pose; - - // Create the pose element if it doesn't exist - // Remove it if it exists, since we are inserting a new one - if (pose_element) - { - // save pose_element in ignition::math::Pose3d and remove child - model_pose = this->parsePose(pose_element->GetText()); - model_tixml->RemoveChild(pose_element); - } - - // Set and link the pose element after adding initial pose - { - // add pose_element Pose to initial pose - ignition::math::Pose3d new_model_pose = model_pose + ignition::math::Pose3d(initial_xyz, initial_q); - - // Create the string of 6 numbers - std::ostringstream pose_stream; - ignition::math::Vector3d model_rpy = new_model_pose.Rot().Euler(); // convert to Euler angles for Gazebo XML - pose_stream << new_model_pose.Pos().X() << " " << new_model_pose.Pos().Y() << " " << new_model_pose.Pos().Z() << " " - << model_rpy.X() << " " << model_rpy.Y() << " " << model_rpy.Z(); - - // Add value to pose element - TiXmlText* text = new TiXmlText(pose_stream.str()); - TiXmlElement* new_pose_element = new TiXmlElement("pose"); - new_pose_element->LinkEndChild(text); - model_tixml->LinkEndChild(new_pose_element); - } -} - -ignition::math::Pose3d GazeboRosApiPlugin::parsePose(const std::string &str) -{ - std::vector pieces; - std::vector vals; - - boost::split(pieces, str, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i) - { - if (pieces[i] != "") - { - try - { - vals.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch(boost::bad_lexical_cast &e) - { - sdferr << "xml key [" << str - << "][" << i << "] value [" << pieces[i] - << "] is not a valid double from a 3-tuple\n"; - return ignition::math::Pose3d(); - } - } - } - - if (vals.size() == 6) - return ignition::math::Pose3d(vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]); - else - { - ROS_ERROR_NAMED("api_plugin", "Beware: failed to parse string [%s] as ignition::math::Pose3d, returning zeros.", str.c_str()); - return ignition::math::Pose3d(); - } -} - -ignition::math::Vector3d GazeboRosApiPlugin::parseVector3(const std::string &str) -{ - std::vector pieces; - std::vector vals; - - boost::split(pieces, str, boost::is_any_of(" ")); - for (unsigned int i = 0; i < pieces.size(); ++i) - { - if (pieces[i] != "") - { - try - { - vals.push_back(boost::lexical_cast(pieces[i].c_str())); - } - catch(boost::bad_lexical_cast &e) - { - sdferr << "xml key [" << str - << "][" << i << "] value [" << pieces[i] - << "] is not a valid double from a 3-tuple\n"; - return ignition::math::Vector3d(); - } - } - } - - if (vals.size() == 3) - return ignition::math::Vector3d(vals[0], vals[1], vals[2]); - else - { - ROS_ERROR_NAMED("api_plugin", "Beware: failed to parse string [%s] as ignition::math::Vector3d, returning zeros.", str.c_str()); - return ignition::math::Vector3d(); - } -} - -void GazeboRosApiPlugin::updateURDFModelPose(TiXmlDocument &gazebo_model_xml, - const ignition::math::Vector3d &initial_xyz, - const ignition::math::Quaterniond &initial_q) -{ - TiXmlElement* model_tixml = (gazebo_model_xml.FirstChildElement("robot")); - if (model_tixml) - { - // replace initial pose of model - // find first instance of xyz and rpy, replace with initial pose - TiXmlElement* origin_key = model_tixml->FirstChildElement("origin"); - - if (!origin_key) - { - origin_key = new TiXmlElement("origin"); - model_tixml->LinkEndChild(origin_key); - } - - ignition::math::Vector3d xyz; - ignition::math::Vector3d rpy; - if (origin_key->Attribute("xyz")) - { - xyz = this->parseVector3(origin_key->Attribute("xyz")); - origin_key->RemoveAttribute("xyz"); - } - if (origin_key->Attribute("rpy")) - { - rpy = this->parseVector3(origin_key->Attribute("rpy")); - origin_key->RemoveAttribute("rpy"); - } - - // add xyz, rpy to initial pose - ignition::math::Pose3d model_pose = ignition::math::Pose3d(xyz, ignition::math::Quaterniond(rpy)) - + ignition::math::Pose3d(initial_xyz, initial_q); - - std::ostringstream xyz_stream; - xyz_stream << model_pose.Pos().X() << " " << model_pose.Pos().Y() << " " << model_pose.Pos().Z(); - - std::ostringstream rpy_stream; - ignition::math::Vector3d model_rpy = model_pose.Rot().Euler(); // convert to Euler angles for Gazebo XML - rpy_stream << model_rpy.X() << " " << model_rpy.Y() << " " << model_rpy.Z(); - - origin_key->SetAttribute("xyz",xyz_stream.str()); - origin_key->SetAttribute("rpy",rpy_stream.str()); - } - else - ROS_WARN_NAMED("api_plugin", "Could not find element in sdf, so name and initial position is not applied"); -} - -void GazeboRosApiPlugin::updateURDFName(TiXmlDocument &gazebo_model_xml, const std::string &model_name) -{ - TiXmlElement* model_tixml = gazebo_model_xml.FirstChildElement("robot"); - // replace model name if one is specified by the user - if (model_tixml) - { - if (model_tixml->Attribute("name") != NULL) - { - // removing old model name - model_tixml->RemoveAttribute("name"); - } - // replace with user specified name - model_tixml->SetAttribute("name",model_name); - } - else - ROS_WARN_NAMED("api_plugin", "Could not find element in URDF, name not replaced"); -} - -void GazeboRosApiPlugin::walkChildAddRobotNamespace(TiXmlNode* model_xml) -{ - TiXmlNode* child = 0; - child = model_xml->IterateChildren(child); - while (child != NULL) - { - if (child->Type() == TiXmlNode::TINYXML_ELEMENT && - child->ValueStr().compare(std::string("plugin")) == 0) - { - if (child->FirstChildElement("robotNamespace") == NULL) - { - TiXmlElement* child_elem = child->ToElement()->FirstChildElement("robotNamespace"); - while (child_elem) - { - child->ToElement()->RemoveChild(child_elem); - child_elem = child->ToElement()->FirstChildElement("robotNamespace"); - } - TiXmlElement* key = new TiXmlElement("robotNamespace"); - TiXmlText* val = new TiXmlText(robot_namespace_); - key->LinkEndChild(val); - child->ToElement()->LinkEndChild(key); - } - } - walkChildAddRobotNamespace(child); - child = model_xml->IterateChildren(child); - } -} - -bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, const std::string &model_name, - gazebo_msgs::SpawnModel::Response &res) -{ - std::string entity_type = gazebo_model_xml.RootElement()->FirstChild()->Value(); - // Convert the entity type to lower case - std::transform(entity_type.begin(), entity_type.end(), entity_type.begin(), ::tolower); - - bool isLight = (entity_type == "light"); - - // push to factory iface - std::ostringstream stream; - stream << gazebo_model_xml; - std::string gazebo_model_xml_string = stream.str(); - ROS_DEBUG_NAMED("api_plugin.xml", "Gazebo Model XML\n\n%s\n\n ",gazebo_model_xml_string.c_str()); - - // publish to factory topic - gazebo::msgs::Factory msg; - gazebo::msgs::Init(msg, "spawn_model"); - msg.set_sdf( gazebo_model_xml_string ); - - //ROS_ERROR_NAMED("api_plugin", "attempting to spawn model name [%s] [%s]", model_name.c_str(),gazebo_model_xml_string.c_str()); - - // FIXME: should use entity_info or add lock to World::receiveMutex - // looking for Model to see if it exists already - gazebo::msgs::Request *entity_info_msg = gazebo::msgs::CreateRequest("entity_info", model_name); - request_pub_->Publish(*entity_info_msg,true); - // todo: should wait for response response_sub_, check to see that if _msg->response == "nonexistant" - -#if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::ModelPtr model = world_->ModelByName(model_name); - gazebo::physics::LightPtr light = world_->LightByName(model_name); -#else - gazebo::physics::ModelPtr model = world_->GetModel(model_name); - gazebo::physics::LightPtr light = world_->Light(model_name); -#endif - if ((isLight && light != NULL) || (model != NULL)) - { - ROS_ERROR_NAMED("api_plugin", "SpawnModel: Failure - model name %s already exist.",model_name.c_str()); - res.success = false; - res.status_message = "SpawnModel: Failure - entity already exists."; - return true; - } - - // for Gazebo 7 and up, use a different method to spawn lights - if (isLight) - { - // Publish the light message to spawn the light (Gazebo 7 and up) - sdf::SDF sdf_light; - sdf_light.SetFromString(gazebo_model_xml_string); - gazebo::msgs::Light msg = gazebo::msgs::LightFromSDF(sdf_light.Root()->GetElement("light")); - msg.set_name(model_name); - factory_light_pub_->Publish(msg); - } - else - { - // Publish the factory message - factory_pub_->Publish(msg); - } - /// FIXME: should change publish to direct invocation World::LoadModel() and/or - /// change the poll for Model existence to common::Events based check. - - /// \brief poll and wait, verify that the model is spawned within Hardcoded 10 seconds - ros::Duration model_spawn_timeout(10.0); - ros::Time timeout = ros::Time::now() + model_spawn_timeout; - - while (ros::ok()) - { - if (ros::Time::now() > timeout) - { - res.success = false; - res.status_message = "SpawnModel: Entity pushed to spawn queue, but spawn service timed out waiting for entity to appear in simulation under the name " + model_name; - return true; - } - - { - //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex()); -#if GAZEBO_MAJOR_VERSION >= 8 - if ((isLight && world_->LightByName(model_name) != NULL) - || (world_->ModelByName(model_name) != NULL)) -#else - if ((isLight && world_->Light(model_name) != NULL) - || (world_->GetModel(model_name) != NULL)) -#endif - break; - } - - ROS_DEBUG_STREAM_ONCE_NAMED("api_plugin","Waiting for " << timeout - ros::Time::now() - << " for entity " << model_name << " to spawn"); - - usleep(2000); - } - - // set result - res.success = true; - res.status_message = "SpawnModel: Successfully spawned entity"; - return true; -} - // Register this plugin with the simulator GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosApiPlugin) } diff --git a/gazebo_msgs/CMakeLists.txt b/gazebo_msgs/CMakeLists.txt index 8095eb7a4..2d8d782f8 100644 --- a/gazebo_msgs/CMakeLists.txt +++ b/gazebo_msgs/CMakeLists.txt @@ -35,6 +35,7 @@ set(srv_files "srv/ApplyBodyWrench.srv" "srv/ApplyJointEffort.srv" "srv/BodyRequest.srv" + "srv/DeleteEntity.srv" "srv/DeleteLight.srv" "srv/DeleteModel.srv" "srv/GetJointProperties.srv" @@ -54,6 +55,7 @@ set(srv_files "srv/SetModelConfiguration.srv" "srv/SetModelState.srv" "srv/SetPhysicsProperties.srv" + "srv/SpawnEntity.srv" "srv/SpawnModel.srv" ) diff --git a/gazebo_msgs/srv/DeleteEntity.srv b/gazebo_msgs/srv/DeleteEntity.srv new file mode 100644 index 000000000..6491c9fb8 --- /dev/null +++ b/gazebo_msgs/srv/DeleteEntity.srv @@ -0,0 +1,5 @@ +string name # Name of the Gazebo entity to be deleted. This can be either + # a model or a light. +--- +bool success # Return true if deletion is successful. +string status_message # Comments if available. diff --git a/gazebo_msgs/srv/SpawnEntity.srv b/gazebo_msgs/srv/SpawnEntity.srv new file mode 100644 index 000000000..3bff5952f --- /dev/null +++ b/gazebo_msgs/srv/SpawnEntity.srv @@ -0,0 +1,12 @@ +string name # Name of the entity to be spawned (optional). +string xml # Entity XML description as a string, either URDF or SDF. +string robot_namespace # Spawn robot and all ROS interfaces under this namespace +geometry_msgs/Pose initial_pose # Initial entity pose. +string reference_frame # initial_pose is defined relative to the frame of this entity. + # If left empty or "world" or "map", then gazebo world frame is + # used. + # If non-existent entity is specified, an error is returned + # and the entity is not spawned. +--- +bool success # Return true if spawned successfully. +string status_message # Comments if available. diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 07118d337..68a036df4 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -16,12 +16,16 @@ find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(gazebo_dev REQUIRED) +find_package(gazebo_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tinyxml_vendor REQUIRED) include_directories( include ${gazebo_dev_INCLUDE_DIRS} ) +# gazebo_ros_node add_library(gazebo_ros_node SHARED src/executor.cpp src/node.cpp @@ -32,6 +36,7 @@ ament_target_dependencies(gazebo_ros_node ) ament_export_libraries(gazebo_ros_node) +# gazebo_ros_utils add_library(gazebo_ros_utils SHARED src/utils.cpp ) @@ -41,6 +46,7 @@ ament_target_dependencies(gazebo_ros_utils ) ament_export_libraries(gazebo_ros_utils) +# gazebo_ros_init add_library(gazebo_ros_init SHARED src/gazebo_ros_init.cpp ) @@ -55,6 +61,21 @@ target_link_libraries(gazebo_ros_init ) ament_export_libraries(gazebo_ros_init) +# gazebo_ros_factory +add_library(gazebo_ros_factory SHARED + src/gazebo_ros_factory.cpp +) +ament_target_dependencies(gazebo_ros_factory + "rclcpp" + "gazebo_dev" + "gazebo_msgs" +) +target_link_libraries(gazebo_ros_factory + gazebo_ros_node + gazebo_ros_utils +) +ament_export_libraries(gazebo_ros_factory) + ament_export_include_directories(include) ament_export_dependencies(rclcpp) ament_export_dependencies(gazebo_dev) @@ -72,8 +93,9 @@ install(DIRECTORY include/ install( TARGETS - gazebo_ros_node + gazebo_ros_factory gazebo_ros_init + gazebo_ros_node gazebo_ros_utils ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/gazebo_ros/include/gazebo_ros/conversions/generic.hpp b/gazebo_ros/include/gazebo_ros/conversions/generic.hpp index db6eb2c38..44b8c8b1d 100644 --- a/gazebo_ros/include/gazebo_ros/conversions/generic.hpp +++ b/gazebo_ros/include/gazebo_ros/conversions/generic.hpp @@ -36,7 +36,7 @@ static rclcpp::Logger conversions_logger = rclcpp::get_logger("gazebo_ros_conver template OUT Convert(const ignition::math::Vector3d & in) { - return OUT(); + OUT::ConversionNotImplemented; } /// Generic conversion from an Ignition Math quaternion to another type. @@ -46,7 +46,7 @@ OUT Convert(const ignition::math::Vector3d & in) template OUT Convert(const ignition::math::Quaterniond & in) { - return OUT(); + OUT::ConversionNotImplemented; } /// Generic conversion from an Gazebo Time object to another type. @@ -56,7 +56,7 @@ OUT Convert(const ignition::math::Quaterniond & in) template OUT Convert(const gazebo::common::Time & in) { - return OUT(); + OUT::ConversionNotImplemented; } /// \brief Specialized conversion from an Gazebo Time to a RCLCPP Time. @@ -75,7 +75,7 @@ rclcpp::Time Convert(const gazebo::common::Time & in) template OUT Convert(const gazebo::msgs::Time & in) { - return OUT(); + OUT::ConversionNotImplemented; } } // namespace gazebo_ros diff --git a/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp b/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp index 503c450dd..8794d2663 100644 --- a/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp +++ b/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp @@ -34,7 +34,7 @@ namespace gazebo_ros template OUT Convert(const geometry_msgs::msg::Vector3 & in) { - return OUT(); + OUT::ConversionNotImplemented; } /// \brief Specialized conversion from a ROS vector message to an Ignition Math vector. @@ -57,7 +57,7 @@ ignition::math::Vector3d Convert(const geometry_msgs::msg::Vector3 & msg) template OUT Convert(const geometry_msgs::msg::Point32 & in) { - return OUT(); + OUT::ConversionNotImplemented; } /// \brief Specialized conversion from a ROS point32 message to an Ignition Math vector. @@ -78,9 +78,9 @@ ignition::math::Vector3d Convert(const geometry_msgs::msg::Point32 & in) /// \return Conversion result /// \tparam OUT Output type template -OUT Convert(const geometry_msgs::msg::Point & in) +OUT Convert(const geometry_msgs::msg::Point &) { - return OUT(); + OUT::ConversionNotImplemented; } /// TODO(louise) This may already exist somewhere else, since it's within the same lib @@ -97,6 +97,19 @@ geometry_msgs::msg::Vector3 Convert(const geometry_msgs::msg::Point & in) return msg; } +/// \brief Specialized conversion from a ROS point message to an Ignition math vector. +/// \param[in] in ROS message to convert. +/// \return A ROS vector message. +template<> +ignition::math::Vector3d Convert(const geometry_msgs::msg::Point & in) +{ + ignition::math::Vector3d out; + out.X(in.x); + out.Y(in.y); + out.Z(in.z); + return out; +} + /// \brief Specialized conversion from an Ignition Math vector to a ROS message. /// \param[in] vec Ignition vector to convert. /// \return ROS geometry vector message @@ -144,7 +157,7 @@ geometry_msgs::msg::Quaternion Convert(const ignition::math::Quaterniond & in) template OUT Convert(const geometry_msgs::msg::Quaternion & in) { - return OUT(); + OUT::ConversionNotImplemented; } /// \brief Specialized conversion from a ROS quaternion message to ignition quaternion diff --git a/gazebo_ros/include/gazebo_ros/conversions/sensor_msgs.hpp b/gazebo_ros/include/gazebo_ros/conversions/sensor_msgs.hpp index 068b4335b..5e5fdfaf6 100644 --- a/gazebo_ros/include/gazebo_ros/conversions/sensor_msgs.hpp +++ b/gazebo_ros/include/gazebo_ros/conversions/sensor_msgs.hpp @@ -42,7 +42,7 @@ namespace gazebo_ros template OUT Convert(const gazebo::msgs::LaserScanStamped & in, double min_intensity = 0.0) { - return OUT(); + OUT::ConversionNotImplemented; } /// \brief Specialized conversion from an Gazebo Laser Scan to a ROS Laser Scan. diff --git a/gazebo_ros/include/gazebo_ros/gazebo_ros_factory.hpp b/gazebo_ros/include/gazebo_ros/gazebo_ros_factory.hpp new file mode 100644 index 000000000..1fa6b01ab --- /dev/null +++ b/gazebo_ros/include/gazebo_ros/gazebo_ros_factory.hpp @@ -0,0 +1,46 @@ +// Copyright 2018 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 GAZEBO_ROS__GAZEBO_ROS_FACTORY_HPP_ +#define GAZEBO_ROS__GAZEBO_ROS_FACTORY_HPP_ + +#include + +#include + +namespace gazebo_ros +{ + +class GazeboRosFactoryPrivate; + +/// Factoryializes ROS with the system arguments passed to Gazebo (i.e. calls rclcpp::init) and +/// provides services to spawn and delete entities from the world. +class GazeboRosFactory : public gazebo::SystemPlugin +{ +public: + /// Constructor + GazeboRosFactory(); + + /// Destructor + virtual ~GazeboRosFactory(); + + // Documentation inherited + void Load(int argc, char ** argv) override; + +private: + std::unique_ptr impl_; +}; + +} // namespace gazebo_ros +#endif // GAZEBO_ROS__GAZEBO_ROS_FACTORY_HPP_ diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index 8ae392f20..cb305ac14 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -23,7 +23,9 @@ builtin_interfaces gazebo_dev + gazebo_msgs rclcpp + tinyxml_vendor geometry_msgs sensor_msgs diff --git a/gazebo_ros/src/gazebo_ros_factory.cpp b/gazebo_ros/src/gazebo_ros_factory.cpp new file mode 100644 index 000000000..cdbbdcd35 --- /dev/null +++ b/gazebo_ros/src/gazebo_ros_factory.cpp @@ -0,0 +1,349 @@ +// Copyright 2018 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_ros/gazebo_ros_factory.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gazebo_ros +{ + +class GazeboRosFactoryPrivate +{ +public: + /// Callback when a world is created. + /// \param[in] _world_name The world's name + void OnWorldCreated(const std::string & _world_name); + + /// \brief Function for inserting an entity into Gazebo from ROS Service Call. + /// Supported formats are SDF and URDF and works with both models and lights. + /// \param[in] req Request + /// \param[out] res Response + void SpawnEntity( + gazebo_msgs::srv::SpawnEntity::Request::SharedPtr req, + gazebo_msgs::srv::SpawnEntity::Response::SharedPtr res); + + /// \brief Function for removing an entity from Gazebo from ROS Service Call. + /// \param[in] req Request + /// \param[out] res Response + void DeleteEntity( + gazebo_msgs::srv::DeleteEntity::Request::SharedPtr req, + gazebo_msgs::srv::DeleteEntity::Response::SharedPtr res); + + /// Iterate over all child elements and add tags to the tags. + /// \param[out] _elem SDF element. + /// \param[in] _robot_namespace Namespace to be added. + void AddNamespace( + const sdf::ElementPtr & _elem, + const std::string & _robot_namespace); + + /// \brief World pointer from Gazebo. + gazebo::physics::WorldPtr world_; + + /// Reusable factory SDF for efficiency. + sdf::SDFPtr factory_sdf_ = std::make_shared(); + + /// ROS node for communication, managed by gazebo_ros. + gazebo_ros::Node::SharedPtr ros_node_; + + /// \brief ROS service to handle requests/responses to spawn entities. + rclcpp::Service::SharedPtr spawn_service_; + + /// \brief ROS service to handle requests/responses to delete entities. + rclcpp::Service::SharedPtr delete_service_; + + /// Gazebo node for communication. + gazebo::transport::NodePtr gz_node_; + + /// Publishes model factory messages. + gazebo::transport::PublisherPtr gz_factory_pub_; + + /// Publishes light factory messages. + gazebo::transport::PublisherPtr gz_factory_light_pub_; + + /// Publishes request messages. + gazebo::transport::PublisherPtr gz_request_pub_; + + /// To be notified once the world is created. + gazebo::event::ConnectionPtr world_created_connection_; +}; + +GazeboRosFactory::GazeboRosFactory() +: impl_(std::make_unique()) +{ +} + +GazeboRosFactory::~GazeboRosFactory() +{ +} + +void GazeboRosFactory::Load(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + // Keep this in the constructor for performance. + // sdf::initFile causes disk access. + sdf::initFile("root.sdf", impl_->factory_sdf_); + + impl_->world_created_connection_ = gazebo::event::Events::ConnectWorldCreated( + std::bind(&GazeboRosFactoryPrivate::OnWorldCreated, impl_.get(), std::placeholders::_1)); +} + +void GazeboRosFactoryPrivate::OnWorldCreated(const std::string & _world_name) +{ + // Only support one world + world_created_connection_.reset(); + + world_ = gazebo::physics::get_world(); + + // ROS transport + ros_node_ = gazebo_ros::Node::Get(); + + spawn_service_ = ros_node_->create_service("spawn_entity", + std::bind(&GazeboRosFactoryPrivate::SpawnEntity, this, + std::placeholders::_1, std::placeholders::_2)); + + delete_service_ = ros_node_->create_service("delete_entity", + std::bind(&GazeboRosFactoryPrivate::DeleteEntity, this, + std::placeholders::_1, std::placeholders::_2)); + + // Gazebo transport + gz_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node()); + gz_node_->Init(_world_name); + gz_factory_pub_ = gz_node_->Advertise("~/factory"); + gz_factory_light_pub_ = gz_node_->Advertise("~/factory/light"); + gz_request_pub_ = gz_node_->Advertise("~/request"); +} + +void GazeboRosFactoryPrivate::SpawnEntity( + gazebo_msgs::srv::SpawnEntity::Request::SharedPtr req, + gazebo_msgs::srv::SpawnEntity::Response::SharedPtr res) +{ + // Parse XML string into SDF + factory_sdf_->Root()->ClearElements(); + if (!sdf::readString(req->xml, factory_sdf_)) { + res->status_message = "Failed to parse XML string: \n" + req->xml; + res->success = false; + return; + } + + // If the XML has a world, use it as root + auto root = factory_sdf_->Root(); + if (root->HasElement("world")) { + root = root->GetElement("world"); + } + + // Get entity SDF + bool isLight{false}; + sdf::ElementPtr entity_elem{nullptr}; + if (root->HasElement("model")) { + entity_elem = root->GetElement("model"); + } else if (root->HasElement("light")) { + entity_elem = root->GetElement("light"); + isLight = true; + } + + if (!entity_elem) { + res->status_message = "Failed to find a model or light on SDF: \n" + root->ToString(""); + res->success = false; + return; + } + + // Update name if set, otherwise keep name from XML + if (!req->name.empty()) { + entity_elem->GetAttribute("name")->SetFromString(req->name); + + // When name is given and the entity exists, we assume it's an error and don't spawn + // When name is not given, spawn anyway, and Gazebo will append a number to the name as needed + if ((isLight && world_->LightByName(req->name) != nullptr) || + world_->ModelByName(req->name) != nullptr) + { + res->success = false; + res->status_message = "Entity [" + req->name + "] already exists."; + return; + } + } + + // Get desired initial pose + auto initial_xyz = Convert(req->initial_pose.position); + auto initial_q = Convert(req->initial_pose.orientation); + + // reference frame for initial pose definition, modify initial pose if defined + auto frame = world_->EntityByName(req->reference_frame); + if (frame) { + // convert to relative pose + ignition::math::Pose3d frame_pose = frame->WorldPose(); + initial_xyz = frame_pose.Pos() + frame_pose.Rot().RotateVector(initial_xyz); + initial_q = frame_pose.Rot() * initial_q; + } else { + if (req->reference_frame == "" || req->reference_frame == "world" || + req->reference_frame == "map" || req->reference_frame == "/map") + { + RCLCPP_DEBUG(ros_node_->get_logger(), + "SpawnEntity: reference_frame is empty/world/map, using inertial frame"); + } else { + res->success = false; + res->status_message = + "Reference frame [" + req->reference_frame + "] not found, did you forget to scope " + "the link by model name?"; + return; + } + } + + // Add pose from service to pose from XML + auto xml_pose = entity_elem->Get("pose"); + auto new_pose = xml_pose + ignition::math::Pose3d(initial_xyz, initial_q); + + entity_elem->GetElement("pose")->Set(new_pose); + + // Walk recursively through the entire file, and add tags to all plugins + auto robot_namespace = req->robot_namespace; + if (!robot_namespace.empty()) { + AddNamespace(entity_elem, robot_namespace); + } + + // Publish factory message + if (isLight) { + auto msg = gazebo::msgs::LightFromSDF(entity_elem); + gz_factory_light_pub_->Publish(msg); + } else { + std::ostringstream sdfStr; + sdfStr << "" << entity_elem->ToString("") << ""; + gazebo::msgs::Factory msg; + msg.set_sdf(sdfStr.str()); + gz_factory_pub_->Publish(msg); + } + + // Only verify spawning if name is set, to keep ROS 1 functionality, but it would be more + // complicated to check in case Gazebo appended a number to the model name. + // TODO(louise) Revisit this once Gazebo provides a factory service that returns the final name. + if (req->name.empty()) { + res->success = true; + res->status_message = "Successfully sent factory message to Gazebo."; + return; + } + + rclcpp::Time timeout = ros_node_->now() + rclcpp::Duration(10, 0); + + while (rclcpp::ok()) { + if (ros_node_->now() > timeout) { + res->success = false; + res->status_message = "Entity pushed to spawn queue, but spawn service timed out" + "waiting for entity to appear in simulation under the name [" + req->name + "]"; + return; + } + + if ((isLight && world_->LightByName(req->name) != nullptr) || + (world_->ModelByName(req->name) != nullptr)) + { + break; + } + usleep(2000); + } + + // set result + res->success = true; + res->status_message = "SpawnEntity: Successfully spawned entity [" + req->name + "]"; +} + +void GazeboRosFactoryPrivate::DeleteEntity( + gazebo_msgs::srv::DeleteEntity::Request::SharedPtr req, + gazebo_msgs::srv::DeleteEntity::Response::SharedPtr res) +{ + auto entity = world_->EntityByName(req->name); + if (!entity) { + res->success = false; + res->status_message = "Entity [" + req->name + "] does not exist"; + return; + } + + // send delete request + auto msg = gazebo::msgs::CreateRequest("entity_delete", req->name); + gz_request_pub_->Publish(*msg, true); + + // Wait and verify that entity is deleted + rclcpp::Time timeout = ros_node_->now() + rclcpp::Duration(10, 0); + while (rclcpp::ok()) { + if (ros_node_->now() > timeout) { + res->success = false; + res->status_message = + "Delete service timed out waiting for entity to disappear from simulation"; + return; + } + + if (!world_->EntityByName(req->name)) { + break; + } + usleep(1000); + } + + res->success = true; + res->status_message = "Successfully deleted entity [" + req->name + "]"; +} + +void GazeboRosFactoryPrivate::AddNamespace( + const sdf::ElementPtr & _elem, + const std::string & _robot_namespace) +{ + for (sdf::ElementPtr child_elem = _elem->GetFirstElement(); child_elem; + child_elem = child_elem->GetNextElement()) + { + // + if (child_elem->GetName() == "plugin") { + // Get / create + auto ros_elem = child_elem->GetElement("ros"); + + // Get namespace element + sdf::ElementPtr ns_elem; + if (ros_elem->HasElement("namespace")) { + ns_elem = ros_elem->GetElement("namespace"); + } else { + // Tell SDF it's ok for to have element with a string value + auto namespace_desc = std::make_shared(); + namespace_desc->SetName("namespace"); + namespace_desc->AddValue("string", "default", true, "ROS namespace"); + ros_elem->AddElementDescription(namespace_desc); + + // Then we can add the element + ns_elem = ros_elem->AddElement("namespace"); + } + + // Set namespace + ns_elem->Set(_robot_namespace); + } + AddNamespace(child_elem, _robot_namespace); + } +} + +GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosFactory) + +} // namespace gazebo_ros diff --git a/gazebo_ros/test/CMakeLists.txt b/gazebo_ros/test/CMakeLists.txt index 7dbad7ac6..79aab33c1 100644 --- a/gazebo_ros/test/CMakeLists.txt +++ b/gazebo_ros/test/CMakeLists.txt @@ -1,5 +1,6 @@ find_package(ament_cmake_gtest REQUIRED) find_package(builtin_interfaces REQUIRED) +find_package(gazebo_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) @@ -29,6 +30,7 @@ file(COPY worlds DESTINATION .) # Tests set(tests test_conversions + test_gazebo_ros_factory test_node test_plugins test_sim_time @@ -47,11 +49,13 @@ foreach(test ${tests}) target_link_libraries(${test} gazebo_ros_node gazebo_ros_utils + gazebo_test_fixture ) ament_target_dependencies(${test} "builtin_interfaces" - "geometry_msgs" "gazebo_dev" + "gazebo_msgs" + "geometry_msgs" "rclcpp" "sensor_msgs" "std_msgs" diff --git a/gazebo_ros/test/test_gazebo_ros_factory.cpp b/gazebo_ros/test/test_gazebo_ros_factory.cpp new file mode 100644 index 000000000..6648589c2 --- /dev/null +++ b/gazebo_ros/test/test_gazebo_ros_factory.cpp @@ -0,0 +1,211 @@ +// Copyright 2018 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 +#include +#include +#include +#include + +class GazeboRosFactoryTest : public gazebo::ServerFixture +{ +}; + +// Since the plugin calls rclcpp:init, and that can be called only once, we can only run one test +TEST_F(GazeboRosFactoryTest, SpawnDelete) +{ + // Load empty world with factory plugin and start paused + this->LoadArgs("-u --verbose -s libgazebo_ros_factory.so"); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Create ROS clients + auto node = std::make_shared("gazebo_ros_factory_test"); + ASSERT_NE(nullptr, node); + + auto spawn_client = node->create_client("spawn_entity"); + ASSERT_NE(nullptr, spawn_client); + EXPECT_TRUE(spawn_client->wait_for_service(std::chrono::seconds(1))); + + auto delete_client = node->create_client("delete_entity"); + ASSERT_NE(nullptr, delete_client); + EXPECT_TRUE(delete_client->wait_for_service(std::chrono::seconds(1))); + + // Spawn SDF model + { + // Check it has no box model + EXPECT_EQ(nullptr, world->ModelByName("sdf_box")); + + // Request spawn + auto request = std::make_shared(); + request->name = "sdf_box"; + request->initial_pose.position.x = 1.0; + request->initial_pose.position.y = 2.0; + request->initial_pose.position.z = 3.0; + request->xml = + "" + "" + "" + "true" + "" + "" + "" + "1.0" + "" + "" + "" + "" + ""; + + auto response_future = spawn_client->async_send_request(request); + EXPECT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, response_future)); + + auto response = response_future.get(); + ASSERT_NE(nullptr, response); + EXPECT_TRUE(response->success); + + // Check it was spawned with the correct name + ASSERT_NE(nullptr, world->ModelByName("sdf_box")); + EXPECT_EQ(world->ModelByName("sdf_box")->WorldPose(), ignition::math::Pose3d(1, 2, 3, 0, 0, 0)); + } + + // Spawn URDF model + { + // Check it has no box model + EXPECT_EQ(nullptr, world->ModelByName("urdf_box")); + + // Request spawn + auto request = std::make_shared(); + request->initial_pose.position.x = 1.0; + request->initial_pose.position.y = 2.0; + request->initial_pose.position.z = 3.0; + request->reference_frame = "sdf_box"; + request->xml = + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + "" + ""; + + auto response_future = spawn_client->async_send_request(request); + EXPECT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, response_future)); + + auto response = response_future.get(); + ASSERT_NE(nullptr, response); + EXPECT_TRUE(response->success); + + // Name was not set, so we wait for spawn + unsigned int sleep{0}; + unsigned int max_sleep{30}; + while (sleep++ < max_sleep && !world->ModelByName("urdf_box")) { + usleep(100000); + } + + // Check it was spawned with the correct name + ASSERT_NE(nullptr, world->ModelByName("urdf_box")); + EXPECT_EQ(world->ModelByName("urdf_box")->WorldPose(), + ignition::math::Pose3d(2, 4, 6, 0, 0, 0)); + } + + // Spawn SDF light + { + // Check it has no dir light + EXPECT_EQ(nullptr, world->LightByName("dir")); + + // Request spawn + auto request = std::make_shared(); + request->name = "dir"; + request->initial_pose.position.z = 10.0; + request->reference_frame = "world"; + request->xml = + "" + "" + "" + ""; + + auto response_future = spawn_client->async_send_request(request); + EXPECT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, response_future)); + + auto response = response_future.get(); + ASSERT_NE(nullptr, response); + EXPECT_TRUE(response->success); + + // Check it was spawned with the correct name + ASSERT_NE(nullptr, world->LightByName("dir")); + EXPECT_EQ(world->LightByName("dir")->WorldPose(), + ignition::math::Pose3d(0, 0, 10, 0, 0, 0)); + } + + // Delete model + { + // Check model exists + EXPECT_NE(nullptr, world->ModelByName("ground_plane")); + + // Request delete + auto request = std::make_shared(); + request->name = "ground_plane"; + + auto response_future = delete_client->async_send_request(request); + EXPECT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, response_future)); + + auto response = response_future.get(); + ASSERT_NE(nullptr, response); + EXPECT_TRUE(response->success); + + // Check it was deleted + EXPECT_EQ(nullptr, world->ModelByName("ground_plane")); + } + + // Delete light + { + // Check light exists + EXPECT_NE(nullptr, world->LightByName("sun")); + + // Request delete + auto request = std::make_shared(); + request->name = "sun"; + + auto response_future = delete_client->async_send_request(request); + EXPECT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, response_future)); + + auto response = response_future.get(); + ASSERT_NE(nullptr, response); + EXPECT_TRUE(response->success); + + // Check it was deleted + EXPECT_EQ(nullptr, world->LightByName("sun")); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}