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 fe549d8a3..468500af4 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 @@ -53,20 +53,9 @@ #include "gazebo_msgs/SetPhysicsProperties.h" #include "gazebo_msgs/GetPhysicsProperties.h" -#include "gazebo_msgs/SetJointProperties.h" -#include "gazebo_msgs/GetWorldProperties.h" - -#include "gazebo_msgs/GetModelProperties.h" - -#include "gazebo_msgs/GetJointProperties.h" #include "gazebo_msgs/ApplyJointEffort.h" -#include "gazebo_msgs/GetLinkProperties.h" -#include "gazebo_msgs/SetLinkProperties.h" - -#include "gazebo_msgs/GetLightProperties.h" -#include "gazebo_msgs/SetLightProperties.h" // Topics #include "geometry_msgs/Vector3.h" @@ -117,36 +106,12 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief advertise services void advertiseServices(); - /// \brief - bool getModelProperties(gazebo_msgs::GetModelProperties::Request &req,gazebo_msgs::GetModelProperties::Response &res); - - /// \brief - bool getWorldProperties(gazebo_msgs::GetWorldProperties::Request &req,gazebo_msgs::GetWorldProperties::Response &res); - - /// \brief - bool getJointProperties(gazebo_msgs::GetJointProperties::Request &req,gazebo_msgs::GetJointProperties::Response &res); - - /// \brief - bool getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req,gazebo_msgs::GetLinkProperties::Response &res); - - /// \brief - bool getLightProperties(gazebo_msgs::GetLightProperties::Request &req,gazebo_msgs::GetLightProperties::Response &res); - - /// \brief - bool setLightProperties(gazebo_msgs::SetLightProperties::Request &req,gazebo_msgs::SetLightProperties::Response &res); - - /// \brief - bool setLinkProperties(gazebo_msgs::SetLinkProperties::Request &req,gazebo_msgs::SetLinkProperties::Response &res); - /// \brief bool setPhysicsProperties(gazebo_msgs::SetPhysicsProperties::Request &req,gazebo_msgs::SetPhysicsProperties::Response &res); /// \brief bool getPhysicsProperties(gazebo_msgs::GetPhysicsProperties::Request &req,gazebo_msgs::GetPhysicsProperties::Response &res); - /// \brief - bool setJointProperties(gazebo_msgs::SetJointProperties::Request &req,gazebo_msgs::SetJointProperties::Response &res); - /// \brief bool applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req,gazebo_msgs::ApplyJointEffort::Response &res); @@ -215,17 +180,9 @@ class GazeboRosApiPlugin : public SystemPlugin gazebo::event::ConnectionPtr time_update_event_; gazebo::event::ConnectionPtr load_gazebo_ros_api_plugin_event_; - ros::ServiceServer get_model_properties_service_; - ros::ServiceServer get_world_properties_service_; - ros::ServiceServer get_joint_properties_service_; - ros::ServiceServer get_link_properties_service_; - ros::ServiceServer get_light_properties_service_; - ros::ServiceServer set_light_properties_service_; - ros::ServiceServer set_link_properties_service_; ros::ServiceServer set_physics_properties_service_; ros::ServiceServer get_physics_properties_service_; ros::ServiceServer apply_body_wrench_service_; - ros::ServiceServer set_joint_properties_service_; ros::ServiceServer apply_joint_effort_service_; ros::ServiceServer set_model_configuration_service_; ros::ServiceServer clear_joint_forces_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 2b3c93fa2..a0fe4994a 100644 --- a/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -221,60 +221,6 @@ void GazeboRosApiPlugin::advertiseServices() pub_clock_ = nh_->advertise("/clock",10); - // Advertise more services on the custom queue - std::string get_model_properties_service_name("get_model_properties"); - ros::AdvertiseServiceOptions get_model_properties_aso = - ros::AdvertiseServiceOptions::create( - get_model_properties_service_name, - boost::bind(&GazeboRosApiPlugin::getModelProperties,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - get_model_properties_service_ = nh_->advertiseService(get_model_properties_aso); - - // Advertise more services on the custom queue - std::string get_world_properties_service_name("get_world_properties"); - ros::AdvertiseServiceOptions get_world_properties_aso = - ros::AdvertiseServiceOptions::create( - get_world_properties_service_name, - boost::bind(&GazeboRosApiPlugin::getWorldProperties,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - get_world_properties_service_ = nh_->advertiseService(get_world_properties_aso); - - // Advertise more services on the custom queue - std::string get_joint_properties_service_name("get_joint_properties"); - ros::AdvertiseServiceOptions get_joint_properties_aso = - ros::AdvertiseServiceOptions::create( - get_joint_properties_service_name, - boost::bind(&GazeboRosApiPlugin::getJointProperties,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - get_joint_properties_service_ = nh_->advertiseService(get_joint_properties_aso); - - // Advertise more services on the custom queue - std::string get_link_properties_service_name("get_link_properties"); - ros::AdvertiseServiceOptions get_link_properties_aso = - ros::AdvertiseServiceOptions::create( - get_link_properties_service_name, - boost::bind(&GazeboRosApiPlugin::getLinkProperties,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - get_link_properties_service_ = nh_->advertiseService(get_link_properties_aso); - - // Advertise more services on the custom queue - std::string get_light_properties_service_name("get_light_properties"); - ros::AdvertiseServiceOptions get_light_properties_aso = - ros::AdvertiseServiceOptions::create( - get_light_properties_service_name, - boost::bind(&GazeboRosApiPlugin::getLightProperties,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - get_light_properties_service_ = nh_->advertiseService(get_light_properties_aso); - - // Advertise more services on the custom queue - std::string set_light_properties_service_name("set_light_properties"); - ros::AdvertiseServiceOptions set_light_properties_aso = - ros::AdvertiseServiceOptions::create( - set_light_properties_service_name, - boost::bind(&GazeboRosApiPlugin::setLightProperties,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - set_light_properties_service_ = nh_->advertiseService(set_light_properties_aso); - // Advertise more services on the custom queue std::string get_physics_properties_service_name("get_physics_properties"); ros::AdvertiseServiceOptions get_physics_properties_aso = @@ -284,15 +230,6 @@ void GazeboRosApiPlugin::advertiseServices() ros::VoidPtr(), &gazebo_queue_); get_physics_properties_service_ = nh_->advertiseService(get_physics_properties_aso); - // Advertise more services on the custom queue - std::string set_link_properties_service_name("set_link_properties"); - ros::AdvertiseServiceOptions set_link_properties_aso = - ros::AdvertiseServiceOptions::create( - set_link_properties_service_name, - boost::bind(&GazeboRosApiPlugin::setLinkProperties,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - set_link_properties_service_ = nh_->advertiseService(set_link_properties_aso); - // Advertise more services on the custom queue std::string set_physics_properties_service_name("set_physics_properties"); ros::AdvertiseServiceOptions set_physics_properties_aso = @@ -311,15 +248,6 @@ void GazeboRosApiPlugin::advertiseServices() ros::VoidPtr(), &gazebo_queue_); set_model_configuration_service_ = nh_->advertiseService(set_model_configuration_aso); - // Advertise more services on the custom queue - std::string set_joint_properties_service_name("set_joint_properties"); - ros::AdvertiseServiceOptions set_joint_properties_aso = - ros::AdvertiseServiceOptions::create( - set_joint_properties_service_name, - boost::bind(&GazeboRosApiPlugin::setJointProperties,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - set_joint_properties_service_ = nh_->advertiseService(set_joint_properties_aso); - // Advertise more services on the custom queue std::string apply_body_wrench_service_name("apply_body_wrench"); ros::AdvertiseServiceOptions apply_body_wrench_aso = @@ -369,297 +297,6 @@ void GazeboRosApiPlugin::advertiseServices() #endif } -bool GazeboRosApiPlugin::getModelProperties(gazebo_msgs::GetModelProperties::Request &req, - gazebo_msgs::GetModelProperties::Response &res) -{ -#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", "GetModelProperties: model [%s] does not exist",req.model_name.c_str()); - res.success = false; - res.status_message = "GetModelProperties: model does not exist"; - return true; - } - else - { - // get model parent name - gazebo::physics::ModelPtr parent_model = boost::dynamic_pointer_cast(model->GetParent()); - if (parent_model) res.parent_model_name = parent_model->GetName(); - - // get list of child bodies, geoms - res.body_names.clear(); - res.geom_names.clear(); - for (unsigned int i = 0 ; i < model->GetChildCount(); i ++) - { - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(model->GetChild(i)); - if (body) - { - res.body_names.push_back(body->GetName()); - // get list of geoms - for (unsigned int j = 0; j < body->GetChildCount() ; j++) - { - gazebo::physics::CollisionPtr geom = boost::dynamic_pointer_cast(body->GetChild(j)); - if (geom) - res.geom_names.push_back(geom->GetName()); - } - } - } - - // get list of joints - res.joint_names.clear(); - - gazebo::physics::Joint_V joints = model->GetJoints(); - for (unsigned int i=0;i< joints.size(); i++) - res.joint_names.push_back( joints[i]->GetName() ); - - // get children model names - res.child_model_names.clear(); - for (unsigned int j = 0; j < model->GetChildCount(); j++) - { - gazebo::physics::ModelPtr child_model = boost::dynamic_pointer_cast(model->GetChild(j)); - if (child_model) - res.child_model_names.push_back(child_model->GetName() ); - } - - // is model static - res.is_static = model->IsStatic(); - - res.success = true; - res.status_message = "GetModelProperties: got properties"; - return true; - } - return true; -} - -bool GazeboRosApiPlugin::getWorldProperties(gazebo_msgs::GetWorldProperties::Request &req, - gazebo_msgs::GetWorldProperties::Response &res) -{ - res.model_names.clear(); -#if GAZEBO_MAJOR_VERSION >= 8 - res.sim_time = world_->SimTime().Double(); - for (unsigned int i = 0; i < world_->ModelCount(); i ++) - res.model_names.push_back(world_->ModelByIndex(i)->GetName()); -#else - res.sim_time = world_->GetSimTime().Double(); - for (unsigned int i = 0; i < world_->GetModelCount(); i ++) - res.model_names.push_back(world_->GetModel(i)->GetName()); -#endif - gzerr << "disablign rendering has not been implemented, rendering is always enabled\n"; - res.rendering_enabled = true; //world->GetRenderEngineEnabled(); - res.success = true; - res.status_message = "GetWorldProperties: got properties"; - return true; -} - -bool GazeboRosApiPlugin::getJointProperties(gazebo_msgs::GetJointProperties::Request &req, - gazebo_msgs::GetJointProperties::Response &res) -{ - gazebo::physics::JointPtr joint; -#if GAZEBO_MAJOR_VERSION >= 8 - for (unsigned int i = 0; i < world_->ModelCount(); i ++) - { - joint = world_->ModelByIndex(i)->GetJoint(req.joint_name); -#else - for (unsigned int i = 0; i < world_->GetModelCount(); i ++) - { - joint = world_->GetModel(i)->GetJoint(req.joint_name); -#endif - if (joint) break; - } - - if (!joint) - { - res.success = false; - res.status_message = "GetJointProperties: joint not found"; - return true; - } - else - { - /// @todo: FIXME - res.type = res.REVOLUTE; - - res.damping.clear(); // to be added to gazebo - //res.damping.push_back(joint->GetDamping(0)); - - res.position.clear(); -#if GAZEBO_MAJOR_VERSION >= 8 - res.position.push_back(joint->Position(0)); -#else - res.position.push_back(joint->GetAngle(0).Radian()); -#endif - - res.rate.clear(); // use GetVelocity(i) - res.rate.push_back(joint->GetVelocity(0)); - - res.success = true; - res.status_message = "GetJointProperties: got properties"; - return true; - } -} - -bool GazeboRosApiPlugin::getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req, - gazebo_msgs::GetLinkProperties::Response &res) -{ -#if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(req.link_name)); -#else - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->GetEntity(req.link_name)); -#endif - if (!body) - { - res.success = false; - res.status_message = "GetLinkProperties: link not found, did you forget to scope the link by model name?"; - return true; - } - else - { - /// @todo: validate - res.gravity_mode = body->GetGravityMode(); - - gazebo::physics::InertialPtr inertia = body->GetInertial(); - -#if GAZEBO_MAJOR_VERSION >= 8 - res.mass = body->GetInertial()->Mass(); - - res.ixx = inertia->IXX(); - res.iyy = inertia->IYY(); - res.izz = inertia->IZZ(); - res.ixy = inertia->IXY(); - res.ixz = inertia->IXZ(); - res.iyz = inertia->IYZ(); - - ignition::math::Vector3d com = body->GetInertial()->CoG(); -#else - res.mass = body->GetInertial()->GetMass(); - - res.ixx = inertia->GetIXX(); - res.iyy = inertia->GetIYY(); - res.izz = inertia->GetIZZ(); - res.ixy = inertia->GetIXY(); - res.ixz = inertia->GetIXZ(); - res.iyz = inertia->GetIYZ(); - - ignition::math::Vector3d com = body->GetInertial()->GetCoG().Ign(); -#endif - res.com.position.x = com.X(); - res.com.position.y = com.Y(); - res.com.position.z = com.Z(); - res.com.orientation.x = 0; // @todo: gazebo do not support rotated inertia yet - res.com.orientation.y = 0; - res.com.orientation.z = 0; - res.com.orientation.w = 1; - - res.success = true; - res.status_message = "GetLinkProperties: got properties"; - return true; - } -} - -bool GazeboRosApiPlugin::getLightProperties(gazebo_msgs::GetLightProperties::Request &req, - gazebo_msgs::GetLightProperties::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 = "getLightProperties: Requested light " + req.light_name + " not found!"; - } - else - { - gazebo::msgs::Light light; - phy_light->FillMsg(light); - - res.diffuse.r = light.diffuse().r(); - res.diffuse.g = light.diffuse().g(); - res.diffuse.b = light.diffuse().b(); - res.diffuse.a = light.diffuse().a(); - - res.attenuation_constant = light.attenuation_constant(); - res.attenuation_linear = light.attenuation_linear(); - res.attenuation_quadratic = light.attenuation_quadratic(); - - res.success = true; - } - - return true; -} - -bool GazeboRosApiPlugin::setLightProperties(gazebo_msgs::SetLightProperties::Request &req, - gazebo_msgs::SetLightProperties::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 = "setLightProperties: Requested light " + req.light_name + " not found!"; - } - else - { - gazebo::msgs::Light light; - - phy_light->FillMsg(light); - - light.mutable_diffuse()->set_r(req.diffuse.r); - light.mutable_diffuse()->set_g(req.diffuse.g); - light.mutable_diffuse()->set_b(req.diffuse.b); - light.mutable_diffuse()->set_a(req.diffuse.a); - - light.set_attenuation_constant(req.attenuation_constant); - light.set_attenuation_linear(req.attenuation_linear); - light.set_attenuation_quadratic(req.attenuation_quadratic); - - light_modify_pub_->Publish(light, true); - - res.success = true; - } - - return true; -} - -bool GazeboRosApiPlugin::setLinkProperties(gazebo_msgs::SetLinkProperties::Request &req, - gazebo_msgs::SetLinkProperties::Response &res) -{ -#if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(req.link_name)); -#else - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->GetEntity(req.link_name)); -#endif - if (!body) - { - res.success = false; - res.status_message = "SetLinkProperties: link not found, did you forget to scope the link by model name?"; - return true; - } - else - { - gazebo::physics::InertialPtr mass = body->GetInertial(); - // @todo: FIXME: add inertia matrix rotation to Gazebo - // mass.SetInertiaRotation(ignition::math::Quaterniondion(req.com.orientation.w,res.com.orientation.x,req.com.orientation.y req.com.orientation.z)); - mass->SetCoG(ignition::math::Vector3d(req.com.position.x,req.com.position.y,req.com.position.z)); - mass->SetInertiaMatrix(req.ixx,req.iyy,req.izz,req.ixy,req.ixz,req.iyz); - mass->SetMass(req.mass); - body->SetGravityMode(req.gravity_mode); - // @todo: mass change unverified - res.success = true; - res.status_message = "SetLinkProperties: properties set"; - return true; - } -} - bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties::Request &req, gazebo_msgs::SetPhysicsProperties::Response &res) { @@ -759,58 +396,6 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: return res.success; } -bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Request &req, - gazebo_msgs::SetJointProperties::Response &res) -{ - /// @todo: current settings only allows for setting of 1DOF joints (e.g. HingeJoint and SliderJoint) correctly. - gazebo::physics::JointPtr joint; -#if GAZEBO_MAJOR_VERSION >= 8 - for (unsigned int i = 0; i < world_->ModelCount(); i ++) - { - joint = world_->ModelByIndex(i)->GetJoint(req.joint_name); -#else - for (unsigned int i = 0; i < world_->GetModelCount(); i ++) - { - joint = world_->GetModel(i)->GetJoint(req.joint_name); -#endif - if (joint) break; - } - - if (!joint) - { - res.success = false; - res.status_message = "SetJointProperties: joint not found"; - return true; - } - else - { - for(unsigned int i=0;i< req.ode_joint_config.damping.size();i++) - joint->SetDamping(i,req.ode_joint_config.damping[i]); - for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++) - joint->SetParam("hi_stop",i,req.ode_joint_config.hiStop[i]); - for(unsigned int i=0;i< req.ode_joint_config.loStop.size();i++) - joint->SetParam("lo_stop",i,req.ode_joint_config.loStop[i]); - for(unsigned int i=0;i< req.ode_joint_config.erp.size();i++) - joint->SetParam("erp",i,req.ode_joint_config.erp[i]); - for(unsigned int i=0;i< req.ode_joint_config.cfm.size();i++) - joint->SetParam("cfm",i,req.ode_joint_config.cfm[i]); - for(unsigned int i=0;i< req.ode_joint_config.stop_erp.size();i++) - joint->SetParam("stop_erp",i,req.ode_joint_config.stop_erp[i]); - for(unsigned int i=0;i< req.ode_joint_config.stop_cfm.size();i++) - joint->SetParam("stop_cfm",i,req.ode_joint_config.stop_cfm[i]); - for(unsigned int i=0;i< req.ode_joint_config.fudge_factor.size();i++) - joint->SetParam("fudge_factor",i,req.ode_joint_config.fudge_factor[i]); - for(unsigned int i=0;i< req.ode_joint_config.fmax.size();i++) - joint->SetParam("fmax",i,req.ode_joint_config.fmax[i]); - for(unsigned int i=0;i< req.ode_joint_config.vel.size();i++) - joint->SetParam("vel",i,req.ode_joint_config.vel[i]); - - res.success = true; - res.status_message = "SetJointProperties: properties set"; - return true; - } -} - bool GazeboRosApiPlugin::applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req, gazebo_msgs::ApplyJointEffort::Response &res) { diff --git a/gazebo_msgs/CMakeLists.txt b/gazebo_msgs/CMakeLists.txt index 616364720..5286a32f1 100644 --- a/gazebo_msgs/CMakeLists.txt +++ b/gazebo_msgs/CMakeLists.txt @@ -44,6 +44,7 @@ set(srv_files "srv/GetLightProperties.srv" "srv/GetLinkProperties.srv" "srv/GetLinkState.srv" + "srv/GetModelList.srv" "srv/GetModelProperties.srv" "srv/GetModelState.srv" "srv/GetPhysicsProperties.srv" diff --git a/gazebo_msgs/srv/GetJointProperties.srv b/gazebo_msgs/srv/GetJointProperties.srv index dd207bd56..3d75430b6 100644 --- a/gazebo_msgs/srv/GetJointProperties.srv +++ b/gazebo_msgs/srv/GetJointProperties.srv @@ -1,18 +1,18 @@ -string joint_name # name of joint +string joint_name # Scoped name of joint --- # joint type uint8 type uint8 REVOLUTE = 0 # single DOF -uint8 CONTINUOUS = 1 # single DOF (revolute w/o joints) +uint8 CONTINUOUS = 1 # single DOF (revolute w/o limits) uint8 PRISMATIC = 2 # single DOF uint8 FIXED = 3 # 0 DOF -uint8 BALL = 4 # 3 DOF +uint8 BALL = 4 # 3 DOF movement, 0 DOF control uint8 UNIVERSAL = 5 # 2 DOF # dynamics properties float64[] damping # joint state float64[] position -float64[] rate +float64[] rate # TODO(chapulina) Rename to velocity # service return status bool success # return true if get successful string status_message # comments if available diff --git a/gazebo_msgs/srv/GetModelList.srv b/gazebo_msgs/srv/GetModelList.srv new file mode 100644 index 000000000..98da46c0c --- /dev/null +++ b/gazebo_msgs/srv/GetModelList.srv @@ -0,0 +1,5 @@ +--- +std_msgs/Header header # Standard metadata for higher-level stamped data types. + # * header.stamp Simulation time when data was collected. +string[] model_names # list of models in the world +bool success # return true if get successful diff --git a/gazebo_msgs/srv/GetModelProperties.srv b/gazebo_msgs/srv/GetModelProperties.srv index c83783dad..7ecf1d2ac 100644 --- a/gazebo_msgs/srv/GetModelProperties.srv +++ b/gazebo_msgs/srv/GetModelProperties.srv @@ -1,9 +1,9 @@ string model_name # name of Gazebo Model --- string parent_model_name # parent model -string canonical_body_name # name of canonical body, body names are prefixed by model name, e.g. pr2::base_link -string[] body_names # list of bodies, body names are prefixed by model name, e.g. pr2::base_link -string[] geom_names # list of geoms +string canonical_body_name # name of canonical link, link names are prefixed by model name, e.g. pr2::base_link +string[] body_names # list of links, link names are prefixed by model name, e.g. pr2::base_link +string[] geom_names # list of collisions string[] joint_names # list of joints attached to the model string[] child_model_names # list of child models bool is_static # returns true if model is static diff --git a/gazebo_msgs/srv/GetWorldProperties.srv b/gazebo_msgs/srv/GetWorldProperties.srv index 422167b9d..ff23b40a2 100644 --- a/gazebo_msgs/srv/GetWorldProperties.srv +++ b/gazebo_msgs/srv/GetWorldProperties.srv @@ -1,3 +1,5 @@ +# Deprecated, kept for ROS 1 bridge. +# Use GetModelList --- float64 sim_time # current sim time string[] model_names # list of models in the world diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 9a1c0fdda..f1f597575 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -83,6 +83,20 @@ target_link_libraries(gazebo_ros_factory ) ament_export_libraries(gazebo_ros_factory) +# gazebo_ros_properties +add_library(gazebo_ros_properties SHARED + src/gazebo_ros_properties.cpp +) +ament_target_dependencies(gazebo_ros_properties + "rclcpp" + "gazebo_dev" + "gazebo_msgs" +) +target_link_libraries(gazebo_ros_properties + gazebo_ros_node +) +ament_export_libraries(gazebo_ros_properties) + # gazebo_ros_state add_library(gazebo_ros_state SHARED src/gazebo_ros_state.cpp @@ -123,6 +137,7 @@ install( gazebo_ros_factory gazebo_ros_init gazebo_ros_node + gazebo_ros_properties gazebo_ros_state gazebo_ros_utils ARCHIVE DESTINATION lib diff --git a/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp b/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp index 310a92216..072d9956d 100644 --- a/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp +++ b/gazebo_ros/include/gazebo_ros/conversions/geometry_msgs.hpp @@ -113,28 +113,6 @@ ignition::math::Vector3d Convert(const geometry_msgs::msg::Point & in) return out; } -/// Generic conversion from a ROS geometry pose message to another type. -/// \param[in] in Input message. -/// \return Conversion result -/// \tparam T Output type -template -T Convert(const geometry_msgs::msg::Pose &) -{ - T::ConversionNotImplemented; -} - -/// \brief Specialized conversion from a ROS pose message to a ROS geometry transform message. -/// \param[in] in ROS pose message to convert. -/// \return A ROS geometry transform message. -template<> -geometry_msgs::msg::Transform Convert(const geometry_msgs::msg::Pose & in) -{ - geometry_msgs::msg::Transform msg; - msg.translation = Convert(in.position); - msg.rotation = in.orientation; - return msg; -} - /// \brief Specialized conversion from an Ignition Math vector to a ROS message. /// \param[in] vec Ignition vector to convert. /// \return ROS geometry vector message @@ -240,5 +218,37 @@ ignition::math::Pose3d Convert(const geometry_msgs::msg::Transform & in) return msg; } +/// Generic conversion from a ROS geometry pose message to another type. +/// \param[in] in Input message. +/// \return Conversion result +/// \tparam T Output type +template +T Convert(const geometry_msgs::msg::Pose &) +{ + T::ConversionNotImplemented; +} + +/// \brief Specialized conversion from a ROS pose message to a ROS geometry transform message. +/// \param[in] in ROS pose message to convert. +/// \return A ROS geometry transform message. +template<> +geometry_msgs::msg::Transform Convert(const geometry_msgs::msg::Pose & in) +{ + geometry_msgs::msg::Transform msg; + msg.translation = Convert(in.position); + msg.rotation = in.orientation; + return msg; +} + +/// \brief Specialized conversion from a ROS pose message to an Ignition Math pose. +/// \param[in] in ROS pose message to convert. +/// \return Ignition Math pose. +template<> +ignition::math::Pose3d Convert(const geometry_msgs::msg::Pose & in) +{ + return {Convert(in.position), + Convert(in.orientation)}; +} + } // namespace gazebo_ros #endif // GAZEBO_ROS__CONVERSIONS__GEOMETRY_MSGS_HPP_ diff --git a/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp b/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp new file mode 100644 index 000000000..e7ff0d145 --- /dev/null +++ b/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp @@ -0,0 +1,73 @@ +// 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_PROPERTIES_HPP_ +#define GAZEBO_ROS__GAZEBO_ROS_PROPERTIES_HPP_ + +#include + +#include + +namespace gazebo_ros +{ + +class GazeboRosPropertiesPrivate; + +/// Provides services and topics to query and set the properties of entities in simulation. +/// +/// Services: +/// +/// get_model_properties (gazebo_msgs::srv::GetModelProperties) +/// Get a model's properties including +/// parent's name, links, collisions, joints and child links. +/// +/// get_joint_properties (gazebo_msgs::srv::GetJointProperties) +/// Get a joint's properties including +/// joint type, dynamics, position and velocity. +/// +/// set_joint_properties (gazebo_msgs::srv::SetJointProperties) +/// Set OJE joint properties. +/// +/// get_link_properties (gazebo_msgs::srv::GetLinkProperties) +/// Get a link's properties including +/// center of mass, gravity, mass and inertia. +/// +/// set_link_properties (gazebo_msgs::srv::SetLinkProperties) +/// Set a link's properties including +/// center of mass, gravity, mass and inertia. +/// +/// get_light_properties (gazebo_msgs::srv::GetLightProperties) +/// Get light's properties including color and attenuation. +/// +/// set_light_properties (gazebo_msgs::srv::SetLightProperties) +/// Set light's properties including color and attenuation. +/// +class GazeboRosProperties : public gazebo::WorldPlugin +{ +public: + /// Constructor + GazeboRosProperties(); + + /// Destructor + virtual ~GazeboRosProperties(); + + // Documentation inherited + void Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf) override; + +private: + std::unique_ptr impl_; +}; + +} // namespace gazebo_ros +#endif // GAZEBO_ROS__GAZEBO_ROS_PROPERTIES_HPP_ diff --git a/gazebo_ros/src/gazebo_ros_factory.cpp b/gazebo_ros/src/gazebo_ros_factory.cpp index 42d415b76..7c641d224 100644 --- a/gazebo_ros/src/gazebo_ros_factory.cpp +++ b/gazebo_ros/src/gazebo_ros_factory.cpp @@ -17,11 +17,14 @@ #include #include #include +#include #include #include #include +#include #include #include +#include #include #include #include @@ -44,6 +47,13 @@ class GazeboRosFactoryPrivate /// \param[in] _world_name The world's name void OnWorldCreated(const std::string & _world_name); + /// \brief Function for receiving the model list from a gazebo world. + /// \param[in] Request + /// \param[out] res Response + void GetModelList( + gazebo_msgs::srv::GetModelList::Request::SharedPtr, + gazebo_msgs::srv::GetModelList::Response::SharedPtr res); + /// \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 @@ -75,6 +85,9 @@ class GazeboRosFactoryPrivate /// ROS node for communication, managed by gazebo_ros. gazebo_ros::Node::SharedPtr ros_node_; + /// \brief ROS service to handle requests/responses to get model list. + rclcpp::Service::SharedPtr model_list_service_; + /// \brief ROS service to handle requests/responses to spawn entities. rclcpp::Service::SharedPtr spawn_service_; @@ -126,6 +139,10 @@ void GazeboRosFactoryPrivate::OnWorldCreated(const std::string & _world_name) // ROS transport ros_node_ = gazebo_ros::Node::Get(); + model_list_service_ = ros_node_->create_service("get_model_list", + std::bind(&GazeboRosFactoryPrivate::GetModelList, this, + std::placeholders::_1, std::placeholders::_2)); + spawn_service_ = ros_node_->create_service("spawn_entity", std::bind(&GazeboRosFactoryPrivate::SpawnEntity, this, std::placeholders::_1, std::placeholders::_2)); @@ -142,6 +159,18 @@ void GazeboRosFactoryPrivate::OnWorldCreated(const std::string & _world_name) gz_request_pub_ = gz_node_->Advertise("~/request"); } +void GazeboRosFactoryPrivate::GetModelList( + gazebo_msgs::srv::GetModelList::Request::SharedPtr, + gazebo_msgs::srv::GetModelList::Response::SharedPtr res) +{ + res->header.stamp = Convert(world_->SimTime()); + res->model_names.clear(); + for (unsigned int i = 0; i < world_->ModelCount(); i++) { + res->model_names.push_back(world_->ModelByIndex(i)->GetName()); + } + res->success = true; +} + void GazeboRosFactoryPrivate::SpawnEntity( gazebo_msgs::srv::SpawnEntity::Request::SharedPtr req, gazebo_msgs::srv::SpawnEntity::Response::SharedPtr res) diff --git a/gazebo_ros/src/gazebo_ros_properties.cpp b/gazebo_ros/src/gazebo_ros_properties.cpp new file mode 100644 index 000000000..7876a29ff --- /dev/null +++ b/gazebo_ros/src/gazebo_ros_properties.cpp @@ -0,0 +1,463 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "gazebo_ros/gazebo_ros_properties.hpp" + +namespace gazebo_ros +{ + +class GazeboRosPropertiesPrivate +{ +public: + /// \brief Callback for get model properties service. + /// \param[in] req Request + /// \param[out] res Response + void GetModelProperties( + gazebo_msgs::srv::GetModelProperties::Request::SharedPtr _req, + gazebo_msgs::srv::GetModelProperties::Response::SharedPtr _res); + + /// \brief Callback for get joint properties service. + /// \param[in] req Request + /// \param[out] res Response + void GetJointProperties( + gazebo_msgs::srv::GetJointProperties::Request::SharedPtr _req, + gazebo_msgs::srv::GetJointProperties::Response::SharedPtr _res); + + /// \brief Callback for get link properties service. + /// \param[in] req Request + /// \param[out] res Response + void GetLinkProperties( + gazebo_msgs::srv::GetLinkProperties::Request::SharedPtr _req, + gazebo_msgs::srv::GetLinkProperties::Response::SharedPtr _res); + + /// \brief Callback for get light properties service. + /// \param[in] req Request + /// \param[out] res Response + void GetLightProperties( + gazebo_msgs::srv::GetLightProperties::Request::SharedPtr _req, + gazebo_msgs::srv::GetLightProperties::Response::SharedPtr _res); + + /// \brief Callback for set joint properties service. + /// \param[in] req Request + /// \param[out] res Response + void SetJointProperties( + gazebo_msgs::srv::SetJointProperties::Request::SharedPtr _req, + gazebo_msgs::srv::SetJointProperties::Response::SharedPtr _res); + + /// \brief Callback for set link properties service. + /// \param[in] req Request + /// \param[out] res Response + void SetLinkProperties( + gazebo_msgs::srv::SetLinkProperties::Request::SharedPtr _req, + gazebo_msgs::srv::SetLinkProperties::Response::SharedPtr _res); + + /// \brief Callback for set light properties service. + /// \param[in] req Request + /// \param[out] res Response + void SetLightProperties( + gazebo_msgs::srv::SetLightProperties::Request::SharedPtr _req, + gazebo_msgs::srv::SetLightProperties::Response::SharedPtr _res); + + /// \brief World pointer from Gazebo. + gazebo::physics::WorldPtr world_; + + /// ROS node for communication, managed by gazebo_ros. + gazebo_ros::Node::SharedPtr ros_node_; + + /// \brief ROS service to handle requests for model properties. + rclcpp::Service::SharedPtr get_model_properties_service_; + + /// \brief ROS service to handle requests for joint properties. + rclcpp::Service::SharedPtr get_joint_properties_service_; + + /// \brief ROS service to handle requests for link properties. + rclcpp::Service::SharedPtr get_link_properties_service_; + + /// \brief ROS service to handle requests for light properties. + rclcpp::Service::SharedPtr get_light_properties_service_; + + /// \brief ROS service to handle requests to set joint properties. + rclcpp::Service::SharedPtr set_joint_properties_service_; + + /// \brief ROS service to handle requests to set link properties. + rclcpp::Service::SharedPtr set_link_properties_service_; + + /// \brief ROS service to handle requests to set light properties. + rclcpp::Service::SharedPtr set_light_properties_service_; + + /// Gazebo node for communication. + gazebo::transport::NodePtr gz_node_; + + /// Publishes light modify messages. + gazebo::transport::PublisherPtr gz_properties_light_pub_; +}; + +GazeboRosProperties::GazeboRosProperties() +: impl_(std::make_unique()) +{ +} + +GazeboRosProperties::~GazeboRosProperties() +{ +} + +void GazeboRosProperties::Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf) +{ + impl_->world_ = _world; + + impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); + + impl_->get_model_properties_service_ = + impl_->ros_node_->create_service( + "get_model_properties", std::bind(&GazeboRosPropertiesPrivate::GetModelProperties, impl_.get(), + std::placeholders::_1, std::placeholders::_2)); + + impl_->get_joint_properties_service_ = + impl_->ros_node_->create_service( + "get_joint_properties", std::bind(&GazeboRosPropertiesPrivate::GetJointProperties, impl_.get(), + std::placeholders::_1, std::placeholders::_2)); + + impl_->get_link_properties_service_ = + impl_->ros_node_->create_service( + "get_link_properties", std::bind(&GazeboRosPropertiesPrivate::GetLinkProperties, impl_.get(), + std::placeholders::_1, std::placeholders::_2)); + + impl_->get_light_properties_service_ = + impl_->ros_node_->create_service( + "get_light_properties", std::bind(&GazeboRosPropertiesPrivate::GetLightProperties, impl_.get(), + std::placeholders::_1, std::placeholders::_2)); + + impl_->set_joint_properties_service_ = + impl_->ros_node_->create_service( + "set_joint_properties", std::bind(&GazeboRosPropertiesPrivate::SetJointProperties, impl_.get(), + std::placeholders::_1, std::placeholders::_2)); + + impl_->set_link_properties_service_ = + impl_->ros_node_->create_service( + "set_link_properties", std::bind(&GazeboRosPropertiesPrivate::SetLinkProperties, impl_.get(), + std::placeholders::_1, std::placeholders::_2)); + + impl_->set_light_properties_service_ = + impl_->ros_node_->create_service( + "set_light_properties", std::bind(&GazeboRosPropertiesPrivate::SetLightProperties, impl_.get(), + std::placeholders::_1, std::placeholders::_2)); + + // Gazebo transport + impl_->gz_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node()); + impl_->gz_node_->Init(_world->Name()); + impl_->gz_properties_light_pub_ = + impl_->gz_node_->Advertise("~/light/modify"); +} + +void GazeboRosPropertiesPrivate::GetModelProperties( + gazebo_msgs::srv::GetModelProperties::Request::SharedPtr _req, + gazebo_msgs::srv::GetModelProperties::Response::SharedPtr _res) +{ + gazebo::physics::ModelPtr model = world_->ModelByName(_req->model_name); + if (!model) { + RCLCPP_ERROR( + ros_node_->get_logger(), "GetModelProperties: model [%s] does not exist", + _req->model_name.c_str()); + _res->success = false; + _res->status_message = "GetModelProperties: model does not exist"; + return; + } + // get model parent name + gazebo::physics::ModelPtr parent_model = + boost::dynamic_pointer_cast(model->GetParent()); + if (parent_model) {_res->parent_model_name = parent_model->GetName();} + + // get list of child links, collisions + _res->body_names.clear(); + _res->geom_names.clear(); + for (unsigned int i = 0; i < model->GetChildCount(); ++i) { + gazebo::physics::LinkPtr link = + boost::dynamic_pointer_cast(model->GetChild(i)); + if (link) { + if (link->IsCanonicalLink()) { + _res->canonical_body_name = link->GetScopedName(); + } + _res->body_names.push_back(link->GetScopedName()); + // get list of collisions + for (unsigned int j = 0; j < link->GetChildCount(); j++) { + gazebo::physics::CollisionPtr collision = + boost::dynamic_pointer_cast(link->GetChild(j)); + if (collision) { + _res->geom_names.push_back(collision->GetName()); + } + } + } + } + + // get list of joints + _res->joint_names.clear(); + + gazebo::physics::Joint_V joints = model->GetJoints(); + for (auto & joint : joints) { + _res->joint_names.push_back(joint->GetName()); + } + + // get children model names + _res->child_model_names.clear(); + for (unsigned int j = 0; j < model->GetChildCount(); j++) { + gazebo::physics::ModelPtr child_model = + boost::dynamic_pointer_cast(model->GetChild(j)); + if (child_model) { + _res->child_model_names.push_back(child_model->GetName() ); + } + } + + // is model static + _res->is_static = model->IsStatic(); + + _res->success = true; + _res->status_message = "GetModelProperties: got properties"; +} + +void GazeboRosPropertiesPrivate::GetJointProperties( + gazebo_msgs::srv::GetJointProperties::Request::SharedPtr _req, + gazebo_msgs::srv::GetJointProperties::Response::SharedPtr _res) +{ + gazebo::physics::JointPtr joint; + for (unsigned int i = 0; i < world_->ModelCount(); ++i) { + joint = world_->ModelByIndex(i)->GetJoint(_req->joint_name); + if (joint) { + break; + } + } + + if (!joint) { + _res->success = false; + _res->status_message = "GetJointProperties: joint not found"; + return; + } + + auto type = joint->GetMsgType(); + if (type == gazebo::msgs::Joint::REVOLUTE) { + _res->type = _res->REVOLUTE; + } else if (type == gazebo::msgs::Joint::PRISMATIC) { + _res->type = _res->PRISMATIC; + } else if (type == gazebo::msgs::Joint::UNIVERSAL) { + _res->type = _res->UNIVERSAL; + } else if (type == gazebo::msgs::Joint::BALL) { + _res->type = _res->BALL; + } else if (type == gazebo::msgs::Joint::FIXED) { + _res->type = _res->FIXED; + } else { + RCLCPP_WARN(ros_node_->get_logger(), "Partial support for joint type [%i]", type); + } + + _res->damping.clear(); + _res->position.clear(); + _res->rate.clear(); + for (auto i = 0u; i < joint->DOF(); ++i) { + _res->damping.push_back(joint->GetDamping(i)); + _res->position.push_back(joint->Position(i)); + _res->rate.push_back(joint->GetVelocity(i)); + } + + _res->success = true; + _res->status_message = "GetJointProperties: got properties"; +} + +void GazeboRosPropertiesPrivate::GetLinkProperties( + gazebo_msgs::srv::GetLinkProperties::Request::SharedPtr _req, + gazebo_msgs::srv::GetLinkProperties::Response::SharedPtr _res) +{ + gazebo::physics::LinkPtr link = + boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); + if (!link) { + _res->success = false; + _res->status_message = + "GetLinkProperties: link not found, did you forget to scope the link by model name?"; + } else { + _res->gravity_mode = link->GetGravityMode(); + + gazebo::physics::InertialPtr inertia = link->GetInertial(); + + _res->mass = link->GetInertial()->Mass(); + + _res->ixx = inertia->IXX(); + _res->iyy = inertia->IYY(); + _res->izz = inertia->IZZ(); + _res->ixy = inertia->IXY(); + _res->ixz = inertia->IXZ(); + _res->iyz = inertia->IYZ(); + + auto com = link->GetInertial()->Pose(); + + _res->com.position = gazebo_ros::Convert(com.Pos()); + _res->com.orientation = gazebo_ros::Convert(com.Rot()); + + _res->success = true; + _res->status_message = "GetLinkProperties: got properties"; + } +} + +void GazeboRosPropertiesPrivate::GetLightProperties( + gazebo_msgs::srv::GetLightProperties::Request::SharedPtr _req, + gazebo_msgs::srv::GetLightProperties::Response::SharedPtr _res) +{ + gazebo::physics::LightPtr phy_light = world_->LightByName(_req->light_name); + if (phy_light == nullptr) { + _res->success = false; + _res->status_message = "getLightProperties: Requested light " + _req->light_name + + " not found!"; + } else { + gazebo::msgs::Light light; + phy_light->FillMsg(light); + + _res->diffuse.r = light.diffuse().r(); + _res->diffuse.g = light.diffuse().g(); + _res->diffuse.b = light.diffuse().b(); + _res->diffuse.a = light.diffuse().a(); + + _res->attenuation_constant = light.attenuation_constant(); + _res->attenuation_linear = light.attenuation_linear(); + _res->attenuation_quadratic = light.attenuation_quadratic(); + + _res->success = true; + } +} + +void GazeboRosPropertiesPrivate::SetJointProperties( + gazebo_msgs::srv::SetJointProperties::Request::SharedPtr _req, + gazebo_msgs::srv::SetJointProperties::Response::SharedPtr _res) +{ + /// @todo: current settings only allows for setting of 1DOF joints + /// (e.g. HingeJoint and SliderJoint) correctly. + gazebo::physics::JointPtr joint; + for (unsigned int i = 0; i < world_->ModelCount(); ++i) { + joint = world_->ModelByIndex(i)->GetJoint(_req->joint_name); + if (joint) {break;} + } + + if (!joint) { + _res->success = false; + _res->status_message = "SetJointProperties: joint not found"; + } else { + for (unsigned int i = 0; i < _req->ode_joint_config.damping.size(); ++i) { + joint->SetDamping(i, _req->ode_joint_config.damping[i]); + } + for (unsigned int i = 0; i < _req->ode_joint_config.hi_stop.size(); ++i) { + joint->SetParam("hi_stop", i, _req->ode_joint_config.hi_stop[i]); + } + for (unsigned int i = 0; i < _req->ode_joint_config.lo_stop.size(); ++i) { + joint->SetParam("lo_stop", i, _req->ode_joint_config.lo_stop[i]); + } + for (unsigned int i = 0; i < _req->ode_joint_config.erp.size(); ++i) { + joint->SetParam("erp", i, _req->ode_joint_config.erp[i]); + } + for (unsigned int i = 0; i < _req->ode_joint_config.cfm.size(); ++i) { + joint->SetParam("cfm", i, _req->ode_joint_config.cfm[i]); + } + for (unsigned int i = 0; i < _req->ode_joint_config.stop_erp.size(); ++i) { + joint->SetParam("stop_erp", i, _req->ode_joint_config.stop_erp[i]); + } + for (unsigned int i = 0; i < _req->ode_joint_config.stop_cfm.size(); ++i) { + joint->SetParam("stop_cfm", i, _req->ode_joint_config.stop_cfm[i]); + } + for (unsigned int i = 0; i < _req->ode_joint_config.fudge_factor.size(); ++i) { + joint->SetParam("fudge_factor", i, _req->ode_joint_config.fudge_factor[i]); + } + for (unsigned int i = 0; i < _req->ode_joint_config.fmax.size(); ++i) { + joint->SetParam("fmax", i, _req->ode_joint_config.fmax[i]); + } + for (unsigned int i = 0; i < _req->ode_joint_config.vel.size(); ++i) { + joint->SetParam("vel", i, _req->ode_joint_config.vel[i]); + } + + _res->success = true; + _res->status_message = "SetJointProperties: properties set"; + } +} + +void GazeboRosPropertiesPrivate::SetLinkProperties( + gazebo_msgs::srv::SetLinkProperties::Request::SharedPtr _req, + gazebo_msgs::srv::SetLinkProperties::Response::SharedPtr _res) +{ + gazebo::physics::LinkPtr link = + boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); + if (!link) { + _res->success = false; + _res->status_message = + "SetLinkProperties: link not found, did you forget to scope the link by model name?"; + } else { + gazebo::physics::InertialPtr mass = link->GetInertial(); + mass->SetCoG(gazebo_ros::Convert(_req->com)); + mass->SetInertiaMatrix(_req->ixx, _req->iyy, _req->izz, _req->ixy, _req->ixz, _req->iyz); + mass->SetMass(_req->mass); + link->SetGravityMode(_req->gravity_mode); + + _res->success = true; + _res->status_message = "SetLinkProperties: properties set"; + } +} + +void GazeboRosPropertiesPrivate::SetLightProperties( + gazebo_msgs::srv::SetLightProperties::Request::SharedPtr _req, + gazebo_msgs::srv::SetLightProperties::Response::SharedPtr _res) +{ + gazebo::physics::LightPtr phy_light = world_->LightByName(_req->light_name); + if (phy_light == nullptr) { + _res->success = false; + _res->status_message = "setLightProperties: Requested light " + _req->light_name + + " not found!"; + } else { + gazebo::msgs::Light light; + + phy_light->FillMsg(light); + + light.mutable_diffuse()->set_r(_req->diffuse.r); + light.mutable_diffuse()->set_g(_req->diffuse.g); + light.mutable_diffuse()->set_b(_req->diffuse.b); + light.mutable_diffuse()->set_a(_req->diffuse.a); + + light.set_attenuation_constant(_req->attenuation_constant); + light.set_attenuation_linear(_req->attenuation_linear); + light.set_attenuation_quadratic(_req->attenuation_quadratic); + + gz_properties_light_pub_->Publish(light); + + _res->success = true; + } +} + +GZ_REGISTER_WORLD_PLUGIN(GazeboRosProperties) + +} // namespace gazebo_ros diff --git a/gazebo_ros/test/CMakeLists.txt b/gazebo_ros/test/CMakeLists.txt index 5d698f3b6..2eb56a2b4 100644 --- a/gazebo_ros/test/CMakeLists.txt +++ b/gazebo_ros/test/CMakeLists.txt @@ -33,6 +33,7 @@ set(tests test_conversions test_gazebo_ros_factory test_gazebo_ros_init + test_gazebo_ros_properties test_gazebo_ros_state test_node test_plugins diff --git a/gazebo_ros/test/test_conversions.cpp b/gazebo_ros/test/test_conversions.cpp index 3bce3d180..edb8ce613 100644 --- a/gazebo_ros/test/test_conversions.cpp +++ b/gazebo_ros/test/test_conversions.cpp @@ -61,6 +61,16 @@ TEST(TestConversions, Pose) EXPECT_EQ(0.3, pose_msg.orientation.y); EXPECT_EQ(-0.1, pose_msg.orientation.z); + // ROS Pose to Ign + auto pose_ign = gazebo_ros::Convert(pose_msg); + EXPECT_EQ(0.6, pose_ign.Pos().X()); + EXPECT_EQ(0.5, pose_ign.Pos().Y()); + EXPECT_EQ(0.7, pose_ign.Pos().Z()); + EXPECT_EQ(0.9, pose_ign.Rot().W()); + EXPECT_EQ(0.0, pose_ign.Rot().X()); + EXPECT_EQ(0.3, pose_ign.Rot().Y()); + EXPECT_EQ(-0.1, pose_ign.Rot().Z()); + // ROS Pose to ROS Transform auto transform_msg = gazebo_ros::Convert(pose_msg); EXPECT_EQ(0.6, transform_msg.translation.x); diff --git a/gazebo_ros/test/test_gazebo_ros_factory.cpp b/gazebo_ros/test/test_gazebo_ros_factory.cpp index 6648589c2..f02275e4d 100644 --- a/gazebo_ros/test/test_gazebo_ros_factory.cpp +++ b/gazebo_ros/test/test_gazebo_ros_factory.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -23,7 +24,7 @@ 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) +TEST_F(GazeboRosFactoryTest, SpawnDeleteList) { // Load empty world with factory plugin and start paused this->LoadArgs("-u --verbose -s libgazebo_ros_factory.so"); @@ -36,6 +37,10 @@ TEST_F(GazeboRosFactoryTest, SpawnDelete) auto node = std::make_shared("gazebo_ros_factory_test"); ASSERT_NE(nullptr, node); + auto model_list_client = node->create_client("get_model_list"); + ASSERT_NE(nullptr, model_list_client); + EXPECT_TRUE(model_list_client->wait_for_service(std::chrono::seconds(1))); + auto spawn_client = node->create_client("spawn_entity"); ASSERT_NE(nullptr, spawn_client); EXPECT_TRUE(spawn_client->wait_for_service(std::chrono::seconds(1))); @@ -44,6 +49,90 @@ TEST_F(GazeboRosFactoryTest, SpawnDelete) ASSERT_NE(nullptr, delete_client); EXPECT_TRUE(delete_client->wait_for_service(std::chrono::seconds(1))); + // Get Model List (Spawn two sdf models and check result) + { + // Model 1 + // Check it has no box1 model + EXPECT_EQ(nullptr, world->ModelByName("sdf_box1")); + + // Request spawn box1 + auto request1 = std::make_shared(); + request1->name = "sdf_box1"; + request1->initial_pose.position.x = -1.0; + request1->initial_pose.position.y = -1.0; + request1->initial_pose.position.z = 0.0; + request1->xml = + "" + "" + "" + "true" + "" + "" + "" + "1.0" + "" + "" + "" + "" + ""; + + auto response_future1 = spawn_client->async_send_request(request1); + EXPECT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, response_future1)); + + auto response1 = response_future1.get(); + ASSERT_NE(nullptr, response1); + EXPECT_TRUE(response1->success); + + // Model 2 + // Check it has no box2 model + EXPECT_EQ(nullptr, world->ModelByName("sdf_box2")); + + // Request spawn box2 + auto request2 = std::make_shared(); + request2->name = "sdf_box2"; + request2->initial_pose.position.x = 1.5; + request2->initial_pose.position.y = 1.5; + request2->initial_pose.position.z = 0.0; + request2->xml = + "" + "" + "" + "true" + "" + "" + "" + "2.0" + "" + "" + "" + "" + ""; + + auto response_future2 = spawn_client->async_send_request(request2); + EXPECT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, response_future2)); + + auto response2 = response_future2.get(); + ASSERT_NE(nullptr, response2); + EXPECT_TRUE(response2->success); + + // Check GetModelList + auto request3 = std::make_shared(); + + auto response_future3 = model_list_client->async_send_request(request3); + EXPECT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, response_future3)); + + auto response3 = response_future3.get(); + ASSERT_NE(nullptr, response3); + EXPECT_TRUE(response3->success); + + EXPECT_EQ(response3->model_names[0], "ground_plane"); + EXPECT_EQ(response3->model_names[1], "sdf_box1"); + EXPECT_EQ(response3->model_names[2], "sdf_box2"); + } + // Spawn SDF model { // Check it has no box model diff --git a/gazebo_ros/test/test_gazebo_ros_properties.cpp b/gazebo_ros/test/test_gazebo_ros_properties.cpp new file mode 100644 index 000000000..5316ebb48 --- /dev/null +++ b/gazebo_ros/test/test_gazebo_ros_properties.cpp @@ -0,0 +1,431 @@ +// 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 +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#define tol 0.01 + +class GazeboRosPropertiesTest : public gazebo::ServerFixture +{ +public: + // Documentation inherited + void SetUp() override; + + void GetModelProperties( + const std::string & _model_name); + + void GetJointProperties( + const std::string & _joint_name, + const double & _damping); + + void GetLinkProperties( + const std::string & _link_name, + const bool & _gravity_mode, + const double & _mass, + const double & _ixx, + const double & _ixy, + const double & _ixz, + const double & _iyy, + const double & _iyz, + const double & _izz); + + void GetLightProperties( + const std::string & _light_name, + const ignition::math::Color & _diffuse, + const double & _attenuation_constant, + const double & _attenuation_linear, + const double & _attenuation_quadratic); + + void SetJointProperties( + const std::string & _joint_name, + const double & _damping); + + void SetLinkProperties( + const std::string & _link_name, + const bool & _gravity_mode, + const double & _mass, + const double & _ixx, + const double & _ixy, + const double & _ixz, + const double & _iyy, + const double & _iyz, + const double & _izz); + + void SetLightProperties( + const std::string & _light_name, + const ignition::math::Color & _diffuse, + const double & _attenuation_constant, + const double & _attenuation_linear, + const double & _attenuation_quadratic); + + gazebo::physics::WorldPtr world_; + rclcpp::Node::SharedPtr node_; + std::shared_ptr> + get_model_properties_client_; + std::shared_ptr> + get_joint_properties_client_; + std::shared_ptr> + get_link_properties_client_; + std::shared_ptr> + get_light_properties_client_; + std::shared_ptr> + set_joint_properties_client_; + std::shared_ptr> + set_link_properties_client_; + std::shared_ptr> + set_light_properties_client_; +}; + +void GazeboRosPropertiesTest::SetUp() +{ + // Load world with state plugin and start paused + this->Load("worlds/gazebo_ros_properties_test.world", true); + + // World + world_ = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world_); + + // Create ROS clients + node_ = std::make_shared("gazebo_ros_properties_test"); + ASSERT_NE(nullptr, node_); + + get_model_properties_client_ = + node_->create_client("test/get_model_properties"); + ASSERT_NE(nullptr, get_model_properties_client_); + EXPECT_TRUE(get_model_properties_client_->wait_for_service(std::chrono::seconds(1))); + + get_joint_properties_client_ = + node_->create_client("test/get_joint_properties"); + ASSERT_NE(nullptr, get_joint_properties_client_); + EXPECT_TRUE(get_joint_properties_client_->wait_for_service(std::chrono::seconds(1))); + + get_link_properties_client_ = + node_->create_client("test/get_link_properties"); + ASSERT_NE(nullptr, get_link_properties_client_); + EXPECT_TRUE(get_link_properties_client_->wait_for_service(std::chrono::seconds(1))); + + get_light_properties_client_ = + node_->create_client("test/get_light_properties"); + ASSERT_NE(nullptr, get_light_properties_client_); + EXPECT_TRUE(get_light_properties_client_->wait_for_service(std::chrono::seconds(1))); + + set_joint_properties_client_ = + node_->create_client("test/set_joint_properties"); + ASSERT_NE(nullptr, set_joint_properties_client_); + EXPECT_TRUE(set_joint_properties_client_->wait_for_service(std::chrono::seconds(1))); + + set_link_properties_client_ = + node_->create_client("test/set_link_properties"); + ASSERT_NE(nullptr, set_link_properties_client_); + EXPECT_TRUE(set_link_properties_client_->wait_for_service(std::chrono::seconds(1))); + + set_light_properties_client_ = + node_->create_client("test/set_light_properties"); + ASSERT_NE(nullptr, set_light_properties_client_); + EXPECT_TRUE(set_light_properties_client_->wait_for_service(std::chrono::seconds(1))); +} + +void GazeboRosPropertiesTest::GetModelProperties( + const std::string & _model_name) +{ + // Get spawned Model properties + auto entity = world_->EntityByName(_model_name); + ASSERT_NE(nullptr, entity); + + auto request = std::make_shared(); + request->model_name = _model_name; + + auto response_future = get_model_properties_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); + + // gazebo models simple_arm + EXPECT_EQ(response->parent_model_name, ""); + EXPECT_EQ(response->canonical_body_name, "simple_arm::arm_base"); + + ASSERT_EQ(5u, response->body_names.size()); + EXPECT_EQ(response->body_names[0], "simple_arm::arm_base"); + EXPECT_EQ(response->body_names[1], "simple_arm::arm_shoulder_pan"); + EXPECT_EQ(response->body_names[2], "simple_arm::arm_elbow_pan"); + EXPECT_EQ(response->body_names[3], "simple_arm::arm_wrist_lift"); + EXPECT_EQ(response->body_names[4], "simple_arm::arm_wrist_roll"); + + ASSERT_EQ(9u, response->geom_names.size()); + EXPECT_EQ(response->geom_names[0], "arm_base_collision"); + EXPECT_EQ(response->geom_names[1], "arm_base_collision_arm_trunk"); + EXPECT_EQ(response->geom_names[2], "arm_shoulder_pan_collision"); + EXPECT_EQ(response->geom_names[3], "arm_shoulder_pan_collision_arm_shoulder"); + EXPECT_EQ(response->geom_names[4], "arm_elbow_pan_collision"); + EXPECT_EQ(response->geom_names[5], "arm_elbow_pan_collision_arm_elbow"); + EXPECT_EQ(response->geom_names[6], "arm_elbow_pan_collision_arm_wrist"); + EXPECT_EQ(response->geom_names[7], "arm_wrist_lift_collision"); + EXPECT_EQ(response->geom_names[8], "arm_wrist_roll_collision"); + + ASSERT_EQ(4u, response->joint_names.size()); + EXPECT_EQ(response->joint_names[0], "arm_shoulder_pan_joint"); + EXPECT_EQ(response->joint_names[1], "arm_elbow_pan_joint"); + EXPECT_EQ(response->joint_names[2], "arm_wrist_lift_joint"); + EXPECT_EQ(response->joint_names[3], "arm_wrist_roll_joint"); + + EXPECT_TRUE(response->child_model_names.empty()); + EXPECT_FALSE(response->is_static); +} + +void GazeboRosPropertiesTest::GetJointProperties( + const std::string & _joint_name, + const double & _damping) +{ + auto request = std::make_shared(); + request->joint_name = _joint_name; + + auto response_future = get_joint_properties_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); + + if (_joint_name == "arm_wrist_roll_joint") { + EXPECT_EQ(response->UNIVERSAL, response->type); + EXPECT_EQ(2u, response->damping.size()); + EXPECT_EQ(2u, response->position.size()); + EXPECT_EQ(2u, response->rate.size()); + } else { + EXPECT_EQ(response->REVOLUTE, response->type); + EXPECT_EQ(1u, response->damping.size()); + EXPECT_EQ(1u, response->position.size()); + EXPECT_EQ(1u, response->rate.size()); + } + + if ((response->damping).size() > 0) { + EXPECT_EQ(_damping, response->damping[0]); + } +} + +void GazeboRosPropertiesTest::SetJointProperties( + const std::string & _joint_name, + const double & _damping) +{ + auto request = std::make_shared(); + request->joint_name = _joint_name; + + request->ode_joint_config.damping.push_back(_damping); + + auto response_future = set_joint_properties_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); +} + +void GazeboRosPropertiesTest::GetLinkProperties( + const std::string & _link_name, + const bool & _gravity_mode, + const double & _mass, + const double & _ixx, + const double & _ixy, + const double & _ixz, + const double & _iyy, + const double & _iyz, + const double & _izz) +{ + auto request = std::make_shared(); + request->link_name = _link_name; + + auto response_future = get_link_properties_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); + + EXPECT_EQ(_gravity_mode, response->gravity_mode) << _link_name; + EXPECT_EQ(_mass, response->mass) << _link_name; + EXPECT_EQ(_ixx, response->ixx) << _link_name; + EXPECT_EQ(_ixy, response->ixy) << _link_name; + EXPECT_EQ(_ixz, response->ixz) << _link_name; + EXPECT_EQ(_iyy, response->iyy) << _link_name; + EXPECT_EQ(_iyz, response->iyz) << _link_name; + EXPECT_EQ(_izz, response->izz) << _link_name; +} + +void GazeboRosPropertiesTest::SetLinkProperties( + const std::string & _link_name, + const bool & _gravity_mode, + const double & _mass, + const double & _ixx, + const double & _ixy, + const double & _ixz, + const double & _iyy, + const double & _iyz, + const double & _izz) +{ + auto request = std::make_shared(); + request->link_name = _link_name; + request->gravity_mode = _gravity_mode; + request->mass = _mass; + request->ixx = _ixx; + request->ixy = _ixy; + request->ixz = _ixz; + request->iyy = _iyy; + request->iyz = _iyz; + request->izz = _izz; + + auto response_future = set_link_properties_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); +} + +void GazeboRosPropertiesTest::GetLightProperties( + const std::string & _light_name, + const ignition::math::Color & _diffuse, + const double & _attenuation_constant, + const double & _attenuation_linear, + const double & _attenuation_quadratic) +{ + auto request = std::make_shared(); + request->light_name = _light_name; + + auto response_future = get_light_properties_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); + + EXPECT_DOUBLE_EQ(_diffuse.R(), response->diffuse.r) << _light_name; + EXPECT_DOUBLE_EQ(_diffuse.G(), response->diffuse.g) << _light_name; + EXPECT_DOUBLE_EQ(_diffuse.B(), response->diffuse.b) << _light_name; + EXPECT_DOUBLE_EQ(_diffuse.A(), response->diffuse.a) << _light_name; + + EXPECT_NEAR(_attenuation_constant, response->attenuation_constant, tol) << _light_name; + EXPECT_NEAR(_attenuation_linear, response->attenuation_linear, tol) << _light_name; + EXPECT_NEAR(_attenuation_quadratic, response->attenuation_quadratic, tol) << _light_name; +} + +void GazeboRosPropertiesTest::SetLightProperties( + const std::string & _light_name, + const ignition::math::Color & _diffuse, + const double & _attenuation_constant, + const double & _attenuation_linear, + const double & _attenuation_quadratic) +{ + auto request = std::make_shared(); + request->light_name = _light_name; + request->diffuse.r = _diffuse.R(); + request->diffuse.g = _diffuse.G(); + request->diffuse.b = _diffuse.B(); + request->diffuse.a = _diffuse.A(); + request->attenuation_constant = _attenuation_constant; + request->attenuation_linear = _attenuation_linear; + request->attenuation_quadratic = _attenuation_quadratic; + + auto response_future = set_light_properties_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); +} + +TEST_F(GazeboRosPropertiesTest, GetSetProperties) +{ + // Get model properties + { + this->GetModelProperties("simple_arm"); + } + // Get / set joint propertires + { + // Get initial joint properties (damping only). + // Note that damping=1.0 is the default value for this particular joint. + this->GetJointProperties("simple_arm::arm_shoulder_pan_joint", 1.0); + + // Set joint properties (damping only) + this->SetJointProperties("simple_arm::arm_shoulder_pan_joint", 0.5); + + // Check new joint properties (damping only). + this->GetJointProperties("simple_arm::arm_shoulder_pan_joint", 0.5); + } + + // Get / set link properties + { + // Get initial link properties + this->GetLinkProperties("simple_arm::arm_base", + true, 101.0, 1.11, 0.0, 0.0, 100.11, 0.0, 1.01); + + // Set link properties + this->SetLinkProperties("simple_arm::arm_base", + true, 170.2, 1.2, 0.3, 0.2, 102.2, 0.2, 1.02); + + // Check new link properties + this->GetLinkProperties("simple_arm::arm_base", + true, 170.2, 1.2, 0.3, 0.2, 102.2, 0.2, 1.02); + } + // Get / set light properties + { + // Get initial light properties + this->GetLightProperties("sun", ignition::math::Color(0.8, 0.8, 0.8, 1.0), + 0.9, 0.01, 0.001); + + // Set light properties + this->SetLightProperties("sun", ignition::math::Color(0.7, 0.1, 0.5, 1.0), + 0.92, 0.0092, 0.002); + + // Check new light properties. Wait for properties to be set first. + rclcpp::sleep_for(std::chrono::milliseconds(500)); + this->GetLightProperties("sun", ignition::math::Color(0.7, 0.1, 0.5, 1.0), + 0.92, 0.0092, 0.002); + } +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros/test/worlds/gazebo_ros_properties_test.world b/gazebo_ros/test/worlds/gazebo_ros_properties_test.world new file mode 100644 index 000000000..d8001bf83 --- /dev/null +++ b/gazebo_ros/test/worlds/gazebo_ros_properties_test.world @@ -0,0 +1,655 @@ + + + + + /test + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + + + 0 0 0.00099 0 -0 0 + + 1.11 + 0 + 0 + 100.11 + 0 + 1.01 + + 101 + + + 0 0 0.05 0 -0 0 + + + 1 1 0.1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.05 0 -0 0 + + + 1 1 0.1 + + + + + + + + 0 0 0.6 0 -0 0 + + + 0.05 + 1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.6 0 -0 0 + + + 0.05 + 1 + + + + + + + 0 + 0 + 0 + + + 0 0 1.1 0 -0 0 + + 0.045455 0 0 0 -0 0 + + 0.011 + 0 + 0 + 0.0225 + 0 + 0.0135 + + 1.1 + + + 0 0 0.05 0 -0 0 + + + 0.05 + 0.1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.05 0 -0 0 + + + 0.05 + 0.1 + + + + + + + + 0.55 0 0.05 0 -0 0 + + + 1 0.05 0.1 + + + 10 + + + + + + + + + + + + + + + 0.55 0 0.05 0 -0 0 + + + 1 0.05 0.1 + + + + + + + 0 + 0 + 0 + + + 1.05 0 1.1 0 -0 0 + + 0.0875 0 0.083333 0 -0 0 + + 0.031 + 0 + 0.005 + 0.07275 + 0 + 0.04475 + + 1.2 + + + 0 0 0.1 0 -0 0 + + + 0.05 + 0.2 + + + 10 + + + + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 0.05 + 0.2 + + + + + + + + 0.3 0 0.15 0 -0 0 + + + 0.5 0.03 0.1 + + + 10 + + + + + + + + + + + + + + + 0.3 0 0.15 0 -0 0 + + + 0.5 0.03 0.1 + + + + + + + + 0.55 0 0.15 0 -0 0 + + + 0.05 + 0.3 + + + 10 + + + + + + + + + + + + + + + 0.55 0 0.15 0 -0 0 + + + 0.05 + 0.3 + + + + + + + 0 + 0 + 0 + + + 1.6 0 1.05 0 -0 0 + + 0 0 0 0 -0 0 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.001 + + 0.1 + + + 0 0 0.5 0 -0 0 + + + 0.03 + 1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.5 0 -0 0 + + + 0.03 + 1 + + + + + + + 0 + 0 + 0 + + + 1.6 0 1 0 -0 0 + + 0 0 0 0 -0 0 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.001 + + 0.1 + + + 0 0 0.025 0 -0 0 + + + 0.05 + 0.05 + + + 10 + + + + + + + + + + + + + + + 0 0 0.025 0 -0 0 + + + 0.05 + 0.05 + + + + + + + 0 + 0 + 0 + + + arm_base + arm_shoulder_pan + + + 1 + 0 + 0 + 0 + + 0 0 1 + 1 + + -1e+16 + 1e+16 + + + + + arm_shoulder_pan + arm_elbow_pan + + + 1 + 0 + 0 + 0 + + 0 0 1 + 1 + + -1e+16 + 1e+16 + + + + + arm_elbow_pan + arm_wrist_lift + + + 1 + 0 + 0 + 0 + + + -0.8 + 0.1 + + 0 0 1 + 1 + + + + arm_wrist_lift + arm_wrist_roll + + + 1 + 0 + 0 + 0 + + + -2.99999 + 2.99999 + + 0 0 1 + 1 + + + + 2 + 0 + 0 + 0 + + + -2.99999 + 2.99999 + + 0 1 0 + 1 + + + 0.033491 0.033439 0 0 -0 0 + + + 71 112000000 + 71 453431127 + 1547434480 978757840 + 71112 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.1e-05 8.1e-05 -0 0 0 -0.003396 + 1 1 1 + + -8.1e-05 8.1e-05 -0 0 0 -0.003396 + -7e-06 -2e-06 0 0 -0 6e-06 + -0 -0 0 0 -0 0 + -0 -0 0 0 -0 0 + + + 1.00927 0.289399 1.1 1e-06 1.1e-05 0.224422 + -5.3e-05 0.000157 0 0 -0 0.000183 + 2.5e-05 -8.8e-05 0 -4e-06 -2.1e-05 -9.1e-05 + 3e-05 -0.000105 0 0 -0 0 + + + -8.1e-05 8.1e-05 1.1 1e-06 -3e-06 0.279153 + -7e-06 -2e-06 0 0 -0 0.000157 + 1e-06 -3e-06 0 0 -0 -7.9e-05 + 1e-06 -4e-06 0 0 -0 0 + + + 1.54547 0.411795 0.249989 1e-06 1.5e-05 0.224421 + -7.5e-05 0.000255 0 0 -0 0.000183 + 3.4e-05 -0.000129 0 -0 -0 -9.2e-05 + 3e-06 -1.3e-05 0 0 -0 0 + + + 1.54547 0.411794 0.199988 1e-06 1.6e-05 0.234364 + -7.5e-05 0.000255 0 0 -0 0.000183 + 3.4e-05 -0.000129 0 -0 -0 -9.2e-05 + 3e-06 -1.3e-05 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 5 -5 2 0 0.275643 2.35619 + orbit + perspective + + + + diff --git a/gazebo_ros/worlds/gazebo_ros_properties_demo.world b/gazebo_ros/worlds/gazebo_ros_properties_demo.world new file mode 100644 index 000000000..714874150 --- /dev/null +++ b/gazebo_ros/worlds/gazebo_ros_properties_demo.world @@ -0,0 +1,655 @@ + + + + + + + /demo + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + + + + 0 0 0.00099 0 -0 0 + + 1.11 + 0 + 0 + 100.11 + 0 + 1.01 + + 101 + + + 0 0 0.05 0 -0 0 + + + 1 1 0.1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.05 0 -0 0 + + + 1 1 0.1 + + + + + + + + 0 0 0.6 0 -0 0 + + + 0.05 + 1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.6 0 -0 0 + + + 0.05 + 1 + + + + + + + 0 + 0 + 0 + + + 0 0 1.1 0 -0 0 + + 0.045455 0 0 0 -0 0 + + 0.011 + 0 + 0 + 0.0225 + 0 + 0.0135 + + 1.1 + + + 0 0 0.05 0 -0 0 + + + 0.05 + 0.1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.05 0 -0 0 + + + 0.05 + 0.1 + + + + + + + + 0.55 0 0.05 0 -0 0 + + + 1 0.05 0.1 + + + 10 + + + + + + + + + + + + + + + 0.55 0 0.05 0 -0 0 + + + 1 0.05 0.1 + + + + + + + 0 + 0 + 0 + + + 1.05 0 1.1 0 -0 0 + + 0.0875 0 0.083333 0 -0 0 + + 0.031 + 0 + 0.005 + 0.07275 + 0 + 0.04475 + + 1.2 + + + 0 0 0.1 0 -0 0 + + + 0.05 + 0.2 + + + 10 + + + + + + + + + + + + + + + 0 0 0.1 0 -0 0 + + + 0.05 + 0.2 + + + + + + + + 0.3 0 0.15 0 -0 0 + + + 0.5 0.03 0.1 + + + 10 + + + + + + + + + + + + + + + 0.3 0 0.15 0 -0 0 + + + 0.5 0.03 0.1 + + + + + + + + 0.55 0 0.15 0 -0 0 + + + 0.05 + 0.3 + + + 10 + + + + + + + + + + + + + + + 0.55 0 0.15 0 -0 0 + + + 0.05 + 0.3 + + + + + + + 0 + 0 + 0 + + + 1.6 0 1.05 0 -0 0 + + 0 0 0 0 -0 0 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.001 + + 0.1 + + + 0 0 0.5 0 -0 0 + + + 0.03 + 1 + + + 10 + + + + + + + + + + + + + + + 0 0 0.5 0 -0 0 + + + 0.03 + 1 + + + + + + + 0 + 0 + 0 + + + 1.6 0 1 0 -0 0 + + 0 0 0 0 -0 0 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.001 + + 0.1 + + + 0 0 0.025 0 -0 0 + + + 0.05 + 0.05 + + + 10 + + + + + + + + + + + + + + + 0 0 0.025 0 -0 0 + + + 0.05 + 0.05 + + + + + + + 0 + 0 + 0 + + + arm_base + arm_shoulder_pan + + + 1 + 0 + 0 + 0 + + 0 0 1 + 1 + + -1e+16 + 1e+16 + + + + + arm_shoulder_pan + arm_elbow_pan + + + 1 + 0 + 0 + 0 + + 0 0 1 + 1 + + -1e+16 + 1e+16 + + + + + arm_elbow_pan + arm_wrist_lift + + + 1 + 0 + 0 + 0 + + + -0.8 + 0.1 + + 0 0 1 + 1 + + + + arm_wrist_lift + arm_wrist_roll + + + 1 + 0 + 0 + 0 + + + -2.99999 + 2.99999 + + 0 0 1 + 1 + + + 0.033491 0.033439 0 0 -0 0 + + + 71 112000000 + 71 453431127 + 1547434480 978757840 + 71112 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.1e-05 8.1e-05 -0 0 0 -0.003396 + 1 1 1 + + -8.1e-05 8.1e-05 -0 0 0 -0.003396 + -7e-06 -2e-06 0 0 -0 6e-06 + -0 -0 0 0 -0 0 + -0 -0 0 0 -0 0 + + + 1.00927 0.289399 1.1 1e-06 1.1e-05 0.224422 + -5.3e-05 0.000157 0 0 -0 0.000183 + 2.5e-05 -8.8e-05 0 -4e-06 -2.1e-05 -9.1e-05 + 3e-05 -0.000105 0 0 -0 0 + + + -8.1e-05 8.1e-05 1.1 1e-06 -3e-06 0.279153 + -7e-06 -2e-06 0 0 -0 0.000157 + 1e-06 -3e-06 0 0 -0 -7.9e-05 + 1e-06 -4e-06 0 0 -0 0 + + + 1.54547 0.411795 0.249989 1e-06 1.5e-05 0.224421 + -7.5e-05 0.000255 0 0 -0 0.000183 + 3.4e-05 -0.000129 0 -0 -0 -9.2e-05 + 3e-06 -1.3e-05 0 0 -0 0 + + + 1.54547 0.411794 0.199988 1e-06 1.6e-05 0.234364 + -7.5e-05 0.000255 0 0 -0 0.000183 + 3.4e-05 -0.000129 0 -0 -0 -9.2e-05 + 3e-06 -1.3e-05 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 5 -5 2 0 0.275643 2.35619 + orbit + perspective + + + +