Skip to content

Commit

Permalink
Revert "fixed conflicts" (#600)
Browse files Browse the repository at this point in the history
This reverts commit 3f12a2e.
  • Loading branch information
j-rivero committed Jul 19, 2017
1 parent 1efd62c commit acbde94
Show file tree
Hide file tree
Showing 3 changed files with 18 additions and 36 deletions.
28 changes: 14 additions & 14 deletions gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h
Expand Up @@ -108,13 +108,13 @@ class GazeboRosApiPlugin : public SystemPlugin

/// \brief Destructor
~GazeboRosApiPlugin();

/// \bried Detect if sig-int shutdown signal is recieved
void shutdownSignal();

/// \brief Gazebo-inherited load function
///
/// Called before Gazebo is loaded. Must not block.
///
/// Called before Gazebo is loaded. Must not block.
/// Capitalized per Gazebo cpp style guidelines
/// \param _argc Number of command line arguments.
/// \param _argv Array of command line arguments.
Expand All @@ -139,12 +139,13 @@ class GazeboRosApiPlugin : public SystemPlugin
void onModelStatesDisconnect();

/// \brief Function for inserting a URDF into Gazebo from ROS Service Call
bool spawnURDFModel(gazebo_msgs::SpawnModel::Request &req,
gazebo_msgs::SpawnModel::Response &res);
bool spawnURDFModel(gazebo_msgs::SpawnModel::Request &req,gazebo_msgs::SpawnModel::Response &res);

/// \brief Function for inserting a URDF into Gazebo from ROS Service Call. Deprecated in ROS Hydro - replace with spawnURDFModel()
ROS_DEPRECATED bool spawnGazeboModel(gazebo_msgs::SpawnModel::Request &req,gazebo_msgs::SpawnModel::Response &res);

/// \brief Both SDFs and converted URDFs get sent to this function for further manipulation from a ROS Service call
bool spawnSDFModel(gazebo_msgs::SpawnModel::Request &req,
gazebo_msgs::SpawnModel::Response &res);
bool spawnSDFModel(gazebo_msgs::SpawnModel::Request &req,gazebo_msgs::SpawnModel::Response &res);

/// \brief delete model given name
bool deleteModel(gazebo_msgs::DeleteModel::Request &req,gazebo_msgs::DeleteModel::Response &res);
Expand Down Expand Up @@ -242,21 +243,21 @@ class GazeboRosApiPlugin : public SystemPlugin
void stripXmlDeclaration(std::string &model_xml);

/// \brief Update the model name and pose of the SDF file before sending to Gazebo
void updateSDFAttributes(TiXmlDocument &gazebo_model_xml, std::string model_name,
void updateSDFAttributes(TiXmlDocument &gazebo_model_xml, std::string model_name,
gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q);

/// \brief Update the model pose of the URDF file before sending to Gazebo
void updateURDFModelPose(TiXmlDocument &gazebo_model_xml,
void updateURDFModelPose(TiXmlDocument &gazebo_model_xml,
gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q);

/// \brief Update the model name of the URDF file before sending to Gazebo
void updateURDFName(TiXmlDocument &gazebo_model_xml, std::string model_name);

/// \brief
void walkChildAddRobotNamespace(TiXmlNode* model_xml);
void walkChildAddRobotNamespace(TiXmlNode* robot_xml);

/// \brief
bool spawnAndConform(TiXmlDocument &gazebo_model_xml, std::string model_name,
bool spawnAndConform(TiXmlDocument &gazebo_model_xml, std::string model_name,
gazebo_msgs::SpawnModel::Response &res);

/// \brief helper function for applyBodyWrench
Expand Down Expand Up @@ -293,7 +294,7 @@ class GazeboRosApiPlugin : public SystemPlugin
bool plugin_loaded_;

// detect if sigint event occurs
bool stop_;
bool stop_;
gazebo::event::ConnectionPtr sigint_event_;

std::string robot_namespace_;
Expand All @@ -316,6 +317,7 @@ class GazeboRosApiPlugin : public SystemPlugin
gazebo::event::ConnectionPtr pub_model_states_event_;
gazebo::event::ConnectionPtr load_gazebo_ros_api_plugin_event_;

ros::ServiceServer spawn_gazebo_model_service_; // DEPRECATED IN HYDRO
ros::ServiceServer spawn_sdf_model_service_;
ros::ServiceServer spawn_urdf_model_service_;
ros::ServiceServer delete_model_service_;
Expand Down Expand Up @@ -387,8 +389,6 @@ class GazeboRosApiPlugin : public SystemPlugin
std::vector<GazeboRosApiPlugin::WrenchBodyJob*> wrench_body_jobs_;
std::vector<GazeboRosApiPlugin::ForceJointJob*> force_joint_jobs_;

/// \brief enable the communication of gazebo information using ROS service/topics
bool enable_ros_network_;
};
}
#endif
4 changes: 0 additions & 4 deletions gazebo_ros/launch/empty_world.launch
Expand Up @@ -10,7 +10,6 @@
<arg name="physics" default="ode"/>
<arg name="verbose" default="false"/>
<arg name="world_name" default="worlds/empty.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
<arg name="enable_ros_network" default="true" />

<!-- set use_sim_time flag -->
<group if="$(arg use_sim_time)">
Expand All @@ -28,9 +27,6 @@
<arg if="$(arg debug)" name="script_type" value="debug"/>

<!-- start gazebo server-->
<group>
<param name="gazebo/enable_ros_network" value="$(arg enable_ros_network)" />
</group>
<node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="false" output="screen"
args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) -e $(arg physics) $(arg extra_gazebo_args) $(arg world_name)" />

Expand Down
22 changes: 4 additions & 18 deletions gazebo_ros/src/gazebo_ros_api_plugin.cpp
Expand Up @@ -33,8 +33,7 @@ GazeboRosApiPlugin::GazeboRosApiPlugin() :
stop_(false),
plugin_loaded_(false),
pub_link_states_connection_count_(0),
pub_model_states_connection_count_(0),
enable_ros_network_
pub_model_states_connection_count_(0)
{
robot_namespace_.clear();
}
Expand All @@ -58,8 +57,7 @@ GazeboRosApiPlugin::~GazeboRosApiPlugin()
gazebo::event::Events::DisconnectWorldCreated(load_gazebo_ros_api_plugin_event_);
gazebo::event::Events::DisconnectWorldUpdateBegin(wrench_update_event_);
gazebo::event::Events::DisconnectWorldUpdateBegin(force_update_event_);
if (enable_ros_network_)
gazebo::event::Events::DisconnectWorldUpdateBegin(time_update_event_);
gazebo::event::Events::DisconnectWorldUpdateBegin(time_update_event_);
ROS_DEBUG_STREAM_NAMED("api_plugin","Slots disconnected");

if (pub_link_states_connection_count_ > 0) // disconnect if there are subscribers on exit
Expand Down Expand Up @@ -153,9 +151,6 @@ void GazeboRosApiPlugin::Load(int argc, char** argv)
// below needs the world to be created first
load_gazebo_ros_api_plugin_event_ = gazebo::event::Events::ConnectWorldCreated(boost::bind(&GazeboRosApiPlugin::loadGazeboRosApiPlugin,this,_1));

nh_->getParam("enable_ros_network", enable_ros_network_);


plugin_loaded_ = true;
ROS_INFO("Finished loading Gazebo ROS API Plugin.");
}
Expand Down Expand Up @@ -195,15 +190,12 @@ void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name)
pub_model_states_connection_count_ = 0;

/// \brief advertise all services
if (enable_ros_network_)
advertiseServices();
advertiseServices();

// hooks for applying forces, publishing simtime on /clock
wrench_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::wrenchBodySchedulerSlot,this));
force_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::forceJointSchedulerSlot,this));

if (enable_ros_network_)
time_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishSimTime,this));
time_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishSimTime,this));
}

void GazeboRosApiPlugin::onResponse(ConstResponsePtr &response)
Expand All @@ -222,12 +214,6 @@ void GazeboRosApiPlugin::gazeboQueueThread()

void GazeboRosApiPlugin::advertiseServices()
{
if (! enable_ros_network_)
{
ROS_INFO_NAMED("api_plugin", "ROS gazebo topics/services are disabled");
return;
}

// publish clock for simulated ros time
pub_clock_ = nh_->advertise<rosgraph_msgs::Clock>("/clock",10);

Expand Down

0 comments on commit acbde94

Please sign in to comment.