From f9a7798532d5a12db005b0704f80dff522c42677 Mon Sep 17 00:00:00 2001 From: chapulina Date: Thu, 14 Feb 2019 14:32:38 -0800 Subject: [PATCH 01/11] [ros2] World plugin to get/set entity state services (#839) remove status_message --- .../gazebo_ros/gazebo_ros_api_plugin.h | 33 -- .../gazebo_ros/src/gazebo_ros_api_plugin.cpp | 414 ------------------ gazebo_msgs/CMakeLists.txt | 3 + gazebo_msgs/msg/EntityState.msg | 9 + gazebo_msgs/msg/LinkState.msg | 2 + gazebo_msgs/msg/ModelState.msg | 2 + gazebo_msgs/srv/DeleteLight.srv | 2 + gazebo_msgs/srv/DeleteModel.srv | 2 + gazebo_msgs/srv/GetEntityState.srv | 11 + gazebo_msgs/srv/GetLinkState.srv | 2 + gazebo_msgs/srv/GetModelState.srv | 2 + gazebo_msgs/srv/SetEntityState.srv | 4 + gazebo_msgs/srv/SetLinkState.srv | 2 + gazebo_msgs/srv/SetModelState.srv | 2 + gazebo_msgs/srv/SpawnModel.srv | 2 + .../worlds/gazebo_ros_diff_drive_demo.world | 2 +- gazebo_ros/CMakeLists.txt | 15 + .../include/gazebo_ros/gazebo_ros_state.hpp | 54 +++ gazebo_ros/src/gazebo_ros_state.cpp | 216 +++++++++ gazebo_ros/test/CMakeLists.txt | 1 + gazebo_ros/test/test_gazebo_ros_state.cpp | 189 ++++++++ .../test/worlds/gazebo_ros_state_test.world | 58 +++ gazebo_ros/worlds/gazebo_ros_state_demo.world | 87 ++++ 23 files changed, 666 insertions(+), 448 deletions(-) create mode 100644 gazebo_msgs/msg/EntityState.msg create mode 100644 gazebo_msgs/srv/GetEntityState.srv create mode 100644 gazebo_msgs/srv/SetEntityState.srv create mode 100644 gazebo_ros/include/gazebo_ros/gazebo_ros_state.hpp create mode 100644 gazebo_ros/src/gazebo_ros_state.cpp create mode 100644 gazebo_ros/test/test_gazebo_ros_state.cpp create mode 100644 gazebo_ros/test/worlds/gazebo_ros_state_test.world create mode 100644 gazebo_ros/worlds/gazebo_ros_state_demo.world 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 af5a7cfb6..8d2dd20c8 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 @@ -58,23 +58,17 @@ #include "gazebo_msgs/GetWorldProperties.h" #include "gazebo_msgs/GetModelProperties.h" -#include "gazebo_msgs/GetModelState.h" -#include "gazebo_msgs/SetModelState.h" #include "gazebo_msgs/GetJointProperties.h" #include "gazebo_msgs/ApplyJointEffort.h" #include "gazebo_msgs/GetLinkProperties.h" #include "gazebo_msgs/SetLinkProperties.h" -#include "gazebo_msgs/SetLinkState.h" -#include "gazebo_msgs/GetLinkState.h" #include "gazebo_msgs/GetLightProperties.h" #include "gazebo_msgs/SetLightProperties.h" // Topics -#include "gazebo_msgs/ModelState.h" -#include "gazebo_msgs/LinkState.h" #include "gazebo_msgs/ModelStates.h" #include "gazebo_msgs/LinkStates.h" @@ -138,9 +132,6 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief void onModelStatesDisconnect(); - /// \brief - bool getModelState(gazebo_msgs::GetModelState::Request &req,gazebo_msgs::GetModelState::Response &res); - /// \brief bool getModelProperties(gazebo_msgs::GetModelProperties::Request &req,gazebo_msgs::GetModelProperties::Response &res); @@ -153,9 +144,6 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief bool getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req,gazebo_msgs::GetLinkProperties::Response &res); - /// \brief - bool getLinkState(gazebo_msgs::GetLinkState::Request &req,gazebo_msgs::GetLinkState::Response &res); - /// \brief bool getLightProperties(gazebo_msgs::GetLightProperties::Request &req,gazebo_msgs::GetLightProperties::Response &res); @@ -174,12 +162,6 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief bool setJointProperties(gazebo_msgs::SetJointProperties::Request &req,gazebo_msgs::SetJointProperties::Response &res); - /// \brief - bool setModelState(gazebo_msgs::SetModelState::Request &req,gazebo_msgs::SetModelState::Response &res); - - /// \brief - void updateModelState(const gazebo_msgs::ModelState::ConstPtr& model_state); - /// \brief bool applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req,gazebo_msgs::ApplyJointEffort::Response &res); @@ -206,12 +188,6 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief bool setModelConfiguration(gazebo_msgs::SetModelConfiguration::Request &req,gazebo_msgs::SetModelConfiguration::Response &res); - /// \brief - bool setLinkState(gazebo_msgs::SetLinkState::Request &req,gazebo_msgs::SetLinkState::Response &res); - - /// \brief - void updateLinkState(const gazebo_msgs::LinkState::ConstPtr& link_state); - /// \brief bool applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &req,gazebo_msgs::ApplyBodyWrench::Response &res); @@ -274,12 +250,10 @@ class GazeboRosApiPlugin : public SystemPlugin gazebo::event::ConnectionPtr pub_model_states_event_; gazebo::event::ConnectionPtr load_gazebo_ros_api_plugin_event_; - ros::ServiceServer get_model_state_service_; 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_link_state_service_; ros::ServiceServer get_light_properties_service_; ros::ServiceServer set_light_properties_service_; ros::ServiceServer set_link_properties_service_; @@ -287,18 +261,14 @@ class GazeboRosApiPlugin : public SystemPlugin ros::ServiceServer get_physics_properties_service_; ros::ServiceServer apply_body_wrench_service_; ros::ServiceServer set_joint_properties_service_; - ros::ServiceServer set_model_state_service_; ros::ServiceServer apply_joint_effort_service_; ros::ServiceServer set_model_configuration_service_; - ros::ServiceServer set_link_state_service_; ros::ServiceServer reset_simulation_service_; ros::ServiceServer reset_world_service_; ros::ServiceServer pause_physics_service_; ros::ServiceServer unpause_physics_service_; ros::ServiceServer clear_joint_forces_service_; ros::ServiceServer clear_body_wrenches_service_; - ros::Subscriber set_link_state_topic_; - ros::Subscriber set_model_state_topic_; ros::Publisher pub_link_states_; ros::Publisher pub_model_states_; int pub_link_states_connection_count_; @@ -346,9 +316,6 @@ class GazeboRosApiPlugin : public SystemPlugin std::vector wrench_body_jobs_; std::vector force_joint_jobs_; - /// \brief index counters to count the accesses on models via GetModelState - std::map access_count_get_model_state_; - /// \brief enable the communication of gazebo information using ROS service/topics bool enable_ros_network_; }; 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 fdfe0ba68..8f81ed9f5 100644 --- a/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -242,15 +242,6 @@ void GazeboRosApiPlugin::advertiseServices() ros::VoidPtr(), &gazebo_queue_); get_model_properties_service_ = nh_->advertiseService(get_model_properties_aso); - // Advertise more services on the custom queue - std::string get_model_state_service_name("get_model_state"); - ros::AdvertiseServiceOptions get_model_state_aso = - ros::AdvertiseServiceOptions::create( - get_model_state_service_name, - boost::bind(&GazeboRosApiPlugin::getModelState,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - get_model_state_service_ = nh_->advertiseService(get_model_state_aso); - // Advertise more services on the custom queue std::string get_world_properties_service_name("get_world_properties"); ros::AdvertiseServiceOptions get_world_properties_aso = @@ -278,15 +269,6 @@ void GazeboRosApiPlugin::advertiseServices() ros::VoidPtr(), &gazebo_queue_); get_link_properties_service_ = nh_->advertiseService(get_link_properties_aso); - // Advertise more services on the custom queue - std::string get_link_state_service_name("get_link_state"); - ros::AdvertiseServiceOptions get_link_state_aso = - ros::AdvertiseServiceOptions::create( - get_link_state_service_name, - boost::bind(&GazeboRosApiPlugin::getLinkState,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - get_link_state_service_ = nh_->advertiseService(get_link_state_aso); - // Advertise more services on the custom queue std::string get_light_properties_service_name("get_light_properties"); ros::AdvertiseServiceOptions get_light_properties_aso = @@ -350,15 +332,6 @@ void GazeboRosApiPlugin::advertiseServices() ros::VoidPtr(), &gazebo_queue_); set_physics_properties_service_ = nh_->advertiseService(set_physics_properties_aso); - // Advertise more services on the custom queue - std::string set_model_state_service_name("set_model_state"); - ros::AdvertiseServiceOptions set_model_state_aso = - ros::AdvertiseServiceOptions::create( - set_model_state_service_name, - boost::bind(&GazeboRosApiPlugin::setModelState,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - set_model_state_service_ = nh_->advertiseService(set_model_state_aso); - // Advertise more services on the custom queue std::string set_model_configuration_service_name("set_model_configuration"); ros::AdvertiseServiceOptions set_model_configuration_aso = @@ -377,32 +350,6 @@ void GazeboRosApiPlugin::advertiseServices() ros::VoidPtr(), &gazebo_queue_); set_joint_properties_service_ = nh_->advertiseService(set_joint_properties_aso); - // Advertise more services on the custom queue - std::string set_link_state_service_name("set_link_state"); - ros::AdvertiseServiceOptions set_link_state_aso = - ros::AdvertiseServiceOptions::create( - set_link_state_service_name, - boost::bind(&GazeboRosApiPlugin::setLinkState,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - set_link_state_service_ = nh_->advertiseService(set_link_state_aso); - - // Advertise topic on custom queue - // topic callback version for set_link_state - ros::SubscribeOptions link_state_so = - ros::SubscribeOptions::create( - "set_link_state",10, - boost::bind( &GazeboRosApiPlugin::updateLinkState,this,_1), - ros::VoidPtr(), &gazebo_queue_); - set_link_state_topic_ = nh_->subscribe(link_state_so); - - // topic callback version for set_model_state - ros::SubscribeOptions model_state_so = - ros::SubscribeOptions::create( - "set_model_state",10, - boost::bind( &GazeboRosApiPlugin::updateModelState,this,_1), - ros::VoidPtr(), &gazebo_queue_); - set_model_state_topic_ = nh_->subscribe(model_state_so); - // Advertise more services on the custom queue std::string pause_physics_service_name("pause_physics"); ros::AdvertiseServiceOptions pause_physics_aso = @@ -525,115 +472,6 @@ void GazeboRosApiPlugin::onModelStatesDisconnect() } } -bool GazeboRosApiPlugin::getModelState(gazebo_msgs::GetModelState::Request &req, - gazebo_msgs::GetModelState::Response &res) -{ -#if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::ModelPtr model = world_->ModelByName(req.model_name); - gazebo::physics::EntityPtr frame = world_->EntityByName(req.relative_entity_name); -#else - gazebo::physics::ModelPtr model = world_->GetModel(req.model_name); - gazebo::physics::EntityPtr frame = world_->GetEntity(req.relative_entity_name); -#endif - if (!model) - { - ROS_ERROR_NAMED("api_plugin", "GetModelState: model [%s] does not exist",req.model_name.c_str()); - res.success = false; - res.status_message = "GetModelState: model does not exist"; - return true; - } - else - { - /** - * @brief creates a header for the result - * @author Markus Bader markus.bader@tuwien.ac.at - * @date 21th Nov 2014 - **/ - { - std::map::iterator it = access_count_get_model_state_.find(req.model_name); - if(it == access_count_get_model_state_.end()) - { - access_count_get_model_state_.insert( std::pair(req.model_name, 1) ); - res.header.seq = 1; - } - else - { - it->second++; - res.header.seq = it->second; - } - res.header.stamp = ros::Time::now(); - res.header.frame_id = req.relative_entity_name; /// @brief this is a redundant information - } - // get model pose - // get model twist -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d model_pose = model->WorldPose(); - ignition::math::Vector3d model_linear_vel = model->WorldLinearVel(); - ignition::math::Vector3d model_angular_vel = model->WorldAngularVel(); -#else - ignition::math::Pose3d model_pose = model->GetWorldPose().Ign(); - ignition::math::Vector3d model_linear_vel = model->GetWorldLinearVel().Ign(); - ignition::math::Vector3d model_angular_vel = model->GetWorldAngularVel().Ign(); -#endif - ignition::math::Vector3d model_pos = model_pose.Pos(); - ignition::math::Quaterniond model_rot = model_pose.Rot(); - - - - if (frame) - { - // convert to relative pose, rates -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d frame_pose = frame->WorldPose(); - ignition::math::Vector3d frame_vpos = frame->WorldLinearVel(); // get velocity in gazebo frame - ignition::math::Vector3d frame_veul = frame->WorldAngularVel(); // get velocity in gazebo frame -#else - ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); - ignition::math::Vector3d frame_vpos = frame->GetWorldLinearVel().Ign(); // get velocity in gazebo frame - ignition::math::Vector3d frame_veul = frame->GetWorldAngularVel().Ign(); // get velocity in gazebo frame -#endif - ignition::math::Pose3d model_rel_pose = model_pose - frame_pose; - model_pos = model_rel_pose.Pos(); - model_rot = model_rel_pose.Rot(); - - model_linear_vel = frame_pose.Rot().RotateVectorReverse(model_linear_vel - frame_vpos); - model_angular_vel = frame_pose.Rot().RotateVectorReverse(model_angular_vel - frame_veul); - } - /// @todo: FIXME map is really wrong, need to use tf here somehow - else if (req.relative_entity_name == "" || req.relative_entity_name == "world" || req.relative_entity_name == "map" || req.relative_entity_name == "/map") - { - ROS_DEBUG_NAMED("api_plugin", "GetModelState: relative_entity_name is empty/world/map, using inertial frame"); - } - else - { - res.success = false; - res.status_message = "GetModelState: reference relative_entity_name not found, did you forget to scope the body by model name?"; - return true; - } - - // fill in response - res.pose.position.x = model_pos.X(); - res.pose.position.y = model_pos.Y(); - res.pose.position.z = model_pos.Z(); - res.pose.orientation.w = model_rot.W(); - res.pose.orientation.x = model_rot.X(); - res.pose.orientation.y = model_rot.Y(); - res.pose.orientation.z = model_rot.Z(); - - res.twist.linear.x = model_linear_vel.X(); - res.twist.linear.y = model_linear_vel.Y(); - res.twist.linear.z = model_linear_vel.Z(); - res.twist.angular.x = model_angular_vel.X(); - res.twist.angular.y = model_angular_vel.Y(); - res.twist.angular.z = model_angular_vel.Z(); - - res.success = true; - res.status_message = "GetModelState: got properties"; - return true; - } - return true; -} - bool GazeboRosApiPlugin::getModelProperties(gazebo_msgs::GetModelProperties::Request &req, gazebo_msgs::GetModelProperties::Response &res) { @@ -824,86 +662,6 @@ bool GazeboRosApiPlugin::getLinkProperties(gazebo_msgs::GetLinkProperties::Reque } } -bool GazeboRosApiPlugin::getLinkState(gazebo_msgs::GetLinkState::Request &req, - gazebo_msgs::GetLinkState::Response &res) -{ -#if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(req.link_name)); - gazebo::physics::EntityPtr frame = world_->EntityByName(req.reference_frame); -#else - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->GetEntity(req.link_name)); - gazebo::physics::EntityPtr frame = world_->GetEntity(req.reference_frame); -#endif - - if (!body) - { - res.success = false; - res.status_message = "GetLinkState: link not found, did you forget to scope the link by model name?"; - return true; - } - - // get body pose - // Get inertial rates -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d body_pose = body->WorldPose(); - ignition::math::Vector3d body_vpos = body->WorldLinearVel(); // get velocity in gazebo frame - ignition::math::Vector3d body_veul = body->WorldAngularVel(); // get velocity in gazebo frame -#else - ignition::math::Pose3d body_pose = body->GetWorldPose().Ign(); - ignition::math::Vector3d body_vpos = body->GetWorldLinearVel().Ign(); // get velocity in gazebo frame - ignition::math::Vector3d body_veul = body->GetWorldAngularVel().Ign(); // get velocity in gazebo frame -#endif - - if (frame) - { - // convert to relative pose, rates -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d frame_pose = frame->WorldPose(); - ignition::math::Vector3d frame_vpos = frame->WorldLinearVel(); // get velocity in gazebo frame - ignition::math::Vector3d frame_veul = frame->WorldAngularVel(); // get velocity in gazebo frame -#else - ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); - ignition::math::Vector3d frame_vpos = frame->GetWorldLinearVel().Ign(); // get velocity in gazebo frame - ignition::math::Vector3d frame_veul = frame->GetWorldAngularVel().Ign(); // get velocity in gazebo frame -#endif - body_pose = body_pose - frame_pose; - - body_vpos = frame_pose.Rot().RotateVectorReverse(body_vpos - frame_vpos); - body_veul = frame_pose.Rot().RotateVectorReverse(body_veul - frame_veul); - } - /// @todo: FIXME 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", "GetLinkState: reference_frame is empty/world/map, using inertial frame"); - } - else - { - res.success = false; - res.status_message = "GetLinkState: reference reference_frame not found, did you forget to scope the link by model name?"; - return true; - } - - res.link_state.link_name = req.link_name; - res.link_state.pose.position.x = body_pose.Pos().X(); - res.link_state.pose.position.y = body_pose.Pos().Y(); - res.link_state.pose.position.z = body_pose.Pos().Z(); - res.link_state.pose.orientation.x = body_pose.Rot().X(); - res.link_state.pose.orientation.y = body_pose.Rot().Y(); - res.link_state.pose.orientation.z = body_pose.Rot().Z(); - res.link_state.pose.orientation.w = body_pose.Rot().W(); - res.link_state.twist.linear.x = body_vpos.X(); - res.link_state.twist.linear.y = body_vpos.Y(); - res.link_state.twist.linear.z = body_vpos.Z(); - res.link_state.twist.angular.x = body_veul.X(); - res.link_state.twist.angular.y = body_veul.Y(); - res.link_state.twist.angular.z = body_veul.Z(); - res.link_state.reference_frame = req.reference_frame; - - res.success = true; - res.status_message = "GetLinkState: got state"; - return true; -} - bool GazeboRosApiPlugin::getLightProperties(gazebo_msgs::GetLightProperties::Request &req, gazebo_msgs::GetLightProperties::Response &res) { @@ -1156,94 +914,6 @@ bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Req } } -bool GazeboRosApiPlugin::setModelState(gazebo_msgs::SetModelState::Request &req, - gazebo_msgs::SetModelState::Response &res) -{ - ignition::math::Vector3d target_pos(req.model_state.pose.position.x,req.model_state.pose.position.y,req.model_state.pose.position.z); - ignition::math::Quaterniond target_rot(req.model_state.pose.orientation.w,req.model_state.pose.orientation.x,req.model_state.pose.orientation.y,req.model_state.pose.orientation.z); - target_rot.Normalize(); // eliminates invalid rotation (0, 0, 0, 0) - ignition::math::Pose3d target_pose(target_pos,target_rot); - ignition::math::Vector3d target_pos_dot(req.model_state.twist.linear.x,req.model_state.twist.linear.y,req.model_state.twist.linear.z); - ignition::math::Vector3d target_rot_dot(req.model_state.twist.angular.x,req.model_state.twist.angular.y,req.model_state.twist.angular.z); - -#if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::ModelPtr model = world_->ModelByName(req.model_state.model_name); -#else - gazebo::physics::ModelPtr model = world_->GetModel(req.model_state.model_name); -#endif - if (!model) - { - ROS_ERROR_NAMED("api_plugin", "Updating ModelState: model [%s] does not exist",req.model_state.model_name.c_str()); - res.success = false; - res.status_message = "SetModelState: model does not exist"; - return true; - } - else - { -#if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::EntityPtr relative_entity = world_->EntityByName(req.model_state.reference_frame); -#else - gazebo::physics::EntityPtr relative_entity = world_->GetEntity(req.model_state.reference_frame); -#endif - if (relative_entity) - { -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d frame_pose = relative_entity->WorldPose(); // - myBody->GetCoMPose(); -#else - ignition::math::Pose3d frame_pose = relative_entity->GetWorldPose().Ign(); // - myBody->GetCoMPose(); -#endif - - target_pose = target_pose + frame_pose; - - // Velocities should be commanded in the requested reference - // frame, so we need to translate them to the world frame - target_pos_dot = frame_pose.Rot().RotateVector(target_pos_dot); - target_rot_dot = frame_pose.Rot().RotateVector(target_rot_dot); - } - /// @todo: FIXME map is really wrong, need to use tf here somehow - else if (req.model_state.reference_frame == "" || req.model_state.reference_frame == "world" || req.model_state.reference_frame == "map" || req.model_state.reference_frame == "/map" ) - { - ROS_DEBUG_NAMED("api_plugin", "Updating ModelState: reference frame is empty/world/map, usig inertial frame"); - } - else - { - ROS_ERROR_NAMED("api_plugin", "Updating ModelState: for model[%s], specified reference frame entity [%s] does not exist", - req.model_state.model_name.c_str(),req.model_state.reference_frame.c_str()); - res.success = false; - res.status_message = "SetModelState: specified reference frame entity does not exist"; - return true; - } - - //ROS_ERROR_NAMED("api_plugin", "target state: %f %f %f",target_pose.Pos().X(),target_pose.Pos().Y(),target_pose.Pos().Z()); - bool is_paused = world_->IsPaused(); - world_->SetPaused(true); - model->SetWorldPose(target_pose); - world_->SetPaused(is_paused); -#if GAZEBO_MAJOR_VERSION >= 8 - //ignition::math::Pose3d p3d = model->WorldPose(); -#else - //ignition::math::Pose3d p3d = model->GetWorldPose().Ign(); -#endif - //ROS_ERROR_NAMED("api_plugin", "model updated state: %f %f %f",p3d.Pos().X(),p3d.Pos().Y(),p3d.Pos().Z()); - - // set model velocity - model->SetLinearVel(target_pos_dot); - model->SetAngularVel(target_rot_dot); - - res.success = true; - res.status_message = "SetModelState: set model state done"; - return true; - } -} - -void GazeboRosApiPlugin::updateModelState(const gazebo_msgs::ModelState::ConstPtr& model_state) -{ - gazebo_msgs::SetModelState::Response res; - gazebo_msgs::SetModelState::Request req; - req.model_state = *model_state; - /*bool success =*/ setModelState(req,res); -} - bool GazeboRosApiPlugin::applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req, gazebo_msgs::ApplyJointEffort::Response &res) { @@ -1415,90 +1085,6 @@ bool GazeboRosApiPlugin::setModelConfiguration(gazebo_msgs::SetModelConfiguratio } } -bool GazeboRosApiPlugin::setLinkState(gazebo_msgs::SetLinkState::Request &req, - gazebo_msgs::SetLinkState::Response &res) -{ -#if GAZEBO_MAJOR_VERSION >= 8 - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(req.link_state.link_name)); - gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast(world_->EntityByName(req.link_state.reference_frame)); -#else - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->GetEntity(req.link_state.link_name)); - gazebo::physics::EntityPtr frame = world_->GetEntity(req.link_state.reference_frame); -#endif - if (!body) - { - ROS_ERROR_NAMED("api_plugin", "Updating LinkState: link [%s] does not exist",req.link_state.link_name.c_str()); - res.success = false; - res.status_message = "SetLinkState: link does not exist"; - return true; - } - - /// @todo: FIXME map is really wrong, unless using tf here somehow - // get reference frame (body/model(link)) pose and - // transform target pose to absolute world frame - ignition::math::Vector3d target_pos(req.link_state.pose.position.x,req.link_state.pose.position.y,req.link_state.pose.position.z); - ignition::math::Quaterniond target_rot(req.link_state.pose.orientation.w,req.link_state.pose.orientation.x,req.link_state.pose.orientation.y,req.link_state.pose.orientation.z); - ignition::math::Pose3d target_pose(target_pos,target_rot); - ignition::math::Vector3d target_linear_vel(req.link_state.twist.linear.x,req.link_state.twist.linear.y,req.link_state.twist.linear.z); - ignition::math::Vector3d target_angular_vel(req.link_state.twist.angular.x,req.link_state.twist.angular.y,req.link_state.twist.angular.z); - - if (frame) - { -#if GAZEBO_MAJOR_VERSION >= 8 - ignition::math::Pose3d frame_pose = frame->WorldPose(); // - myBody->GetCoMPose(); - ignition::math::Vector3d frame_linear_vel = frame->WorldLinearVel(); - ignition::math::Vector3d frame_angular_vel = frame->WorldAngularVel(); -#else - ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); // - myBody->GetCoMPose(); - ignition::math::Vector3d frame_linear_vel = frame->GetWorldLinearVel().Ign(); - ignition::math::Vector3d frame_angular_vel = frame->GetWorldAngularVel().Ign(); -#endif - ignition::math::Vector3d frame_pos = frame_pose.Pos(); - ignition::math::Quaterniond frame_rot = frame_pose.Rot(); - - //std::cout << " debug : " << frame->GetName() << " : " << frame_pose << " : " << target_pose << std::endl; - target_pose = target_pose + frame_pose; - - target_linear_vel -= frame_linear_vel; - target_angular_vel -= frame_angular_vel; - } - else if (req.link_state.reference_frame == "" || req.link_state.reference_frame == "world" || req.link_state.reference_frame == "map" || req.link_state.reference_frame == "/map") - { - ROS_INFO_NAMED("api_plugin", "Updating LinkState: reference_frame is empty/world/map, using inertial frame"); - } - else - { - ROS_ERROR_NAMED("api_plugin", "Updating LinkState: reference_frame is not a valid entity name"); - res.success = false; - res.status_message = "SetLinkState: failed"; - return true; - } - - //std::cout << " debug : " << target_pose << std::endl; - //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex()); - - bool is_paused = world_->IsPaused(); - if (!is_paused) world_->SetPaused(true); - body->SetWorldPose(target_pose); - world_->SetPaused(is_paused); - - // set body velocity to desired twist - body->SetLinearVel(target_linear_vel); - body->SetAngularVel(target_angular_vel); - - res.success = true; - res.status_message = "SetLinkState: success"; - return true; -} - -void GazeboRosApiPlugin::updateLinkState(const gazebo_msgs::LinkState::ConstPtr& link_state) -{ - gazebo_msgs::SetLinkState::Request req; - gazebo_msgs::SetLinkState::Response res; - req.link_state = *link_state; - /*bool success = */ setLinkState(req,res); -} - void GazeboRosApiPlugin::transformWrench( ignition::math::Vector3d &target_force, ignition::math::Vector3d &target_torque, const ignition::math::Vector3d &reference_force, const ignition::math::Vector3d &reference_torque, diff --git a/gazebo_msgs/CMakeLists.txt b/gazebo_msgs/CMakeLists.txt index 2d8d782f8..616364720 100644 --- a/gazebo_msgs/CMakeLists.txt +++ b/gazebo_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(rosidl_default_generators REQUIRED) set(msg_files "msg/ContactState.msg" "msg/ContactsState.msg" + "msg/EntityState.msg" "msg/LinkState.msg" "msg/LinkStates.msg" "msg/ModelState.msg" @@ -39,6 +40,7 @@ set(srv_files "srv/DeleteLight.srv" "srv/DeleteModel.srv" "srv/GetJointProperties.srv" + "srv/GetEntityState.srv" "srv/GetLightProperties.srv" "srv/GetLinkProperties.srv" "srv/GetLinkState.srv" @@ -47,6 +49,7 @@ set(srv_files "srv/GetPhysicsProperties.srv" "srv/GetWorldProperties.srv" "srv/JointRequest.srv" + "srv/SetEntityState.srv" "srv/SetJointProperties.srv" "srv/SetJointTrajectory.srv" "srv/SetLightProperties.srv" diff --git a/gazebo_msgs/msg/EntityState.msg b/gazebo_msgs/msg/EntityState.msg new file mode 100644 index 000000000..03a62e43b --- /dev/null +++ b/gazebo_msgs/msg/EntityState.msg @@ -0,0 +1,9 @@ +# Holds an entity's pose and twist +string name # Entity's scoped name. + # An entity can be a model, link, collision, light, etc. + # Be sure to use gazebo scoped naming notation (e.g. [model_name::link_name]) +geometry_msgs/Pose pose # Pose in reference frame. +geometry_msgs/Twist twist # Twist in reference frame. +string reference_frame # Pose/twist are expressed relative to the frame of this entity. + # Leaving empty or "world" defaults to inertial world frame. + diff --git a/gazebo_msgs/msg/LinkState.msg b/gazebo_msgs/msg/LinkState.msg index 6dcf278b8..5bea9c6a7 100644 --- a/gazebo_msgs/msg/LinkState.msg +++ b/gazebo_msgs/msg/LinkState.msg @@ -1,3 +1,5 @@ +# Deprecated, kept for ROS 1 bridge. +# Use EntityState # @todo: FIXME: sets pose and twist of a link. All children link poses/twists of the URDF tree are not updated accordingly, but should be. string link_name # link name, link_names are in gazebo scoped name notation, [model_name::body_name] geometry_msgs/Pose pose # desired pose in reference frame diff --git a/gazebo_msgs/msg/ModelState.msg b/gazebo_msgs/msg/ModelState.msg index 10b5ad8ce..236714537 100644 --- a/gazebo_msgs/msg/ModelState.msg +++ b/gazebo_msgs/msg/ModelState.msg @@ -1,3 +1,5 @@ +# Deprecated, kept for ROS 1 bridge. +# Use EntityState # Set Gazebo Model pose and twist string model_name # model to set state (pose and twist) geometry_msgs/Pose pose # desired pose in reference frame diff --git a/gazebo_msgs/srv/DeleteLight.srv b/gazebo_msgs/srv/DeleteLight.srv index 0743f760d..7dda905f1 100644 --- a/gazebo_msgs/srv/DeleteLight.srv +++ b/gazebo_msgs/srv/DeleteLight.srv @@ -1,3 +1,5 @@ +# Deprecated, kept for ROS 1 bridge. +# Use DeleteEntity string light_name # name of the light to be deleted --- bool success # return true if deletion is successful diff --git a/gazebo_msgs/srv/DeleteModel.srv b/gazebo_msgs/srv/DeleteModel.srv index fa895aa50..5405ab867 100644 --- a/gazebo_msgs/srv/DeleteModel.srv +++ b/gazebo_msgs/srv/DeleteModel.srv @@ -1,3 +1,5 @@ +# Deprecated, kept for ROS 1 bridge. +# Use DeleteEntity string model_name # name of the Gazebo Model to be deleted --- bool success # return true if deletion is successful diff --git a/gazebo_msgs/srv/GetEntityState.srv b/gazebo_msgs/srv/GetEntityState.srv new file mode 100644 index 000000000..642721782 --- /dev/null +++ b/gazebo_msgs/srv/GetEntityState.srv @@ -0,0 +1,11 @@ +string name # Entity's scoped name. + # An entity can be a model, link, collision, light, etc. + # Be sure to use gazebo scoped naming notation (e.g. [model_name::link_name]) +string reference_frame # Return pose and twist relative to this entity. + # Leaving empty or "world" will use inertial world frame. +--- +std_msgs/Header header # Standard metadata for higher-level stamped data types. + # * header.stamp Timestamp related to the pose. + # * header.frame_id Filled with the relative_frame. +gazebo_msgs/EntityState state # Contains pose and twist. +bool success # Return true if get was successful. If false, the state contains garbage. diff --git a/gazebo_msgs/srv/GetLinkState.srv b/gazebo_msgs/srv/GetLinkState.srv index 63bdcf3b1..22acb545f 100644 --- a/gazebo_msgs/srv/GetLinkState.srv +++ b/gazebo_msgs/srv/GetLinkState.srv @@ -1,3 +1,5 @@ +# Deprecated, kept for ROS 1 bridge. +# Use GetEntityState string link_name # name of link # link names are prefixed by model name, e.g. pr2::base_link string reference_frame # reference frame of returned information, must be a valid link diff --git a/gazebo_msgs/srv/GetModelState.srv b/gazebo_msgs/srv/GetModelState.srv index 16461ceaf..ca68065ff 100644 --- a/gazebo_msgs/srv/GetModelState.srv +++ b/gazebo_msgs/srv/GetModelState.srv @@ -1,3 +1,5 @@ +# Deprecated, kept for ROS 1 bridge. +# Use GetEntityState string model_name # name of Gazebo Model string relative_entity_name # return pose and twist relative to this entity # an entity can be a model, body, or geom diff --git a/gazebo_msgs/srv/SetEntityState.srv b/gazebo_msgs/srv/SetEntityState.srv new file mode 100644 index 000000000..457f7e525 --- /dev/null +++ b/gazebo_msgs/srv/SetEntityState.srv @@ -0,0 +1,4 @@ +gazebo_msgs/EntityState state # Entity state to set to. + # Be sure to fill all fields, values of zero have meaning. +--- +bool success # Return true if setting state was successful. diff --git a/gazebo_msgs/srv/SetLinkState.srv b/gazebo_msgs/srv/SetLinkState.srv index 6530a151d..6be967431 100644 --- a/gazebo_msgs/srv/SetLinkState.srv +++ b/gazebo_msgs/srv/SetLinkState.srv @@ -1,3 +1,5 @@ +# Deprecated, kept for ROS 1 bridge. +# Use SetEntityState gazebo_msgs/LinkState link_state --- bool success # return true if set wrench successful diff --git a/gazebo_msgs/srv/SetModelState.srv b/gazebo_msgs/srv/SetModelState.srv index 2095e2a4a..0a2ae90b0 100644 --- a/gazebo_msgs/srv/SetModelState.srv +++ b/gazebo_msgs/srv/SetModelState.srv @@ -1,3 +1,5 @@ +# Deprecated, kept for ROS 1 bridge. +# Use SetEntityState gazebo_msgs/ModelState model_state --- bool success # return true if setting state successful diff --git a/gazebo_msgs/srv/SpawnModel.srv b/gazebo_msgs/srv/SpawnModel.srv index 7dadd64f9..535a27b76 100644 --- a/gazebo_msgs/srv/SpawnModel.srv +++ b/gazebo_msgs/srv/SpawnModel.srv @@ -1,3 +1,5 @@ +# Deprecated, kept for ROS 1 bridge. +# Use SpawnEntity string model_name # name of the model to be spawn string model_xml # this should be an urdf or gazebo xml string robot_namespace # spawn robot and all ROS interfaces under this namespace diff --git a/gazebo_plugins/worlds/gazebo_ros_diff_drive_demo.world b/gazebo_plugins/worlds/gazebo_ros_diff_drive_demo.world index a2806b5f6..386f95635 100644 --- a/gazebo_plugins/worlds/gazebo_ros_diff_drive_demo.world +++ b/gazebo_plugins/worlds/gazebo_ros_diff_drive_demo.world @@ -14,7 +14,7 @@ Try listening to TF: - ros2 run tf2_ros tf2_echo odom chassis + ros2 run tf2_ros tf2_echo odom_demo chassis ros2 run tf2_ros tf2_echo chassis right_wheel diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 68a036df4..9189341a9 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -76,6 +76,20 @@ target_link_libraries(gazebo_ros_factory ) ament_export_libraries(gazebo_ros_factory) +# gazebo_ros_state +add_library(gazebo_ros_state SHARED + src/gazebo_ros_state.cpp +) +ament_target_dependencies(gazebo_ros_state + "rclcpp" + "gazebo_dev" + "gazebo_msgs" +) +target_link_libraries(gazebo_ros_state + gazebo_ros_node +) +ament_export_libraries(gazebo_ros_state) + ament_export_include_directories(include) ament_export_dependencies(rclcpp) ament_export_dependencies(gazebo_dev) @@ -96,6 +110,7 @@ install( gazebo_ros_factory gazebo_ros_init gazebo_ros_node + gazebo_ros_state gazebo_ros_utils ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/gazebo_ros/include/gazebo_ros/gazebo_ros_state.hpp b/gazebo_ros/include/gazebo_ros/gazebo_ros_state.hpp new file mode 100644 index 000000000..e8656d411 --- /dev/null +++ b/gazebo_ros/include/gazebo_ros/gazebo_ros_state.hpp @@ -0,0 +1,54 @@ +// 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_STATE_HPP_ +#define GAZEBO_ROS__GAZEBO_ROS_STATE_HPP_ + +#include + +#include + +namespace gazebo_ros +{ + +class GazeboRosStatePrivate; + +/// Provides services and topics to query and set the state of entities in +/// simualtion, such as position and velocity. +/// +/// Services: +/// +/// get_entity_state (gazebo_msgs::srv::GetEntityState) +/// Get an entity's position and velocity. +/// +/// set_entity_state (gazebo_msgs::srv::SetEntityState) +/// Set an entity's position and velocity. +class GazeboRosState : public gazebo::WorldPlugin +{ +public: + /// Constructor + GazeboRosState(); + + /// Destructor + virtual ~GazeboRosState(); + + // 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_STATE_HPP_ diff --git a/gazebo_ros/src/gazebo_ros_state.cpp b/gazebo_ros/src/gazebo_ros_state.cpp new file mode 100644 index 000000000..8cb08680f --- /dev/null +++ b/gazebo_ros/src/gazebo_ros_state.cpp @@ -0,0 +1,216 @@ +// 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 "gazebo_ros/conversions/builtin_interfaces.hpp" +#include "gazebo_ros/conversions/geometry_msgs.hpp" +#include "gazebo_ros/gazebo_ros_state.hpp" + +namespace gazebo_ros +{ + +class GazeboRosStatePrivate +{ +public: + /// \brief Callback for get entity state service. + /// \param[in] req Request + /// \param[out] res Response + void GetEntityState( + gazebo_msgs::srv::GetEntityState::Request::SharedPtr _req, + gazebo_msgs::srv::GetEntityState::Response::SharedPtr _res); + + /// \brief Callback for set entity state service. + /// \param[in] req Request + /// \param[out] res Response + void SetEntityState( + gazebo_msgs::srv::SetEntityState::Request::SharedPtr _req, + gazebo_msgs::srv::SetEntityState::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 entity states. + rclcpp::Service::SharedPtr get_entity_state_service_; + + /// \brief ROS service to handle requests to set entity states. + rclcpp::Service::SharedPtr set_entity_state_service_; +}; + +GazeboRosState::GazeboRosState() +: impl_(std::make_unique()) +{ +} + +GazeboRosState::~GazeboRosState() +{ +} + +void GazeboRosState::Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf) +{ + impl_->world_ = _world; + + impl_->ros_node_ = gazebo_ros::Node::Get(_sdf); + + impl_->get_entity_state_service_ = + impl_->ros_node_->create_service( + "get_entity_state", std::bind(&GazeboRosStatePrivate::GetEntityState, impl_.get(), + std::placeholders::_1, std::placeholders::_2)); + + impl_->set_entity_state_service_ = + impl_->ros_node_->create_service( + "set_entity_state", std::bind(&GazeboRosStatePrivate::SetEntityState, impl_.get(), + std::placeholders::_1, std::placeholders::_2)); +} + +void GazeboRosStatePrivate::GetEntityState( + gazebo_msgs::srv::GetEntityState::Request::SharedPtr _req, + gazebo_msgs::srv::GetEntityState::Response::SharedPtr _res) +{ + // Target entity + auto entity = world_->EntityByName(_req->name); + if (!entity) { + _res->success = false; + + RCLCPP_ERROR(ros_node_->get_logger(), + "GetEntityState: entity [%s] does not exist", _req->name); + return; + } + + auto entity_pose = entity->WorldPose(); + auto entity_lin_vel = entity->WorldLinearVel(); + auto entity_ang_vel = entity->WorldAngularVel(); + + // Frame of reference + auto frame = world_->EntityByName(_req->reference_frame); + if (frame) { + auto frame_pose = frame->WorldPose(); + auto frame_lin_vel = frame->WorldLinearVel(); + auto frame_ang_vel = frame->WorldAngularVel(); + + entity_pose = entity_pose - frame_pose; + + // convert to relative + entity_lin_vel = frame_pose.Rot().RotateVectorReverse(entity_lin_vel - frame_lin_vel); + entity_ang_vel = frame_pose.Rot().RotateVectorReverse(entity_ang_vel - frame_ang_vel); + } else if (_req->reference_frame == "" || _req->reference_frame == "world") { + RCLCPP_DEBUG(ros_node_->get_logger(), + "GetEntityState: reference_frame is empty/world, using inertial frame"); + } else { + _res->success = false; + + RCLCPP_ERROR(ros_node_->get_logger(), + "GetEntityState: reference entity [%s] not found, did you forget to scope the entity name?", + _req->name); + return; + } + + // Fill in response + _res->header.stamp = Convert(world_->SimTime()); + _res->header.frame_id = _req->reference_frame; + + _res->state.pose.position = Convert(entity_pose.Pos()); + _res->state.pose.orientation = Convert(entity_pose.Rot()); + + _res->state.twist.linear = Convert(entity_lin_vel); + _res->state.twist.angular = Convert(entity_ang_vel); + + _res->success = true; +} + +void GazeboRosStatePrivate::SetEntityState( + gazebo_msgs::srv::SetEntityState::Request::SharedPtr _req, + gazebo_msgs::srv::SetEntityState::Response::SharedPtr _res) +{ + auto entity_pos = Convert(_req->state.pose.position); + auto entity_rot = Convert(_req->state.pose.orientation); + + // Eliminate invalid rotation (0, 0, 0, 0) + entity_rot.Normalize(); + + ignition::math::Pose3d entity_pose(entity_pos, entity_rot); + auto entity_lin_vel = Convert(_req->state.twist.linear); + auto entity_ang_vel = Convert(_req->state.twist.angular); + + // Target entity + auto entity = world_->EntityByName(_req->state.name); + if (!entity) { + _res->success = false; + + RCLCPP_ERROR(ros_node_->get_logger(), + "SetEntityState: entity [%s] does not exist", _req->state.name); + return; + } + + // Frame of reference + auto frame = world_->EntityByName(_req->state.reference_frame); + if (frame) { + auto frame_pose = frame->WorldPose(); + entity_pose = entity_pose + frame_pose; + + // Velocities should be commanded in the requested reference + // frame, so we need to translate them to the world frame + entity_lin_vel = frame_pose.Rot().RotateVector(entity_lin_vel); + entity_ang_vel = frame_pose.Rot().RotateVector(entity_ang_vel); + } else if (_req->state.reference_frame == "" || _req->state.reference_frame == "world") { + RCLCPP_DEBUG(ros_node_->get_logger(), + "SetEntityState: reference_frame is empty/world, using inertial frame"); + } else { + _res->success = false; + + RCLCPP_ERROR(ros_node_->get_logger(), + "GetEntityState: reference entity [%s] not found, did you forget to scope the entity name?", + _req->state.name); + return; + } + + // Set state + auto model = boost::dynamic_pointer_cast(entity); + auto link = boost::dynamic_pointer_cast(entity); + auto light = boost::dynamic_pointer_cast(entity); + + bool is_paused = world_->IsPaused(); + + world_->SetPaused(true); + entity->SetWorldPose(entity_pose); + world_->SetPaused(is_paused); + + if (model) { + model->SetLinearVel(entity_lin_vel); + model->SetAngularVel(entity_ang_vel); + } else if (link) { + link->SetLinearVel(entity_lin_vel); + link->SetAngularVel(entity_ang_vel); + } + + // Fill response + _res->success = true; +} + +GZ_REGISTER_WORLD_PLUGIN(GazeboRosState) + +} // namespace gazebo_ros diff --git a/gazebo_ros/test/CMakeLists.txt b/gazebo_ros/test/CMakeLists.txt index 79aab33c1..5bc99546b 100644 --- a/gazebo_ros/test/CMakeLists.txt +++ b/gazebo_ros/test/CMakeLists.txt @@ -31,6 +31,7 @@ file(COPY worlds DESTINATION .) set(tests test_conversions test_gazebo_ros_factory + test_gazebo_ros_state test_node test_plugins test_sim_time diff --git a/gazebo_ros/test/test_gazebo_ros_state.cpp b/gazebo_ros/test/test_gazebo_ros_state.cpp new file mode 100644 index 000000000..57ee660c3 --- /dev/null +++ b/gazebo_ros/test/test_gazebo_ros_state.cpp @@ -0,0 +1,189 @@ +// 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 + +#define tol 10e-2 + +class GazeboRosStateTest : public gazebo::ServerFixture +{ +public: + // Documentation inherited + void SetUp() override; + + /// Helper function to call get state service + void GetState( + const std::string & _entity, + const ignition::math::Pose3d & _pose, + const ignition::math::Vector3d & _lin_vel = ignition::math::Vector3d::Zero, + const ignition::math::Vector3d & _ang_vel = ignition::math::Vector3d::Zero); + + /// Helper function to call set state service + void SetState( + const std::string & _entity, + const ignition::math::Pose3d & _pose, + const ignition::math::Vector3d & _lin_vel = ignition::math::Vector3d::Zero, + const ignition::math::Vector3d & _ang_vel = ignition::math::Vector3d::Zero); + + gazebo::physics::WorldPtr world_; + rclcpp::Node::SharedPtr node_; + std::shared_ptr> get_state_client_; + std::shared_ptr> set_state_client_; +}; + +void GazeboRosStateTest::SetUp() +{ + // Load world with state plugin and start paused + this->Load("worlds/gazebo_ros_state_test.world", true); + + // World + world_ = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world_); + + // Create ROS clients + node_ = std::make_shared("gazebo_ros_state_test"); + ASSERT_NE(nullptr, node_); + + get_state_client_ = + node_->create_client("test/get_entity_state"); + ASSERT_NE(nullptr, get_state_client_); + EXPECT_TRUE(get_state_client_->wait_for_service(std::chrono::seconds(1))); + + set_state_client_ = + node_->create_client("test/set_entity_state"); + ASSERT_NE(nullptr, set_state_client_); + EXPECT_TRUE(set_state_client_->wait_for_service(std::chrono::seconds(1))); +} + +void GazeboRosStateTest::GetState( + const std::string & _entity, + const ignition::math::Pose3d & _pose, + const ignition::math::Vector3d & _lin_vel, + const ignition::math::Vector3d & _ang_vel) +{ + auto entity = world_->EntityByName(_entity); + ASSERT_NE(nullptr, entity); + + auto request = std::make_shared(); + request->name = _entity; + + auto response_future = get_state_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_NEAR(_pose.Pos().X(), response->state.pose.position.x, tol) << _entity; + EXPECT_NEAR(_pose.Pos().Y(), response->state.pose.position.y, tol) << _entity; + EXPECT_NEAR(_pose.Pos().Z(), response->state.pose.position.z, tol) << _entity; + + EXPECT_NEAR(_pose.Rot().X(), response->state.pose.orientation.x, tol) << _entity; + EXPECT_NEAR(_pose.Rot().Y(), response->state.pose.orientation.y, tol) << _entity; + EXPECT_NEAR(_pose.Rot().Z(), response->state.pose.orientation.z, tol) << _entity; + EXPECT_NEAR(_pose.Rot().W(), response->state.pose.orientation.w, tol) << _entity; + + EXPECT_NEAR(_lin_vel.X(), response->state.twist.linear.x, tol) << _entity; + EXPECT_NEAR(_lin_vel.Y(), response->state.twist.linear.y, tol) << _entity; + EXPECT_NEAR(_lin_vel.Z(), response->state.twist.linear.z, tol) << _entity; + + EXPECT_NEAR(_ang_vel.X(), response->state.twist.angular.x, tol) << _entity; + EXPECT_NEAR(_ang_vel.Y(), response->state.twist.angular.y, tol) << _entity; + EXPECT_NEAR(_ang_vel.Z(), response->state.twist.angular.z, tol) << _entity; +} + +void GazeboRosStateTest::SetState( + const std::string & _entity, + const ignition::math::Pose3d & _pose, + const ignition::math::Vector3d & _lin_vel, + const ignition::math::Vector3d & _ang_vel) +{ + auto request = std::make_shared(); + request->state.name = _entity; + request->state.pose.position = gazebo_ros::Convert(_pose.Pos()); + request->state.pose.orientation = + gazebo_ros::Convert(_pose.Rot()); + request->state.twist.linear = gazebo_ros::Convert(_lin_vel); + request->state.twist.angular = gazebo_ros::Convert(_ang_vel); + + auto response_future = set_state_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(GazeboRosStateTest, GetSet) +{ + // Get / set model state + { + // Get initial state + this->GetState("boxes", ignition::math::Pose3d(0, 0, 0.5, 0, 0, 0)); + + // Set new state + this->SetState("boxes", ignition::math::Pose3d(1.0, 2.0, 10.0, 0, 0, 0), + ignition::math::Vector3d(4.0, 0, 0), ignition::math::Vector3d::Zero); + + // Check new state + this->GetState("boxes", ignition::math::Pose3d(1.0, 2.0, 10.0, 0, 0, 0), + ignition::math::Vector3d(4.0, 0, 0), ignition::math::Vector3d::Zero); + } + + // Get / set light state + { + // Get initial state + this->GetState("sun", ignition::math::Pose3d(0, 0, 10, 0, 0, 0)); + + // Set new state + this->SetState("sun", ignition::math::Pose3d(1.0, 2.0, 3.0, 0.1, 0.2, 0.3)); + + // Check new state + this->GetState("sun", ignition::math::Pose3d(1.0, 2.0, 3.0, 0.1, 0.2, 0.3)); + } + + // Get / set link state + { + // Get initial state - note that is was moved with the model + this->GetState("boxes::top", ignition::math::Pose3d(1.0, 2.0, 11.25, 0, 0, 0), + ignition::math::Vector3d(4.0, 0, 0), ignition::math::Vector3d::Zero); + + // Set new state + this->SetState("boxes::top", ignition::math::Pose3d(10, 20, 30, 0.1, 0, 0), + ignition::math::Vector3d(1.0, 2.0, 3.0), ignition::math::Vector3d(0.0, 0.0, 4.0)); + + // Check new state + this->GetState("boxes::top", ignition::math::Pose3d(10, 20, 30, 0.1, 0, 0), + ignition::math::Vector3d(1.0, 2.0, 3.0), ignition::math::Vector3d(0.0, 0.0, 4.0)); + } +} + +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_state_test.world b/gazebo_ros/test/worlds/gazebo_ros_state_test.world new file mode 100644 index 000000000..f06297a2f --- /dev/null +++ b/gazebo_ros/test/worlds/gazebo_ros_state_test.world @@ -0,0 +1,58 @@ + + + + + + /test + + + + + model://ground_plane + + + model://sun + + + + 0 0 0.5 0 0 0 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + 0 0 1.25 0 0 0 + true + + + + 0.5 0.5 0.5 + + + + + + + 0.5 0.5 0.5 + + + + + + + + + diff --git a/gazebo_ros/worlds/gazebo_ros_state_demo.world b/gazebo_ros/worlds/gazebo_ros_state_demo.world new file mode 100644 index 000000000..020df15e9 --- /dev/null +++ b/gazebo_ros/worlds/gazebo_ros_state_demo.world @@ -0,0 +1,87 @@ + + + + + + + /demo + + + + + model://ground_plane + + + model://sun + + + + 0 0 0.5 0 0 0 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + 0 0 1.25 0 0 0 + true + + + + 0.5 0.5 0.5 + + + + + + + 0.5 0.5 0.5 + + + + + + + + + From eb628c953d05470d2f874b983b3bcd202cc1f541 Mon Sep 17 00:00:00 2001 From: chapulina Date: Fri, 15 Feb 2019 09:38:45 -0800 Subject: [PATCH 02/11] [ros2] Port time commands (pause / reset) (#866) --- .../gazebo_ros/gazebo_ros_api_plugin.h | 16 -- .../gazebo_ros/src/gazebo_ros_api_plugin.cpp | 61 -------- gazebo_ros/CMakeLists.txt | 2 + gazebo_ros/package.xml | 1 + gazebo_ros/src/gazebo_ros_init.cpp | 135 +++++++++++++++- gazebo_ros/test/CMakeLists.txt | 3 + gazebo_ros/test/test_gazebo_ros_init.cpp | 146 ++++++++++++++++++ gazebo_ros/test/worlds/free_fall.world | 27 ++++ 8 files changed, 311 insertions(+), 80 deletions(-) create mode 100644 gazebo_ros/test/test_gazebo_ros_init.cpp create mode 100644 gazebo_ros/test/worlds/free_fall.world 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 8d2dd20c8..127bac878 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 @@ -165,18 +165,6 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief bool applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req,gazebo_msgs::ApplyJointEffort::Response &res); - /// \brief - bool resetSimulation(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res); - - /// \brief - bool resetWorld(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res); - - /// \brief - bool pausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res); - - /// \brief - bool unpausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res); - /// \brief bool clearJointForces(gazebo_msgs::JointRequest::Request &req,gazebo_msgs::JointRequest::Response &res); bool clearJointForces(std::string joint_name); @@ -263,10 +251,6 @@ class GazeboRosApiPlugin : public SystemPlugin ros::ServiceServer set_joint_properties_service_; ros::ServiceServer apply_joint_effort_service_; ros::ServiceServer set_model_configuration_service_; - ros::ServiceServer reset_simulation_service_; - ros::ServiceServer reset_world_service_; - ros::ServiceServer pause_physics_service_; - ros::ServiceServer unpause_physics_service_; ros::ServiceServer clear_joint_forces_service_; ros::ServiceServer clear_body_wrenches_service_; ros::Publisher pub_link_states_; 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 8f81ed9f5..cdd4c336e 100644 --- a/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -350,24 +350,6 @@ void GazeboRosApiPlugin::advertiseServices() ros::VoidPtr(), &gazebo_queue_); set_joint_properties_service_ = nh_->advertiseService(set_joint_properties_aso); - // Advertise more services on the custom queue - std::string pause_physics_service_name("pause_physics"); - ros::AdvertiseServiceOptions pause_physics_aso = - ros::AdvertiseServiceOptions::create( - pause_physics_service_name, - boost::bind(&GazeboRosApiPlugin::pausePhysics,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - pause_physics_service_ = nh_->advertiseService(pause_physics_aso); - - // Advertise more services on the custom queue - std::string unpause_physics_service_name("unpause_physics"); - ros::AdvertiseServiceOptions unpause_physics_aso = - ros::AdvertiseServiceOptions::create( - unpause_physics_service_name, - boost::bind(&GazeboRosApiPlugin::unpausePhysics,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - unpause_physics_service_ = nh_->advertiseService(unpause_physics_aso); - // Advertise more services on the custom queue std::string apply_body_wrench_service_name("apply_body_wrench"); ros::AdvertiseServiceOptions apply_body_wrench_aso = @@ -404,25 +386,6 @@ void GazeboRosApiPlugin::advertiseServices() ros::VoidPtr(), &gazebo_queue_); clear_body_wrenches_service_ = nh_->advertiseService(clear_body_wrenches_aso); - // Advertise more services on the custom queue - std::string reset_simulation_service_name("reset_simulation"); - ros::AdvertiseServiceOptions reset_simulation_aso = - ros::AdvertiseServiceOptions::create( - reset_simulation_service_name, - boost::bind(&GazeboRosApiPlugin::resetSimulation,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - reset_simulation_service_ = nh_->advertiseService(reset_simulation_aso); - - // Advertise more services on the custom queue - std::string reset_world_service_name("reset_world"); - ros::AdvertiseServiceOptions reset_world_aso = - ros::AdvertiseServiceOptions::create( - reset_world_service_name, - boost::bind(&GazeboRosApiPlugin::resetWorld,this,_1,_2), - ros::VoidPtr(), &gazebo_queue_); - reset_world_service_ = nh_->advertiseService(reset_world_aso); - - // set param for use_sim_time if not set by user already if(!(nh_->hasParam("/use_sim_time"))) nh_->setParam("/use_sim_time", true); @@ -956,30 +919,6 @@ bool GazeboRosApiPlugin::applyJointEffort(gazebo_msgs::ApplyJointEffort::Request return true; } -bool GazeboRosApiPlugin::resetSimulation(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res) -{ - world_->Reset(); - return true; -} - -bool GazeboRosApiPlugin::resetWorld(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res) -{ - world_->ResetEntities(gazebo::physics::Base::MODEL); - return true; -} - -bool GazeboRosApiPlugin::pausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res) -{ - world_->SetPaused(true); - return true; -} - -bool GazeboRosApiPlugin::unpausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res) -{ - world_->SetPaused(false); - return true; -} - bool GazeboRosApiPlugin::clearJointForces(gazebo_msgs::JointRequest::Request &req, gazebo_msgs::JointRequest::Response &res) { diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 9189341a9..d956b9b68 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(rclcpp REQUIRED) find_package(gazebo_dev REQUIRED) find_package(gazebo_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(tinyxml_vendor REQUIRED) include_directories( @@ -54,6 +55,7 @@ ament_target_dependencies(gazebo_ros_init "builtin_interfaces" "gazebo_dev" "rclcpp" + "std_srvs" ) target_link_libraries(gazebo_ros_init gazebo_ros_node diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index 11f90c5a8..c8a4a002f 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -25,6 +25,7 @@ gazebo_dev gazebo_msgs rclcpp + std_srvs tinyxml_vendor geometry_msgs diff --git a/gazebo_ros/src/gazebo_ros_init.cpp b/gazebo_ros/src/gazebo_ros_init.cpp index fdee25843..31863f7a6 100644 --- a/gazebo_ros/src/gazebo_ros_init.cpp +++ b/gazebo_ros/src/gazebo_ros_init.cpp @@ -15,12 +15,18 @@ #include "gazebo_ros/gazebo_ros_init.hpp" #include +#include +#include + #include #include #include + #include +#include #include +#include namespace gazebo_ros { @@ -28,14 +34,77 @@ namespace gazebo_ros class GazeboRosInitPrivate { public: + /// Constructor GazeboRosInitPrivate(); + + /// Callback when a world is created. + /// \param[in] _world_name The world's name + void OnWorldCreated(const std::string & _world_name); + + /// Publish simulation time. + /// \param[in] _info World update information. + void PublishSimTime(const gazebo::common::UpdateInfo & _info); + + /// Callback from ROS service to reset simulation. + /// \param[in] req Empty request + /// \param[out] res Empty response + void OnResetSimulation( + std_srvs::srv::Empty::Request::SharedPtr req, + std_srvs::srv::Empty::Response::SharedPtr res); + + /// Callback from ROS service to reset world. + /// \param[in] req Empty request + /// \param[out] res Empty response + void OnResetWorld( + std_srvs::srv::Empty::Request::SharedPtr req, + std_srvs::srv::Empty::Response::SharedPtr res); + + /// Callback from ROS service to pause physics. + /// \param[in] req Empty request + /// \param[out] res Empty response + void OnPause( + std_srvs::srv::Empty::Request::SharedPtr req, + std_srvs::srv::Empty::Response::SharedPtr res); + + /// Callback from ROS service to unpause (play) physics. + /// \param[in] req Empty request + /// \param[out] res Empty response + void OnUnpause( + std_srvs::srv::Empty::Request::SharedPtr req, + std_srvs::srv::Empty::Response::SharedPtr res); + + /// \brief Keep a pointer to the world. + gazebo::physics::WorldPtr world_; + + /// Gazebo-ROS node gazebo_ros::Node::SharedPtr ros_node_; + + /// Publishes simulation time rclcpp::Publisher::SharedPtr clock_pub_; + + /// ROS service to handle requests to reset simulation. + rclcpp::Service::SharedPtr reset_simulation_service_; + + /// ROS service to handle requests to reset world. + rclcpp::Service::SharedPtr reset_world_service_; + + /// ROS service to handle requests to pause physics. + rclcpp::Service::SharedPtr pause_service_; + + /// ROS service to handle requests to unpause physics. + rclcpp::Service::SharedPtr unpause_service_; + + /// Connection to world update event, called at every iteration gazebo::event::ConnectionPtr world_update_event_; + + /// To be notified once the world is created. + gazebo::event::ConnectionPtr world_created_event_; + + /// Throttler for clock publisher. gazebo_ros::Throttler throttler_; - static constexpr double DEFAULT_PUBLISH_FREQUENCY = 10.; - void PublishSimTime(const gazebo::common::UpdateInfo & _info); + /// Default frequency for clock publisher. + static constexpr double DEFAULT_PUBLISH_FREQUENCY = 10.; }; GazeboRosInit::GazeboRosInit() @@ -85,6 +154,36 @@ void GazeboRosInit::Load(int argc, char ** argv) impl_->world_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin( std::bind(&GazeboRosInitPrivate::PublishSimTime, impl_.get(), std::placeholders::_1)); + + // Get a callback when a world is created + impl_->world_created_event_ = gazebo::event::Events::ConnectWorldCreated( + std::bind(&GazeboRosInitPrivate::OnWorldCreated, impl_.get(), std::placeholders::_1)); +} + +void GazeboRosInitPrivate::OnWorldCreated(const std::string & _world_name) +{ + // Only support one world + world_created_event_.reset(); + + world_ = gazebo::physics::get_world(_world_name); + + // Reset services + reset_simulation_service_ = ros_node_->create_service("reset_simulation", + std::bind(&GazeboRosInitPrivate::OnResetSimulation, this, + std::placeholders::_1, std::placeholders::_2)); + + reset_world_service_ = ros_node_->create_service("reset_world", + std::bind(&GazeboRosInitPrivate::OnResetWorld, this, + std::placeholders::_1, std::placeholders::_2)); + + // Pause services + pause_service_ = ros_node_->create_service("pause_physics", + std::bind(&GazeboRosInitPrivate::OnPause, this, + std::placeholders::_1, std::placeholders::_2)); + + unpause_service_ = ros_node_->create_service("unpause_physics", + std::bind(&GazeboRosInitPrivate::OnUnpause, this, + std::placeholders::_1, std::placeholders::_2)); } GazeboRosInitPrivate::GazeboRosInitPrivate() @@ -94,13 +193,43 @@ GazeboRosInitPrivate::GazeboRosInitPrivate() void GazeboRosInitPrivate::PublishSimTime(const gazebo::common::UpdateInfo & _info) { - if (!throttler_.IsReady(_info.realTime)) {return;} + if (!throttler_.IsReady(_info.realTime)) { + return; + } rosgraph_msgs::msg::Clock clock; clock.clock = gazebo_ros::Convert(_info.simTime); clock_pub_->publish(clock); } +void GazeboRosInitPrivate::OnResetSimulation( + std_srvs::srv::Empty::Request::SharedPtr, + std_srvs::srv::Empty::Response::SharedPtr) +{ + world_->Reset(); +} + +void GazeboRosInitPrivate::OnResetWorld( + std_srvs::srv::Empty::Request::SharedPtr, + std_srvs::srv::Empty::Response::SharedPtr) +{ + world_->ResetEntities(gazebo::physics::Base::MODEL); +} + +void GazeboRosInitPrivate::OnPause( + std_srvs::srv::Empty::Request::SharedPtr, + std_srvs::srv::Empty::Response::SharedPtr) +{ + world_->SetPaused(true); +} + +void GazeboRosInitPrivate::OnUnpause( + std_srvs::srv::Empty::Request::SharedPtr, + std_srvs::srv::Empty::Response::SharedPtr) +{ + world_->SetPaused(false); +} + GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosInit) } // namespace gazebo_ros diff --git a/gazebo_ros/test/CMakeLists.txt b/gazebo_ros/test/CMakeLists.txt index 5bc99546b..5d698f3b6 100644 --- a/gazebo_ros/test/CMakeLists.txt +++ b/gazebo_ros/test/CMakeLists.txt @@ -4,6 +4,7 @@ find_package(gazebo_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) # Plugins set(plugins @@ -31,6 +32,7 @@ file(COPY worlds DESTINATION .) set(tests test_conversions test_gazebo_ros_factory + test_gazebo_ros_init test_gazebo_ros_state test_node test_plugins @@ -60,6 +62,7 @@ foreach(test ${tests}) "rclcpp" "sensor_msgs" "std_msgs" + "std_srvs" ) endforeach() diff --git a/gazebo_ros/test/test_gazebo_ros_init.cpp b/gazebo_ros/test/test_gazebo_ros_init.cpp new file mode 100644 index 000000000..bcce9b52a --- /dev/null +++ b/gazebo_ros/test/test_gazebo_ros_init.cpp @@ -0,0 +1,146 @@ +// 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 + +class GazeboRosInitTest : public gazebo::ServerFixture +{ +}; + +// Since the plugin calls rclcpp:init, and that can be called only once, we can only run one test +TEST_F(GazeboRosInitTest, Commands) +{ + // Load empty world with init plugin and start paused + this->LoadArgs("-u --verbose -s libgazebo_ros_init.so worlds/free_fall.world"); + + // World + auto world = gazebo::physics::get_world(); + ASSERT_NE(nullptr, world); + + // Model + auto model = world->ModelByName("box"); + ASSERT_NE(nullptr, model); + + // ROS node + auto node = std::make_shared("gazebo_ros_init_test"); + ASSERT_NE(nullptr, node); + + // Pause / unpause simulation + { + // Check world is paused + EXPECT_TRUE(world->IsPaused()); + + // Create unpause client + auto unpause_physics_client = node->create_client("unpause_physics"); + ASSERT_NE(nullptr, unpause_physics_client); + EXPECT_TRUE(unpause_physics_client->wait_for_service(std::chrono::seconds(1))); + + // Request + auto request = std::make_shared(); + + auto response_future = unpause_physics_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); + + // Check world is unpaused + EXPECT_FALSE(world->IsPaused()); + + // Create pause client + auto pause_physics_client = node->create_client("pause_physics"); + ASSERT_NE(nullptr, pause_physics_client); + EXPECT_TRUE(pause_physics_client->wait_for_service(std::chrono::seconds(1))); + + // Request + response_future = pause_physics_client->async_send_request(request); + EXPECT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, response_future)); + + response = response_future.get(); + ASSERT_NE(nullptr, response); + + // Check world is paused + EXPECT_TRUE(world->IsPaused()); + } + + // Reset + { + // Step the world so the box falls a bit + world->Step(1000); + + // Check time increases and model moves + EXPECT_GE(world->SimTime(), gazebo::common::Time(1.0)); + EXPECT_LT(model->WorldPose().Pos().Z(), 0.0); + + // Create reset simulation client + auto reset_simulation_client = node->create_client("reset_simulation"); + ASSERT_NE(nullptr, reset_simulation_client); + EXPECT_TRUE(reset_simulation_client->wait_for_service(std::chrono::seconds(1))); + + // Request + auto request = std::make_shared(); + + auto response_future = reset_simulation_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); + + // Check time and model pose + EXPECT_EQ(gazebo::common::Time(0.0), world->SimTime()); + EXPECT_GT(model->WorldPose().Pos().Z(), 0.0); + + // Step the world + world->Step(1000); + + // Check time increases and model moves + EXPECT_GE(world->SimTime(), gazebo::common::Time(1.0)); + EXPECT_LT(model->WorldPose().Pos().Z(), 0.0); + + // Create reset client + auto reset_world_client = node->create_client("reset_world"); + ASSERT_NE(nullptr, reset_world_client); + EXPECT_TRUE(reset_world_client->wait_for_service(std::chrono::seconds(1))); + + // Request + request = std::make_shared(); + + response_future = reset_world_client->async_send_request(request); + EXPECT_EQ(rclcpp::executor::FutureReturnCode::SUCCESS, + rclcpp::spin_until_future_complete(node, response_future)); + + response = response_future.get(); + ASSERT_NE(nullptr, response); + + // Check model pose was reset, but not time + EXPECT_GE(world->SimTime(), gazebo::common::Time(1.0)); + EXPECT_GT(model->WorldPose().Pos().Z(), 0.0); + } + + auto reset_world_client = node->create_client("reset_world"); + ASSERT_NE(nullptr, reset_world_client); + EXPECT_TRUE(reset_world_client->wait_for_service(std::chrono::seconds(1))); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros/test/worlds/free_fall.world b/gazebo_ros/test/worlds/free_fall.world new file mode 100644 index 000000000..fe6ae3ae9 --- /dev/null +++ b/gazebo_ros/test/worlds/free_fall.world @@ -0,0 +1,27 @@ + + + + + model://sun + + + 0 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + From 70dd88eef9c093d9bf16294f0710b37d6aa100d0 Mon Sep 17 00:00:00 2001 From: nzlz Date: Mon, 21 Jan 2019 14:06:24 +0700 Subject: [PATCH 03/11] [ros2] Migration of get/set world, model, joint, link, light properties --- .../gazebo_ros/gazebo_ros_api_plugin.h | 43 -- .../gazebo_ros/src/gazebo_ros_api_plugin.cpp | 415 ----------- gazebo_msgs/CMakeLists.txt | 1 + gazebo_msgs/srv/GetModelList.srv | 5 + gazebo_msgs/srv/GetWorldProperties.srv | 2 + gazebo_ros/CMakeLists.txt | 15 + .../gazebo_ros/gazebo_ros_properties.hpp | 46 ++ gazebo_ros/src/gazebo_ros_factory.cpp | 27 + gazebo_ros/src/gazebo_ros_properties.cpp | 456 ++++++++++++ gazebo_ros/test/CMakeLists.txt | 1 + gazebo_ros/test/test_gazebo_ros_factory.cpp | 92 ++- .../test/test_gazebo_ros_properties.cpp | 416 +++++++++++ .../worlds/gazebo_ros_properties_test.world | 641 +++++++++++++++++ .../worlds/gazebo_ros_properties_demo.world | 676 ++++++++++++++++++ 14 files changed, 2377 insertions(+), 459 deletions(-) create mode 100644 gazebo_msgs/srv/GetModelList.srv create mode 100644 gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp create mode 100644 gazebo_ros/src/gazebo_ros_properties.cpp create mode 100644 gazebo_ros/test/test_gazebo_ros_properties.cpp create mode 100644 gazebo_ros/test/worlds/gazebo_ros_properties_test.world create mode 100644 gazebo_ros/worlds/gazebo_ros_properties_demo.world 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 127bac878..28fe25449 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 "gazebo_msgs/ModelStates.h" @@ -132,36 +121,12 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief void onModelStatesDisconnect(); - /// \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); @@ -238,17 +203,9 @@ class GazeboRosApiPlugin : public SystemPlugin gazebo::event::ConnectionPtr pub_model_states_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 cdd4c336e..51fe7ca56 100644 --- a/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/.ros1_unported/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -233,60 +233,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 = @@ -314,15 +260,6 @@ void GazeboRosApiPlugin::advertiseServices() ros::VoidPtr(), &gazebo_queue_); pub_model_states_ = nh_->advertise(pub_model_states_ao); - // 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 = @@ -341,15 +278,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 = @@ -435,297 +363,6 @@ void GazeboRosApiPlugin::onModelStatesDisconnect() } } -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) { @@ -825,58 +462,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/GetModelList.srv b/gazebo_msgs/srv/GetModelList.srv new file mode 100644 index 000000000..04ad7e756 --- /dev/null +++ b/gazebo_msgs/srv/GetModelList.srv @@ -0,0 +1,5 @@ +--- +float64 sim_time # current sim time +string[] model_names # list of models in the world +bool success # return true if get successful +string status_message # comments if available 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 d956b9b68..e3c96bc77 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -78,6 +78,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 @@ -112,6 +126,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/gazebo_ros_properties.hpp b/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp new file mode 100644 index 000000000..f2c669a32 --- /dev/null +++ b/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.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_PROPERTIES_HPP_ +#define GAZEBO_ROS__GAZEBO_ROS_PROPERTIES_HPP_ + +#include + +#include + +namespace gazebo_ros +{ + +class GazeboRosPropertiesPrivate; + +/// TODO + +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_ \ No newline at end of file diff --git a/gazebo_ros/src/gazebo_ros_factory.cpp b/gazebo_ros/src/gazebo_ros_factory.cpp index 42d415b76..5dc11eff4 100644 --- a/gazebo_ros/src/gazebo_ros_factory.cpp +++ b/gazebo_ros/src/gazebo_ros_factory.cpp @@ -19,7 +19,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -44,6 +46,12 @@ 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[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 +83,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 +137,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 +157,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->model_names.clear(); + res->sim_time = world_->SimTime().Double(); + for (unsigned int i = 0; i < world_->ModelCount(); i ++) + res->model_names.push_back(world_->ModelByIndex(i)->GetName()); + res->success = true; + res->status_message = "GetModelList: got model list"; +} + 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..be67cf3ce --- /dev/null +++ b/gazebo_ros/src/gazebo_ros_properties.cpp @@ -0,0 +1,456 @@ +// 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. + +// TODO. remove unnecessary dependencies +#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 factory 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"; + } + 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"; + } +} + +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"; + } + else + { + /// @todo: FIXME + _res->type = _res->REVOLUTE; + + _res->damping.clear(); + _res->damping.push_back(joint->GetDamping(0)); + + _res->position.clear(); + _res->position.push_back(joint->Position(0)); + + _res->rate.clear(); // use GetVelocity(i) + _res->rate.push_back(joint->GetVelocity(0)); + + _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 body = boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); + if (!body) + { + _res->success = false; + _res->status_message = "GetLinkProperties: link not found, did you forget to scope the link by model name?"; + } + else + { + /// @todo: validate + _res->gravity_mode = body->GetGravityMode(); + + gazebo::physics::InertialPtr inertia = body->GetInertial(); + + _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(); + + _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"; + } +} + +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 == 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; + } +} + +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 body = boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); + if (!body) + { + _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 = 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"; + } +} + +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 == 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); + + gz_properties_light_pub_->Publish(light); + + _res->success = true; + } +} + +GZ_REGISTER_WORLD_PLUGIN(GazeboRosProperties) + +} // namespace gazebo_ros \ No newline at end of file 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_gazebo_ros_factory.cpp b/gazebo_ros/test/test_gazebo_ros_factory.cpp index 6648589c2..b16c7b53d 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,91 @@ 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..cc8fc2955 --- /dev/null +++ b/gazebo_ros/test/test_gazebo_ros_properties.cpp @@ -0,0 +1,416 @@ +// 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 + + +#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::Vector4d & _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::Vector4d & _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, ""); + + EXPECT_EQ(response->body_names[0], "arm_base"); + EXPECT_EQ(response->body_names[1], "arm_shoulder_pan"); + EXPECT_EQ(response->body_names[2], "arm_elbow_pan"); + EXPECT_EQ(response->body_names[3], "arm_wrist_lift"); + EXPECT_EQ(response->body_names[4], "arm_wrist_roll"); + + EXPECT_EQ(response->geom_names[0], "arm_base_geom"); + EXPECT_EQ(response->geom_names[1], "arm_base_geom_arm_trunk"); + EXPECT_EQ(response->geom_names[2], "arm_shoulder_pan_geom"); + EXPECT_EQ(response->geom_names[3], "arm_shoulder_pan_geom_arm_shoulder"); + EXPECT_EQ(response->geom_names[4], "arm_elbow_pan_geom"); + EXPECT_EQ(response->geom_names[5], "arm_elbow_pan_geom_arm_elbow"); + EXPECT_EQ(response->geom_names[6], "arm_elbow_pan_geom_arm_wrist"); + EXPECT_EQ(response->geom_names[7], "arm_wrist_lift_geom"); + EXPECT_EQ(response->geom_names[8], "arm_wrist_roll_geom"); + + 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"); + + std::vector v; // Empty + EXPECT_EQ(response->child_model_names, v); + 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); + + std::vector v; + v.push_back(_damping); + if ((response->damping).size() > 0){ + EXPECT_EQ(response->damping, v); + } +} + +void GazeboRosPropertiesTest::SetJointProperties( + const std::string & _joint_name, + const double & _damping) +{ + auto request = std::make_shared(); + request->joint_name = _joint_name; + + std::vector v; + v.push_back(_damping); + request->ode_joint_config.damping = v; + + 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::Vector4d & _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_NEAR(_diffuse.X(), response->diffuse.r, tol) << _light_name; + EXPECT_NEAR(_diffuse.Y(), response->diffuse.g, tol) << _light_name; + EXPECT_NEAR(_diffuse.Z(), response->diffuse.b, tol) << _light_name; + EXPECT_NEAR(_diffuse.W(), response->diffuse.a, tol) << _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::Vector4d & _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.X(); + request->diffuse.g = _diffuse.Y(); + request->diffuse.b = _diffuse.Z(); + request->diffuse.a = _diffuse.W(); + 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::Vector4d(0.800000011920929, 0.800000011920929, 0.800000011920929, 1.0), + 0.8999999761581421, 0.009999999776482582, 0.0010000000474974513); + + // Set light properties + this->SetLightProperties("sun", ignition::math::Vector4d(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::Vector4d(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..57c495d8a --- /dev/null +++ b/gazebo_ros/test/worlds/gazebo_ros_properties_test.world @@ -0,0 +1,641 @@ + + + + + /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 + + + 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..85b9c7ac7 --- /dev/null +++ b/gazebo_ros/worlds/gazebo_ros_properties_demo.world @@ -0,0 +1,676 @@ + + + + + + + /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 -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 + + + 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 + + + + \ No newline at end of file From b5f4352d1d488ad40ac76b080d6a6cda3b39e676 Mon Sep 17 00:00:00 2001 From: nzlz Date: Mon, 21 Jan 2019 16:40:46 +0700 Subject: [PATCH 04/11] Trying to pass CI test, try n1. --- .../gazebo_ros/gazebo_ros_properties.hpp | 4 +- gazebo_ros/src/gazebo_ros_factory.cpp | 3 +- gazebo_ros/src/gazebo_ros_properties.cpp | 205 +++++++++--------- gazebo_ros/test/test_gazebo_ros_factory.cpp | 1 - .../test/test_gazebo_ros_properties.cpp | 120 +++++----- 5 files changed, 163 insertions(+), 170 deletions(-) diff --git a/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp b/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp index f2c669a32..475a7d499 100644 --- a/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp +++ b/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp @@ -24,8 +24,6 @@ namespace gazebo_ros class GazeboRosPropertiesPrivate; -/// TODO - class GazeboRosProperties : public gazebo::WorldPlugin { public: @@ -43,4 +41,4 @@ class GazeboRosProperties : public gazebo::WorldPlugin }; } // namespace gazebo_ros -#endif // GAZEBO_ROS__GAZEBO_ROS_PROPERTIES_HPP_ \ No newline at end of file +#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 5dc11eff4..75a98fb3e 100644 --- a/gazebo_ros/src/gazebo_ros_factory.cpp +++ b/gazebo_ros/src/gazebo_ros_factory.cpp @@ -163,8 +163,9 @@ void GazeboRosFactoryPrivate::GetModelList( { res->model_names.clear(); res->sim_time = world_->SimTime().Double(); - for (unsigned int i = 0; i < world_->ModelCount(); i ++) + for (unsigned int i = 0; i < world_->ModelCount(); i++){ res->model_names.push_back(world_->ModelByIndex(i)->GetName()); + } res->success = true; res->status_message = "GetModelList: got model list"; } diff --git a/gazebo_ros/src/gazebo_ros_properties.cpp b/gazebo_ros/src/gazebo_ros_properties.cpp index be67cf3ce..29c709c8b 100644 --- a/gazebo_ros/src/gazebo_ros_properties.cpp +++ b/gazebo_ros/src/gazebo_ros_properties.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -// TODO. remove unnecessary dependencies #include #include #include @@ -43,7 +42,6 @@ namespace gazebo_ros class GazeboRosPropertiesPrivate { public: - /// \brief Callback for get model properties service. /// \param[in] req Request /// \param[out] res Response @@ -125,7 +123,6 @@ class GazeboRosPropertiesPrivate /// Publishes light factory messages. gazebo::transport::PublisherPtr gz_properties_light_pub_; - }; GazeboRosProperties::GazeboRosProperties() @@ -181,42 +178,42 @@ void GazeboRosProperties::Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr // 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"); - + 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_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()); + 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"; - } - else - { + }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(); + 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) - { + 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) + 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()); + } } } } @@ -225,16 +222,19 @@ void GazeboRosPropertiesPrivate::GetModelProperties( _res->joint_names.clear(); gazebo::physics::Joint_V joints = model->GetJoints(); - for (unsigned int i=0;i< joints.size(); i++) + 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) + 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 @@ -246,23 +246,19 @@ void GazeboRosPropertiesPrivate::GetModelProperties( } void GazeboRosPropertiesPrivate::GetJointProperties( - gazebo_msgs::srv::GetJointProperties::Request::SharedPtr _req, - gazebo_msgs::srv::GetJointProperties::Response::SharedPtr _res) + 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 ++) - { + for (unsigned int i = 0; i < world_->ModelCount(); i ++) { joint = world_->ModelByIndex(i)->GetJoint(_req->joint_name); - if (joint) break; + if (joint) {break;} } - if (!joint) - { + if (!joint) { _res->success = false; _res->status_message = "GetJointProperties: joint not found"; - } - else - { + }else{ /// @todo: FIXME _res->type = _res->REVOLUTE; @@ -272,7 +268,7 @@ void GazeboRosPropertiesPrivate::GetJointProperties( _res->position.clear(); _res->position.push_back(joint->Position(0)); - _res->rate.clear(); // use GetVelocity(i) + _res->rate.clear(); // use GetVelocity(i) _res->rate.push_back(joint->GetVelocity(0)); _res->success = true; @@ -281,17 +277,16 @@ void GazeboRosPropertiesPrivate::GetJointProperties( } void GazeboRosPropertiesPrivate::GetLinkProperties( - gazebo_msgs::srv::GetLinkProperties::Request::SharedPtr _req, - gazebo_msgs::srv::GetLinkProperties::Response::SharedPtr _res) + gazebo_msgs::srv::GetLinkProperties::Request::SharedPtr _req, + gazebo_msgs::srv::GetLinkProperties::Response::SharedPtr _res) { - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); - if (!body) - { + gazebo::physics::LinkPtr body = + boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); + if (!body) { _res->success = false; - _res->status_message = "GetLinkProperties: link not found, did you forget to scope the link by model name?"; - } - else - { + _res->status_message = + "GetLinkProperties: link not found, did you forget to scope the link by model name?"; + }else{ /// @todo: validate _res->gravity_mode = body->GetGravityMode(); @@ -311,7 +306,7 @@ void GazeboRosPropertiesPrivate::GetLinkProperties( _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.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; @@ -322,17 +317,15 @@ void GazeboRosPropertiesPrivate::GetLinkProperties( } void GazeboRosPropertiesPrivate::GetLightProperties( - gazebo_msgs::srv::GetLightProperties::Request::SharedPtr _req, - gazebo_msgs::srv::GetLightProperties::Response::SharedPtr _res) + 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 == NULL) - { + if (phy_light == NULL) { _res->success = false; - _res->status_message = "getLightProperties: Requested light " + _req->light_name + " not found!"; - } - else - { + _res->status_message = "getLightProperties: Requested light " + _req->light_name + + " not found!"; + }else{ gazebo::msgs::Light light; phy_light->FillMsg(light); @@ -350,44 +343,41 @@ void GazeboRosPropertiesPrivate::GetLightProperties( } void GazeboRosPropertiesPrivate::SetJointProperties( - gazebo_msgs::srv::SetJointProperties::Request::SharedPtr _req, - gazebo_msgs::srv::SetJointProperties::Response::SharedPtr _res) + 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. + /// @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 ++) - { + for (unsigned int i = 0; i < world_->ModelCount(); i ++) { joint = world_->ModelByIndex(i)->GetJoint(_req->joint_name); - if (joint) break; + if (joint) {break;} } - if (!joint) - { + 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]); + }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"; @@ -395,21 +385,24 @@ void GazeboRosPropertiesPrivate::SetJointProperties( } void GazeboRosPropertiesPrivate::SetLinkProperties( - gazebo_msgs::srv::SetLinkProperties::Request::SharedPtr _req, - gazebo_msgs::srv::SetLinkProperties::Response::SharedPtr _res) + gazebo_msgs::srv::SetLinkProperties::Request::SharedPtr _req, + gazebo_msgs::srv::SetLinkProperties::Response::SharedPtr _res) { - gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); - if (!body) - { + gazebo::physics::LinkPtr body = + boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); + if (!body) { _res->success = false; - _res->status_message = "SetLinkProperties: link not found, did you forget to scope the link by model name?"; - } - else - { + _res->status_message = + "SetLinkProperties: link not found, did you forget to scope the link by model name?"; + }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.SetInertiaRotation(ignition::math::Quaternion(_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); @@ -421,17 +414,15 @@ void GazeboRosPropertiesPrivate::SetLinkProperties( } void GazeboRosPropertiesPrivate::SetLightProperties( - gazebo_msgs::srv::SetLightProperties::Request::SharedPtr _req, - gazebo_msgs::srv::SetLightProperties::Response::SharedPtr _res) + 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 == NULL) - { + if (phy_light == NULL) { _res->success = false; - _res->status_message = "setLightProperties: Requested light " + _req->light_name + " not found!"; - } - else - { + _res->status_message = "setLightProperties: Requested light " + _req->light_name + + " not found!"; + }else{ gazebo::msgs::Light light; phy_light->FillMsg(light); @@ -453,4 +444,4 @@ void GazeboRosPropertiesPrivate::SetLightProperties( GZ_REGISTER_WORLD_PLUGIN(GazeboRosProperties) -} // namespace gazebo_ros \ No newline at end of file +} // namespace gazebo_ros diff --git a/gazebo_ros/test/test_gazebo_ros_factory.cpp b/gazebo_ros/test/test_gazebo_ros_factory.cpp index b16c7b53d..f02275e4d 100644 --- a/gazebo_ros/test/test_gazebo_ros_factory.cpp +++ b/gazebo_ros/test/test_gazebo_ros_factory.cpp @@ -131,7 +131,6 @@ TEST_F(GazeboRosFactoryTest, SpawnDeleteList) 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 diff --git a/gazebo_ros/test/test_gazebo_ros_properties.cpp b/gazebo_ros/test/test_gazebo_ros_properties.cpp index cc8fc2955..648cf68fe 100644 --- a/gazebo_ros/test/test_gazebo_ros_properties.cpp +++ b/gazebo_ros/test/test_gazebo_ros_properties.cpp @@ -32,7 +32,7 @@ #include #include - +#include #define tol 0.01 @@ -43,62 +43,68 @@ class GazeboRosPropertiesTest : public gazebo::ServerFixture void SetUp() override; void GetModelProperties( - const std::string & _model_name); + const std::string & _model_name); void GetJointProperties( - const std::string & _joint_name, - const double & _damping); + 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); + 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::Vector4d & _diffuse, - const double & _attenuation_constant, - const double & _attenuation_linear, - const double & _attenuation_quadratic); + const std::string & _light_name, + const ignition::math::Vector4d & _diffuse, + const double & _attenuation_constant, + const double & _attenuation_linear, + const double & _attenuation_quadratic); void SetJointProperties( - const std::string & _joint_name, - const double & _damping); + 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); + 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::Vector4d & _diffuse, - const double & _attenuation_constant, - const double & _attenuation_linear, - const double & _attenuation_quadratic); + const std::string & _light_name, + const ignition::math::Vector4d & _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_; - + 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() @@ -148,11 +154,10 @@ void GazeboRosPropertiesTest::SetUp() 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) + const std::string & _model_name) { // Get spawned Model properties auto entity = world_->EntityByName(_model_name); @@ -193,7 +198,7 @@ void GazeboRosPropertiesTest::GetModelProperties( EXPECT_EQ(response->joint_names[1], "arm_elbow_pan_joint"); EXPECT_EQ(response->joint_names[2], "arm_wrist_lift_joint"); - std::vector v; // Empty + std::vector v; // Empty EXPECT_EQ(response->child_model_names, v); EXPECT_FALSE(response->is_static); } @@ -215,7 +220,7 @@ void GazeboRosPropertiesTest::GetJointProperties( std::vector v; v.push_back(_damping); - if ((response->damping).size() > 0){ + if ((response->damping).size() > 0) { EXPECT_EQ(response->damping, v); } } @@ -250,7 +255,7 @@ void GazeboRosPropertiesTest::GetLinkProperties( const double & _iyy, const double & _iyz, const double & _izz) -{ +{ auto request = std::make_shared(); request->link_name = _link_name; @@ -379,32 +384,31 @@ TEST_F(GazeboRosPropertiesTest, GetSetProperties) // 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); + 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); + 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); + 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::Vector4d(0.800000011920929, 0.800000011920929, 0.800000011920929, 1.0), - 0.8999999761581421, 0.009999999776482582, 0.0010000000474974513); + this->GetLightProperties("sun", ignition::math::Vector4d(0.800000011920929, 0.800000011920929, + 0.800000011920929, 1.0), 0.8999999761581421, 0.009999999776482582, 0.0010000000474974513); // Set light properties this->SetLightProperties("sun", ignition::math::Vector4d(0.7, 0.1, 0.5, 1.0), - 0.92, 0.0092, 0.002); + 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::Vector4d(0.7, 0.1, 0.5, 1.0), - 0.92, 0.0092, 0.002); + 0.92, 0.0092, 0.002); } } From cb891db90eebac84bb6c45e38f2ad084920bc927 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 5 Mar 2019 16:31:02 -0800 Subject: [PATCH 05/11] clean up some linter warnings --- gazebo_ros/src/gazebo_ros_factory.cpp | 2 +- gazebo_ros/src/gazebo_ros_properties.cpp | 51 ++++++++++--------- .../test/test_gazebo_ros_properties.cpp | 14 ++--- 3 files changed, 34 insertions(+), 33 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_factory.cpp b/gazebo_ros/src/gazebo_ros_factory.cpp index 75a98fb3e..25ee2ad33 100644 --- a/gazebo_ros/src/gazebo_ros_factory.cpp +++ b/gazebo_ros/src/gazebo_ros_factory.cpp @@ -163,7 +163,7 @@ void GazeboRosFactoryPrivate::GetModelList( { res->model_names.clear(); res->sim_time = world_->SimTime().Double(); - for (unsigned int i = 0; i < world_->ModelCount(); i++){ + for (unsigned int i = 0; i < world_->ModelCount(); ++i) { res->model_names.push_back(world_->ModelByIndex(i)->GetName()); } res->success = true; diff --git a/gazebo_ros/src/gazebo_ros_properties.cpp b/gazebo_ros/src/gazebo_ros_properties.cpp index 29c709c8b..99c9225d1 100644 --- a/gazebo_ros/src/gazebo_ros_properties.cpp +++ b/gazebo_ros/src/gazebo_ros_properties.cpp @@ -179,7 +179,7 @@ void GazeboRosProperties::Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr 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"); + impl_->gz_node_->Advertise("~/light/modify"); } void GazeboRosPropertiesPrivate::GetModelProperties( @@ -193,23 +193,23 @@ void GazeboRosPropertiesPrivate::GetModelProperties( _req->model_name.c_str()); _res->success = false; _res->status_message = "GetModelProperties: model does not exist"; - }else{ + } else { // get model parent name - gazebo::physics::ModelPtr parent_model = - boost::dynamic_pointer_cast(model->GetParent()); + 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 = + 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 = + 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()); @@ -222,15 +222,14 @@ void GazeboRosPropertiesPrivate::GetModelProperties( _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() ); + 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 = + 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() ); @@ -250,15 +249,17 @@ void GazeboRosPropertiesPrivate::GetJointProperties( gazebo_msgs::srv::GetJointProperties::Response::SharedPtr _res) { gazebo::physics::JointPtr joint; - for (unsigned int i = 0; i < world_->ModelCount(); i ++) { + for (unsigned int i = 0; i < world_->ModelCount(); ++i) { joint = world_->ModelByIndex(i)->GetJoint(_req->joint_name); - if (joint) {break;} + if (joint) { + break; + } } if (!joint) { _res->success = false; _res->status_message = "GetJointProperties: joint not found"; - }else{ + } else { /// @todo: FIXME _res->type = _res->REVOLUTE; @@ -280,13 +281,13 @@ void GazeboRosPropertiesPrivate::GetLinkProperties( gazebo_msgs::srv::GetLinkProperties::Request::SharedPtr _req, gazebo_msgs::srv::GetLinkProperties::Response::SharedPtr _res) { - gazebo::physics::LinkPtr body = + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); if (!body) { _res->success = false; - _res->status_message = + _res->status_message = "GetLinkProperties: link not found, did you forget to scope the link by model name?"; - }else{ + } else { /// @todo: validate _res->gravity_mode = body->GetGravityMode(); @@ -325,7 +326,7 @@ void GazeboRosPropertiesPrivate::GetLightProperties( _res->success = false; _res->status_message = "getLightProperties: Requested light " + _req->light_name + " not found!"; - }else{ + } else { gazebo::msgs::Light light; phy_light->FillMsg(light); @@ -346,10 +347,10 @@ 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 + /// @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 ++) { + for (unsigned int i = 0; i < world_->ModelCount(); ++i) { joint = world_->ModelByIndex(i)->GetJoint(_req->joint_name); if (joint) {break;} } @@ -388,11 +389,11 @@ void GazeboRosPropertiesPrivate::SetLinkProperties( gazebo_msgs::srv::SetLinkProperties::Request::SharedPtr _req, gazebo_msgs::srv::SetLinkProperties::Response::SharedPtr _res) { - gazebo::physics::LinkPtr body = + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); if (!body) { _res->success = false; - _res->status_message = + _res->status_message = "SetLinkProperties: link not found, did you forget to scope the link by model name?"; }else{ gazebo::physics::InertialPtr mass = body->GetInertial(); @@ -403,7 +404,7 @@ void GazeboRosPropertiesPrivate::SetLinkProperties( _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->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 diff --git a/gazebo_ros/test/test_gazebo_ros_properties.cpp b/gazebo_ros/test/test_gazebo_ros_properties.cpp index 648cf68fe..2a9af13ce 100644 --- a/gazebo_ros/test/test_gazebo_ros_properties.cpp +++ b/gazebo_ros/test/test_gazebo_ros_properties.cpp @@ -91,19 +91,19 @@ class GazeboRosPropertiesTest : public gazebo::ServerFixture gazebo::physics::WorldPtr world_; rclcpp::Node::SharedPtr node_; - std::shared_ptr> + std::shared_ptr> get_model_properties_client_; - std::shared_ptr> + std::shared_ptr> get_joint_properties_client_; - std::shared_ptr> + std::shared_ptr> get_link_properties_client_; - std::shared_ptr> + std::shared_ptr> get_light_properties_client_; - std::shared_ptr> + std::shared_ptr> set_joint_properties_client_; - std::shared_ptr> + std::shared_ptr> set_link_properties_client_; - std::shared_ptr> + std::shared_ptr> set_light_properties_client_; }; From 2d76a9bf8343e966c69a87e84831c6ff5245c433 Mon Sep 17 00:00:00 2001 From: nzlz Date: Wed, 6 Mar 2019 14:26:50 +0700 Subject: [PATCH 06/11] Requested changes in review, unfinished --- gazebo_msgs/srv/GetModelList.srv | 4 +- gazebo_ros/src/gazebo_ros_factory.cpp | 9 +-- gazebo_ros/src/gazebo_ros_properties.cpp | 71 +++++++++++++----------- gazebo_ros/test/test_plugins.cpp | 4 +- 4 files changed, 49 insertions(+), 39 deletions(-) diff --git a/gazebo_msgs/srv/GetModelList.srv b/gazebo_msgs/srv/GetModelList.srv index 04ad7e756..e5e3e9165 100644 --- a/gazebo_msgs/srv/GetModelList.srv +++ b/gazebo_msgs/srv/GetModelList.srv @@ -1,5 +1,5 @@ --- -float64 sim_time # current sim time +std_msgs/Header header # Standard metadata for higher-level stamped data types. + # * header.stamp Timestamp related to the pose. string[] model_names # list of models in the world bool success # return true if get successful -string status_message # comments if available diff --git a/gazebo_ros/src/gazebo_ros_factory.cpp b/gazebo_ros/src/gazebo_ros_factory.cpp index 25ee2ad33..7c641d224 100644 --- a/gazebo_ros/src/gazebo_ros_factory.cpp +++ b/gazebo_ros/src/gazebo_ros_factory.cpp @@ -17,13 +17,14 @@ #include #include #include +#include #include #include -#include #include #include #include #include +#include #include #include #include @@ -47,6 +48,7 @@ class GazeboRosFactoryPrivate 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, @@ -161,13 +163,12 @@ 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(); - res->sim_time = world_->SimTime().Double(); - for (unsigned int i = 0; i < world_->ModelCount(); ++i) { + for (unsigned int i = 0; i < world_->ModelCount(); i++) { res->model_names.push_back(world_->ModelByIndex(i)->GetName()); } res->success = true; - res->status_message = "GetModelList: got model list"; } void GazeboRosFactoryPrivate::SpawnEntity( diff --git a/gazebo_ros/src/gazebo_ros_properties.cpp b/gazebo_ros/src/gazebo_ros_properties.cpp index 99c9225d1..ce3841c88 100644 --- a/gazebo_ros/src/gazebo_ros_properties.cpp +++ b/gazebo_ros/src/gazebo_ros_properties.cpp @@ -20,15 +20,15 @@ #include #include #include -#include -#include #include -#include #include +#include +#include #include +#include #include -#include #include +#include #include #include @@ -202,13 +202,13 @@ void GazeboRosPropertiesPrivate::GetModelProperties( // get list of child bodies, geoms _res->body_names.clear(); _res->geom_names.clear(); - for (unsigned int i = 0; i < model->GetChildCount(); ++i) { + 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) { + for (unsigned int j = 0; j < body->GetChildCount(); j++) { gazebo::physics::CollisionPtr geom = boost::dynamic_pointer_cast(body->GetChild(j)); if (geom) { @@ -249,7 +249,7 @@ void GazeboRosPropertiesPrivate::GetJointProperties( gazebo_msgs::srv::GetJointProperties::Response::SharedPtr _res) { gazebo::physics::JointPtr joint; - for (unsigned int i = 0; i < world_->ModelCount(); ++i) { + for (unsigned int i = 0; i < world_->ModelCount(); i++) { joint = world_->ModelByIndex(i)->GetJoint(_req->joint_name); if (joint) { break; @@ -322,10 +322,10 @@ void GazeboRosPropertiesPrivate::GetLightProperties( gazebo_msgs::srv::GetLightProperties::Response::SharedPtr _res) { gazebo::physics::LightPtr phy_light = world_->LightByName(_req->light_name); - if (phy_light == NULL) { + if (phy_light == nullptr) { _res->success = false; - _res->status_message = "getLightProperties: Requested light " + _req->light_name - + " not found!"; + _res->status_message = "getLightProperties: Requested light " + _req->light_name + + " not found!"; } else { gazebo::msgs::Light light; phy_light->FillMsg(light); @@ -350,7 +350,7 @@ void GazeboRosPropertiesPrivate::SetJointProperties( /// @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) { + for (unsigned int i = 0; i < world_->ModelCount(); i++) { joint = world_->ModelByIndex(i)->GetJoint(_req->joint_name); if (joint) {break;} } @@ -358,27 +358,37 @@ void GazeboRosPropertiesPrivate::SetJointProperties( 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++) + } 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++) + } + 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++) + } + 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++) + } + 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++) + } + 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++) + } + 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++) + } + 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++) + } + 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++) + } + 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++) + } + 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"; @@ -395,19 +405,18 @@ void GazeboRosPropertiesPrivate::SetLinkProperties( _res->success = false; _res->status_message = "SetLinkProperties: link not found, did you forget to scope the link by model name?"; - }else{ + } else { gazebo::physics::InertialPtr mass = body->GetInertial(); // @todo: FIXME: add inertia matrix rotation to Gazebo // mass.SetInertiaRotation(ignition::math::Quaternion(_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)); + _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"; @@ -419,11 +428,11 @@ void GazeboRosPropertiesPrivate::SetLightProperties( gazebo_msgs::srv::SetLightProperties::Response::SharedPtr _res) { gazebo::physics::LightPtr phy_light = world_->LightByName(_req->light_name); - if (phy_light == NULL) { + if (phy_light == nullptr) { _res->success = false; - _res->status_message = "setLightProperties: Requested light " + _req->light_name - + " not found!"; - }else{ + _res->status_message = "setLightProperties: Requested light " + _req->light_name + + " not found!"; + } else { gazebo::msgs::Light light; phy_light->FillMsg(light); diff --git a/gazebo_ros/test/test_plugins.cpp b/gazebo_ros/test/test_plugins.cpp index 34204e982..7964bcfdc 100644 --- a/gazebo_ros/test/test_plugins.cpp +++ b/gazebo_ros/test/test_plugins.cpp @@ -72,9 +72,9 @@ INSTANTIATE_TEST_CASE_P(Plugins, TestPlugins, ::testing::Values( TestParams({{"-s", "./libcreate_node_without_init.so"}, {"test"}}), TestParams({{"-s", "./libmultiple_nodes.so"}, {"testA", "testB"}}), TestParams({{"-s", "libgazebo_ros_init.so", "worlds/ros_world_plugin.world", - "hello_ros_world:/test:=/new_test"}, {"new_test"}}), + "hello_ros_world:/test:=/new_test"}, {"new_test"}}), TestParams({{"-s", "libgazebo_ros_init.so", "-s", "libgazebo_ros_factory.so", - "worlds/ros_world_plugin.world"}, {"test"}}), + "worlds/ros_world_plugin.world"}, {"test"}}), TestParams({{"-s", "libgazebo_ros_init.so", "worlds/sdf_node_plugin.world"}, {"/foo/my_topic"}}) ), ); From e395e5c96d2988be58474218f7f74c191890d6eb Mon Sep 17 00:00:00 2001 From: nzlz Date: Wed, 6 Mar 2019 14:44:09 +0700 Subject: [PATCH 07/11] Fix uncrustify --- gazebo_ros/src/gazebo_ros_properties.cpp | 26 ++++++++++++------------ 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_properties.cpp b/gazebo_ros/src/gazebo_ros_properties.cpp index ce3841c88..da21f0804 100644 --- a/gazebo_ros/src/gazebo_ros_properties.cpp +++ b/gazebo_ros/src/gazebo_ros_properties.cpp @@ -323,9 +323,9 @@ void GazeboRosPropertiesPrivate::GetLightProperties( { 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!"; + _res->success = false; + _res->status_message = "getLightProperties: Requested light " + _req->light_name + + " not found!"; } else { gazebo::msgs::Light light; phy_light->FillMsg(light); @@ -359,34 +359,34 @@ void GazeboRosPropertiesPrivate::SetJointProperties( _res->success = false; _res->status_message = "SetJointProperties: joint not found"; } else { - for (unsigned int i = 0; i < _req->ode_joint_config.damping.size(); i++){ + 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++){ + 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++){ + 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++){ + 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++){ + 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++){ + 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++){ + 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++){ + 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++){ + 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++){ + for (unsigned int i = 0; i < _req->ode_joint_config.vel.size(); i++) { joint->SetParam("vel", i, _req->ode_joint_config.vel[i]); } From 5a43460a624fe427f7ae2b0c38137a234c8eb361 Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Tue, 6 Aug 2019 21:06:57 +0530 Subject: [PATCH 08/11] Address reviews --- gazebo_msgs/srv/GetModelProperties.srv | 6 +- gazebo_ros/src/gazebo_ros_properties.cpp | 116 +++++++++--------- .../test/test_gazebo_ros_properties.cpp | 34 ++--- .../worlds/gazebo_ros_properties_test.world | 36 +++--- .../worlds/gazebo_ros_properties_demo.world | 36 +++--- 5 files changed, 114 insertions(+), 114 deletions(-) diff --git a/gazebo_msgs/srv/GetModelProperties.srv b/gazebo_msgs/srv/GetModelProperties.srv index c83783dad..8d33b754c 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_link_name # name of canonical link, link names are prefixed by model name, e.g. pr2::base_link +string[] link_names # list of links, link names are prefixed by model name, e.g. pr2::base_link +string[] collision_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_ros/src/gazebo_ros_properties.cpp b/gazebo_ros/src/gazebo_ros_properties.cpp index da21f0804..19690c30e 100644 --- a/gazebo_ros/src/gazebo_ros_properties.cpp +++ b/gazebo_ros/src/gazebo_ros_properties.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -30,6 +31,7 @@ #include #include #include +#include #include #include @@ -121,7 +123,7 @@ class GazeboRosPropertiesPrivate /// Gazebo node for communication. gazebo::transport::NodePtr gz_node_; - /// Publishes light factory messages. + /// Publishes light modify messages. gazebo::transport::PublisherPtr gz_properties_light_pub_; }; @@ -193,55 +195,58 @@ void GazeboRosPropertiesPrivate::GetModelProperties( _req->model_name.c_str()); _res->success = false; _res->status_message = "GetModelProperties: model does not exist"; - } 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()); - } + 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->link_names.clear(); + _res->collision_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_link_name = link->GetScopedName(); + } + _res->link_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->collision_names.push_back(collision->GetName()); } } } + } - // get list of joints - _res->joint_names.clear(); + // 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()); - } + 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() ); - } + // 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(); + // is model static + _res->is_static = model->IsStatic(); - _res->success = true; - _res->status_message = "GetModelProperties: got properties"; - } + _res->success = true; + _res->status_message = "GetModelProperties: got properties"; } void GazeboRosPropertiesPrivate::GetJointProperties( @@ -281,19 +286,19 @@ void GazeboRosPropertiesPrivate::GetLinkProperties( gazebo_msgs::srv::GetLinkProperties::Request::SharedPtr _req, gazebo_msgs::srv::GetLinkProperties::Response::SharedPtr _res) { - gazebo::physics::LinkPtr body = + gazebo::physics::LinkPtr link = boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); - if (!body) { + if (!link) { _res->success = false; _res->status_message = "GetLinkProperties: link not found, did you forget to scope the link by model name?"; } else { /// @todo: validate - _res->gravity_mode = body->GetGravityMode(); + _res->gravity_mode = link->GetGravityMode(); - gazebo::physics::InertialPtr inertia = body->GetInertial(); + gazebo::physics::InertialPtr inertia = link->GetInertial(); - _res->mass = body->GetInertial()->Mass(); + _res->mass = link->GetInertial()->Mass(); _res->ixx = inertia->IXX(); _res->iyy = inertia->IYY(); @@ -302,15 +307,10 @@ void GazeboRosPropertiesPrivate::GetLinkProperties( _res->ixz = inertia->IXZ(); _res->iyz = inertia->IYZ(); - ignition::math::Vector3d com = body->GetInertial()->CoG(); + auto com = link->GetInertial()->Pose(); - _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->com.position = gazebo_ros::Convert(com.Pos()); + _res->com.orientation = gazebo_ros::Convert(com.Rot()); _res->success = true; _res->status_message = "GetLinkProperties: got properties"; @@ -399,14 +399,14 @@ void GazeboRosPropertiesPrivate::SetLinkProperties( gazebo_msgs::srv::SetLinkProperties::Request::SharedPtr _req, gazebo_msgs::srv::SetLinkProperties::Response::SharedPtr _res) { - gazebo::physics::LinkPtr body = + gazebo::physics::LinkPtr link = boost::dynamic_pointer_cast(world_->EntityByName(_req->link_name)); - if (!body) { + 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 = body->GetInertial(); + gazebo::physics::InertialPtr mass = link->GetInertial(); // @todo: FIXME: add inertia matrix rotation to Gazebo // mass.SetInertiaRotation(ignition::math::Quaternion(_req->com.orientation.w, // _res->com.orientation.x,_req->com.orientation.y _req->com.orientation.z)); @@ -416,7 +416,7 @@ void GazeboRosPropertiesPrivate::SetLinkProperties( _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); + link->SetGravityMode(_req->gravity_mode); _res->success = true; _res->status_message = "SetLinkProperties: properties set"; diff --git a/gazebo_ros/test/test_gazebo_ros_properties.cpp b/gazebo_ros/test/test_gazebo_ros_properties.cpp index 2a9af13ce..330d7b3ca 100644 --- a/gazebo_ros/test/test_gazebo_ros_properties.cpp +++ b/gazebo_ros/test/test_gazebo_ros_properties.cpp @@ -176,23 +176,23 @@ void GazeboRosPropertiesTest::GetModelProperties( // gazebo models simple_arm EXPECT_EQ(response->parent_model_name, ""); - EXPECT_EQ(response->canonical_body_name, ""); - - EXPECT_EQ(response->body_names[0], "arm_base"); - EXPECT_EQ(response->body_names[1], "arm_shoulder_pan"); - EXPECT_EQ(response->body_names[2], "arm_elbow_pan"); - EXPECT_EQ(response->body_names[3], "arm_wrist_lift"); - EXPECT_EQ(response->body_names[4], "arm_wrist_roll"); - - EXPECT_EQ(response->geom_names[0], "arm_base_geom"); - EXPECT_EQ(response->geom_names[1], "arm_base_geom_arm_trunk"); - EXPECT_EQ(response->geom_names[2], "arm_shoulder_pan_geom"); - EXPECT_EQ(response->geom_names[3], "arm_shoulder_pan_geom_arm_shoulder"); - EXPECT_EQ(response->geom_names[4], "arm_elbow_pan_geom"); - EXPECT_EQ(response->geom_names[5], "arm_elbow_pan_geom_arm_elbow"); - EXPECT_EQ(response->geom_names[6], "arm_elbow_pan_geom_arm_wrist"); - EXPECT_EQ(response->geom_names[7], "arm_wrist_lift_geom"); - EXPECT_EQ(response->geom_names[8], "arm_wrist_roll_geom"); + EXPECT_EQ(response->canonical_link_name, "simple_arm::arm_base"); + + EXPECT_EQ(response->link_names[0], "simple_arm::arm_base"); + EXPECT_EQ(response->link_names[1], "simple_arm::arm_shoulder_pan"); + EXPECT_EQ(response->link_names[2], "simple_arm::arm_elbow_pan"); + EXPECT_EQ(response->link_names[3], "simple_arm::arm_wrist_lift"); + EXPECT_EQ(response->link_names[4], "simple_arm::arm_wrist_roll"); + + EXPECT_EQ(response->collision_names[0], "arm_base_collision"); + EXPECT_EQ(response->collision_names[1], "arm_base_collision_arm_trunk"); + EXPECT_EQ(response->collision_names[2], "arm_shoulder_pan_collision"); + EXPECT_EQ(response->collision_names[3], "arm_shoulder_pan_collision_arm_shoulder"); + EXPECT_EQ(response->collision_names[4], "arm_elbow_pan_collision"); + EXPECT_EQ(response->collision_names[5], "arm_elbow_pan_collision_arm_elbow"); + EXPECT_EQ(response->collision_names[6], "arm_elbow_pan_collision_arm_wrist"); + EXPECT_EQ(response->collision_names[7], "arm_wrist_lift_collision"); + EXPECT_EQ(response->collision_names[8], "arm_wrist_roll_collision"); EXPECT_EQ(response->joint_names[0], "arm_shoulder_pan_joint"); EXPECT_EQ(response->joint_names[1], "arm_elbow_pan_joint"); diff --git a/gazebo_ros/test/worlds/gazebo_ros_properties_test.world b/gazebo_ros/test/worlds/gazebo_ros_properties_test.world index 57c495d8a..829dac7f7 100644 --- a/gazebo_ros/test/worlds/gazebo_ros_properties_test.world +++ b/gazebo_ros/test/worlds/gazebo_ros_properties_test.world @@ -100,7 +100,7 @@ 101 - + 0 0 0.05 0 -0 0 @@ -121,7 +121,7 @@ - + 0 0 0.05 0 -0 0 @@ -135,7 +135,7 @@ - + 0 0 0.6 0 -0 0 @@ -157,7 +157,7 @@ - + 0 0 0.6 0 -0 0 @@ -190,7 +190,7 @@ 1.1 - + 0 0 0.05 0 -0 0 @@ -212,7 +212,7 @@ - + 0 0 0.05 0 -0 0 @@ -227,7 +227,7 @@ - + 0.55 0 0.05 0 -0 0 @@ -248,7 +248,7 @@ - + 0.55 0 0.05 0 -0 0 @@ -280,7 +280,7 @@ 1.2 - + 0 0 0.1 0 -0 0 @@ -302,7 +302,7 @@ - + 0 0 0.1 0 -0 0 @@ -317,7 +317,7 @@ - + 0.3 0 0.15 0 -0 0 @@ -338,7 +338,7 @@ - + 0.3 0 0.15 0 -0 0 @@ -352,7 +352,7 @@ - + 0.55 0 0.15 0 -0 0 @@ -374,7 +374,7 @@ - + 0.55 0 0.15 0 -0 0 @@ -407,7 +407,7 @@ 0.1 - + 0 0 0.5 0 -0 0 @@ -429,7 +429,7 @@ - + 0 0 0.5 0 -0 0 @@ -462,7 +462,7 @@ 0.1 - + 0 0 0.025 0 -0 0 @@ -484,7 +484,7 @@ - + 0 0 0.025 0 -0 0 diff --git a/gazebo_ros/worlds/gazebo_ros_properties_demo.world b/gazebo_ros/worlds/gazebo_ros_properties_demo.world index 85b9c7ac7..54cbb587d 100644 --- a/gazebo_ros/worlds/gazebo_ros_properties_demo.world +++ b/gazebo_ros/worlds/gazebo_ros_properties_demo.world @@ -135,7 +135,7 @@ 101 - + 0 0 0.05 0 -0 0 @@ -156,7 +156,7 @@ - + 0 0 0.05 0 -0 0 @@ -170,7 +170,7 @@ - + 0 0 0.6 0 -0 0 @@ -192,7 +192,7 @@ - + 0 0 0.6 0 -0 0 @@ -225,7 +225,7 @@ 1.1 - + 0 0 0.05 0 -0 0 @@ -247,7 +247,7 @@ - + 0 0 0.05 0 -0 0 @@ -262,7 +262,7 @@ - + 0.55 0 0.05 0 -0 0 @@ -283,7 +283,7 @@ - + 0.55 0 0.05 0 -0 0 @@ -315,7 +315,7 @@ 1.2 - + 0 0 0.1 0 -0 0 @@ -337,7 +337,7 @@ - + 0 0 0.1 0 -0 0 @@ -352,7 +352,7 @@ - + 0.3 0 0.15 0 -0 0 @@ -373,7 +373,7 @@ - + 0.3 0 0.15 0 -0 0 @@ -387,7 +387,7 @@ - + 0.55 0 0.15 0 -0 0 @@ -409,7 +409,7 @@ - + 0.55 0 0.15 0 -0 0 @@ -442,7 +442,7 @@ 0.1 - + 0 0 0.5 0 -0 0 @@ -464,7 +464,7 @@ - + 0 0 0.5 0 -0 0 @@ -497,7 +497,7 @@ 0.1 - + 0 0 0.025 0 -0 0 @@ -519,7 +519,7 @@ - + 0 0 0.025 0 -0 0 From 5293a5ae192a7bc34a08758eeb7cb51dbd74fcb3 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 14 Aug 2019 17:24:04 -0700 Subject: [PATCH 09/11] more tests, joint types Signed-off-by: Louise Poubel --- gazebo_msgs/srv/GetJointProperties.srv | 8 +-- gazebo_msgs/srv/GetModelList.srv | 2 +- gazebo_ros/src/gazebo_ros_properties.cpp | 66 +++++++++++-------- .../test/test_gazebo_ros_properties.cpp | 59 ++++++++++------- .../worlds/gazebo_ros_properties_test.world | 16 ++++- .../worlds/gazebo_ros_properties_demo.world | 25 +------ 6 files changed, 96 insertions(+), 80 deletions(-) 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 index e5e3e9165..98da46c0c 100644 --- a/gazebo_msgs/srv/GetModelList.srv +++ b/gazebo_msgs/srv/GetModelList.srv @@ -1,5 +1,5 @@ --- std_msgs/Header header # Standard metadata for higher-level stamped data types. - # * header.stamp Timestamp related to the pose. + # * 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_ros/src/gazebo_ros_properties.cpp b/gazebo_ros/src/gazebo_ros_properties.cpp index 19690c30e..ad3994c2e 100644 --- a/gazebo_ros/src/gazebo_ros_properties.cpp +++ b/gazebo_ros/src/gazebo_ros_properties.cpp @@ -205,7 +205,7 @@ void GazeboRosPropertiesPrivate::GetModelProperties( // get list of child links, collisions _res->link_names.clear(); _res->collision_names.clear(); - for (unsigned int i = 0; i < model->GetChildCount(); i++) { + for (unsigned int i = 0; i < model->GetChildCount(); ++i) { gazebo::physics::LinkPtr link = boost::dynamic_pointer_cast(model->GetChild(i)); if (link) { @@ -254,7 +254,7 @@ void GazeboRosPropertiesPrivate::GetJointProperties( gazebo_msgs::srv::GetJointProperties::Response::SharedPtr _res) { gazebo::physics::JointPtr joint; - for (unsigned int i = 0; i < world_->ModelCount(); i++) { + for (unsigned int i = 0; i < world_->ModelCount(); ++i) { joint = world_->ModelByIndex(i)->GetJoint(_req->joint_name); if (joint) { break; @@ -264,22 +264,35 @@ void GazeboRosPropertiesPrivate::GetJointProperties( if (!joint) { _res->success = false; _res->status_message = "GetJointProperties: joint not found"; - } else { - /// @todo: FIXME - _res->type = _res->REVOLUTE; - - _res->damping.clear(); - _res->damping.push_back(joint->GetDamping(0)); - - _res->position.clear(); - _res->position.push_back(joint->Position(0)); + return; + } - _res->rate.clear(); // use GetVelocity(i) - _res->rate.push_back(joint->GetVelocity(0)); + 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->success = true; - _res->status_message = "GetJointProperties: got properties"; + _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( @@ -293,7 +306,6 @@ void GazeboRosPropertiesPrivate::GetLinkProperties( _res->status_message = "GetLinkProperties: link not found, did you forget to scope the link by model name?"; } else { - /// @todo: validate _res->gravity_mode = link->GetGravityMode(); gazebo::physics::InertialPtr inertia = link->GetInertial(); @@ -350,7 +362,7 @@ void GazeboRosPropertiesPrivate::SetJointProperties( /// @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++) { + for (unsigned int i = 0; i < world_->ModelCount(); ++i) { joint = world_->ModelByIndex(i)->GetJoint(_req->joint_name); if (joint) {break;} } @@ -359,34 +371,34 @@ void GazeboRosPropertiesPrivate::SetJointProperties( _res->success = false; _res->status_message = "SetJointProperties: joint not found"; } else { - for (unsigned int i = 0; i < _req->ode_joint_config.damping.size(); i++) { + 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++) { + 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++) { + 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++) { + 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++) { + 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++) { + 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++) { + 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++) { + 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++) { + 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++) { + for (unsigned int i = 0; i < _req->ode_joint_config.vel.size(); ++i) { joint->SetParam("vel", i, _req->ode_joint_config.vel[i]); } diff --git a/gazebo_ros/test/test_gazebo_ros_properties.cpp b/gazebo_ros/test/test_gazebo_ros_properties.cpp index 330d7b3ca..edbaed92c 100644 --- a/gazebo_ros/test/test_gazebo_ros_properties.cpp +++ b/gazebo_ros/test/test_gazebo_ros_properties.cpp @@ -62,7 +62,7 @@ class GazeboRosPropertiesTest : public gazebo::ServerFixture void GetLightProperties( const std::string & _light_name, - const ignition::math::Vector4d & _diffuse, + const ignition::math::Color & _diffuse, const double & _attenuation_constant, const double & _attenuation_linear, const double & _attenuation_quadratic); @@ -84,7 +84,7 @@ class GazeboRosPropertiesTest : public gazebo::ServerFixture void SetLightProperties( const std::string & _light_name, - const ignition::math::Vector4d & _diffuse, + const ignition::math::Color & _diffuse, const double & _attenuation_constant, const double & _attenuation_linear, const double & _attenuation_quadratic); @@ -178,12 +178,14 @@ void GazeboRosPropertiesTest::GetModelProperties( EXPECT_EQ(response->parent_model_name, ""); EXPECT_EQ(response->canonical_link_name, "simple_arm::arm_base"); + ASSERT_EQ(5u, response->link_names.size()); EXPECT_EQ(response->link_names[0], "simple_arm::arm_base"); EXPECT_EQ(response->link_names[1], "simple_arm::arm_shoulder_pan"); EXPECT_EQ(response->link_names[2], "simple_arm::arm_elbow_pan"); EXPECT_EQ(response->link_names[3], "simple_arm::arm_wrist_lift"); EXPECT_EQ(response->link_names[4], "simple_arm::arm_wrist_roll"); + ASSERT_EQ(9u, response->collision_names.size()); EXPECT_EQ(response->collision_names[0], "arm_base_collision"); EXPECT_EQ(response->collision_names[1], "arm_base_collision_arm_trunk"); EXPECT_EQ(response->collision_names[2], "arm_shoulder_pan_collision"); @@ -194,12 +196,13 @@ void GazeboRosPropertiesTest::GetModelProperties( EXPECT_EQ(response->collision_names[7], "arm_wrist_lift_collision"); EXPECT_EQ(response->collision_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"); - std::vector v; // Empty - EXPECT_EQ(response->child_model_names, v); + EXPECT_TRUE(response->child_model_names.empty()); EXPECT_FALSE(response->is_static); } @@ -218,10 +221,20 @@ void GazeboRosPropertiesTest::GetJointProperties( ASSERT_NE(nullptr, response); EXPECT_TRUE(response->success); - std::vector v; - v.push_back(_damping); + 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(response->damping, v); + EXPECT_EQ(_damping, response->damping[0]); } } @@ -232,9 +245,7 @@ void GazeboRosPropertiesTest::SetJointProperties( auto request = std::make_shared(); request->joint_name = _joint_name; - std::vector v; - v.push_back(_damping); - request->ode_joint_config.damping = v; + 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, @@ -310,7 +321,7 @@ void GazeboRosPropertiesTest::SetLinkProperties( void GazeboRosPropertiesTest::GetLightProperties( const std::string & _light_name, - const ignition::math::Vector4d & _diffuse, + const ignition::math::Color & _diffuse, const double & _attenuation_constant, const double & _attenuation_linear, const double & _attenuation_quadratic) @@ -326,10 +337,10 @@ void GazeboRosPropertiesTest::GetLightProperties( ASSERT_NE(nullptr, response); EXPECT_TRUE(response->success); - EXPECT_NEAR(_diffuse.X(), response->diffuse.r, tol) << _light_name; - EXPECT_NEAR(_diffuse.Y(), response->diffuse.g, tol) << _light_name; - EXPECT_NEAR(_diffuse.Z(), response->diffuse.b, tol) << _light_name; - EXPECT_NEAR(_diffuse.W(), response->diffuse.a, tol) << _light_name; + 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; @@ -338,17 +349,17 @@ void GazeboRosPropertiesTest::GetLightProperties( void GazeboRosPropertiesTest::SetLightProperties( const std::string & _light_name, - const ignition::math::Vector4d & _diffuse, + 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.X(); - request->diffuse.g = _diffuse.Y(); - request->diffuse.b = _diffuse.Z(); - request->diffuse.a = _diffuse.W(); + 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; @@ -398,16 +409,16 @@ TEST_F(GazeboRosPropertiesTest, GetSetProperties) // Get / set light properties { // Get initial light properties - this->GetLightProperties("sun", ignition::math::Vector4d(0.800000011920929, 0.800000011920929, - 0.800000011920929, 1.0), 0.8999999761581421, 0.009999999776482582, 0.0010000000474974513); + 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::Vector4d(0.7, 0.1, 0.5, 1.0), + 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::Vector4d(0.7, 0.1, 0.5, 1.0), + this->GetLightProperties("sun", ignition::math::Color(0.7, 0.1, 0.5, 1.0), 0.92, 0.0092, 0.002); } } diff --git a/gazebo_ros/test/worlds/gazebo_ros_properties_test.world b/gazebo_ros/test/worlds/gazebo_ros_properties_test.world index 829dac7f7..d8001bf83 100644 --- a/gazebo_ros/test/worlds/gazebo_ros_properties_test.world +++ b/gazebo_ros/test/worlds/gazebo_ros_properties_test.world @@ -557,7 +557,7 @@ 1 - + arm_wrist_lift arm_wrist_roll @@ -574,6 +574,20 @@ 0 0 1 1 + + + 2 + 0 + 0 + 0 + + + -2.99999 + 2.99999 + + 0 1 0 + 1 + 0.033491 0.033439 0 0 -0 0 diff --git a/gazebo_ros/worlds/gazebo_ros_properties_demo.world b/gazebo_ros/worlds/gazebo_ros_properties_demo.world index 54cbb587d..714874150 100644 --- a/gazebo_ros/worlds/gazebo_ros_properties_demo.world +++ b/gazebo_ros/worlds/gazebo_ros_properties_demo.world @@ -27,7 +27,7 @@ Or a links properties: ros2 service call /demo/set_link_properties 'gazebo_msgs/SetLinkProperties' '{link_name: "simple_arm::arm_base", mass: 150.0 , izz: 2.0}' - + Or a lights properties: ros2 service call /demo/set_light_properties 'gazebo_msgs/SetLightProperties' '{light_name: "sun", diffuse: {r: 0.8, g: 0.7, b: 0.2, a: 1.0}, attenuation_constant: 0.9, attenuation_linear: 0.01, attenuation_quadratic: 0.001}' @@ -100,27 +100,6 @@ 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 - @@ -673,4 +652,4 @@ - \ No newline at end of file + From aac7178559b886a1558d743b164c3421f3fba90b Mon Sep 17 00:00:00 2001 From: Shivesh Khaitan Date: Tue, 20 Aug 2019 14:37:12 +0530 Subject: [PATCH 10/11] Revert changes to GetModelProperties message Document gazebo_ros_properties header --- gazebo_msgs/srv/GetModelProperties.srv | 6 +-- .../gazebo_ros/gazebo_ros_properties.hpp | 29 ++++++++++++++ gazebo_ros/src/gazebo_ros_properties.cpp | 10 ++--- .../test/test_gazebo_ros_properties.cpp | 38 +++++++++---------- 4 files changed, 56 insertions(+), 27 deletions(-) diff --git a/gazebo_msgs/srv/GetModelProperties.srv b/gazebo_msgs/srv/GetModelProperties.srv index 8d33b754c..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_link_name # name of canonical link, link names are prefixed by model name, e.g. pr2::base_link -string[] link_names # list of links, link names are prefixed by model name, e.g. pr2::base_link -string[] collision_names # list of collisions +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_ros/include/gazebo_ros/gazebo_ros_properties.hpp b/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp index 475a7d499..e7ff0d145 100644 --- a/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp +++ b/gazebo_ros/include/gazebo_ros/gazebo_ros_properties.hpp @@ -24,6 +24,35 @@ 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: diff --git a/gazebo_ros/src/gazebo_ros_properties.cpp b/gazebo_ros/src/gazebo_ros_properties.cpp index ad3994c2e..bd5e1b206 100644 --- a/gazebo_ros/src/gazebo_ros_properties.cpp +++ b/gazebo_ros/src/gazebo_ros_properties.cpp @@ -203,22 +203,22 @@ void GazeboRosPropertiesPrivate::GetModelProperties( if (parent_model) {_res->parent_model_name = parent_model->GetName();} // get list of child links, collisions - _res->link_names.clear(); - _res->collision_names.clear(); + _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_link_name = link->GetScopedName(); + _res->canonical_body_name = link->GetScopedName(); } - _res->link_names.push_back(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->collision_names.push_back(collision->GetName()); + _res->geom_names.push_back(collision->GetName()); } } } diff --git a/gazebo_ros/test/test_gazebo_ros_properties.cpp b/gazebo_ros/test/test_gazebo_ros_properties.cpp index edbaed92c..5316ebb48 100644 --- a/gazebo_ros/test/test_gazebo_ros_properties.cpp +++ b/gazebo_ros/test/test_gazebo_ros_properties.cpp @@ -176,25 +176,25 @@ void GazeboRosPropertiesTest::GetModelProperties( // gazebo models simple_arm EXPECT_EQ(response->parent_model_name, ""); - EXPECT_EQ(response->canonical_link_name, "simple_arm::arm_base"); - - ASSERT_EQ(5u, response->link_names.size()); - EXPECT_EQ(response->link_names[0], "simple_arm::arm_base"); - EXPECT_EQ(response->link_names[1], "simple_arm::arm_shoulder_pan"); - EXPECT_EQ(response->link_names[2], "simple_arm::arm_elbow_pan"); - EXPECT_EQ(response->link_names[3], "simple_arm::arm_wrist_lift"); - EXPECT_EQ(response->link_names[4], "simple_arm::arm_wrist_roll"); - - ASSERT_EQ(9u, response->collision_names.size()); - EXPECT_EQ(response->collision_names[0], "arm_base_collision"); - EXPECT_EQ(response->collision_names[1], "arm_base_collision_arm_trunk"); - EXPECT_EQ(response->collision_names[2], "arm_shoulder_pan_collision"); - EXPECT_EQ(response->collision_names[3], "arm_shoulder_pan_collision_arm_shoulder"); - EXPECT_EQ(response->collision_names[4], "arm_elbow_pan_collision"); - EXPECT_EQ(response->collision_names[5], "arm_elbow_pan_collision_arm_elbow"); - EXPECT_EQ(response->collision_names[6], "arm_elbow_pan_collision_arm_wrist"); - EXPECT_EQ(response->collision_names[7], "arm_wrist_lift_collision"); - EXPECT_EQ(response->collision_names[8], "arm_wrist_roll_collision"); + 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"); From f0e9796585665ad7c5b41f53af1f2d0db9333006 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 20 Aug 2019 18:38:30 -0700 Subject: [PATCH 11/11] Convert msgs pose to math pose and use it on SetCoG Signed-off-by: Louise Poubel --- .../gazebo_ros/conversions/geometry_msgs.hpp | 54 +++++++++++-------- gazebo_ros/src/gazebo_ros_properties.cpp | 8 +-- gazebo_ros/test/test_conversions.cpp | 10 ++++ 3 files changed, 43 insertions(+), 29 deletions(-) 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/src/gazebo_ros_properties.cpp b/gazebo_ros/src/gazebo_ros_properties.cpp index bd5e1b206..7876a29ff 100644 --- a/gazebo_ros/src/gazebo_ros_properties.cpp +++ b/gazebo_ros/src/gazebo_ros_properties.cpp @@ -419,13 +419,7 @@ void GazeboRosPropertiesPrivate::SetLinkProperties( "SetLinkProperties: link not found, did you forget to scope the link by model name?"; } else { gazebo::physics::InertialPtr mass = link->GetInertial(); - // @todo: FIXME: add inertia matrix rotation to Gazebo - // mass.SetInertiaRotation(ignition::math::Quaternion(_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->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); 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);