gazebo_ros_api_plugin.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012-2014 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
/*
 * Desc: External interfaces for Gazebo
 * Author: Nate Koenig, John Hsu, Dave Coleman
 * Date: 25 Apr 2010
 */

#ifndef __GAZEBO_ROS_API_PLUGIN_HH__
#define __GAZEBO_ROS_API_PLUGIN_HH__

#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <errno.h>
#include <iostream>

#include <tinyxml.h>

#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/transport/transport.hh>

// ROS
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/subscribe_options.h>
#include <ros/package.h>
#include <rosgraph_msgs/Clock.h>

// Services
#include "std_srvs/Empty.h"

#include "gazebo_msgs/JointRequest.h"
#include "gazebo_msgs/BodyRequest.h"

#include "gazebo_msgs/SpawnModel.h"
#include "gazebo_msgs/DeleteModel.h"
#include "gazebo_msgs/DeleteLight.h"

#include "gazebo_msgs/ApplyBodyWrench.h"

#include "gazebo_msgs/SetPhysicsProperties.h"
#include "gazebo_msgs/GetPhysicsProperties.h"

#include "gazebo_msgs/SetJointProperties.h"

#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"
#include "gazebo_msgs/PerformanceMetrics.h"

#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Wrench.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Twist.h"

// For model pose transform to set custom joint angles
#include <ros/ros.h>
#include <gazebo_msgs/SetModelConfiguration.h>
#include <boost/shared_ptr.hpp>

// For physics dynamics reconfigure
#include <dynamic_reconfigure/server.h>
#include <gazebo_ros/PhysicsConfig.h>
#include "gazebo_msgs/SetPhysicsProperties.h"
#include "gazebo_msgs/GetPhysicsProperties.h"

#include <boost/algorithm/string.hpp>

#ifndef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
#if (GAZEBO_MAJOR_VERSION == 11 && GAZEBO_MINOR_VERSION > 1) || \
    (GAZEBO_MAJOR_VERSION == 9 && GAZEBO_MINOR_VERSION > 14)
#define GAZEBO_ROS_HAS_PERFORMANCE_METRICS
#endif
#endif  // ifndef GAZEBO_ROS_HAS_PERFORMANCE_METRICS

namespace gazebo
{

class GazeboRosApiPlugin : public SystemPlugin
{
public:
  GazeboRosApiPlugin();

  ~GazeboRosApiPlugin();

  void shutdownSignal();

  void Load(int argc, char** argv);

  void gazeboQueueThread();

  void advertiseServices();

  void onLinkStatesConnect();

  void onModelStatesConnect();

  void onLinkStatesDisconnect();

  void onModelStatesDisconnect();

#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
  void onPerformanceMetricsConnect();

  void onPerformanceMetricsDisconnect();
#endif

  bool spawnURDFModel(gazebo_msgs::SpawnModel::Request &req,
                      gazebo_msgs::SpawnModel::Response &res);

  bool spawnSDFModel(gazebo_msgs::SpawnModel::Request &req,
                     gazebo_msgs::SpawnModel::Response &res);

  bool deleteModel(gazebo_msgs::DeleteModel::Request &req,gazebo_msgs::DeleteModel::Response &res);

  bool deleteLight(gazebo_msgs::DeleteLight::Request &req,gazebo_msgs::DeleteLight::Response &res);

  bool getModelState(gazebo_msgs::GetModelState::Request &req,gazebo_msgs::GetModelState::Response &res);

  bool getModelProperties(gazebo_msgs::GetModelProperties::Request &req,gazebo_msgs::GetModelProperties::Response &res);

  bool getWorldProperties(gazebo_msgs::GetWorldProperties::Request &req,gazebo_msgs::GetWorldProperties::Response &res);

  bool getJointProperties(gazebo_msgs::GetJointProperties::Request &req,gazebo_msgs::GetJointProperties::Response &res);

  bool getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req,gazebo_msgs::GetLinkProperties::Response &res);

  bool getLinkState(gazebo_msgs::GetLinkState::Request &req,gazebo_msgs::GetLinkState::Response &res);

  bool getLightProperties(gazebo_msgs::GetLightProperties::Request &req,gazebo_msgs::GetLightProperties::Response &res);

  bool setLightProperties(gazebo_msgs::SetLightProperties::Request &req,gazebo_msgs::SetLightProperties::Response &res);

  bool setLinkProperties(gazebo_msgs::SetLinkProperties::Request &req,gazebo_msgs::SetLinkProperties::Response &res);

  bool setPhysicsProperties(gazebo_msgs::SetPhysicsProperties::Request &req,gazebo_msgs::SetPhysicsProperties::Response &res);

  bool getPhysicsProperties(gazebo_msgs::GetPhysicsProperties::Request &req,gazebo_msgs::GetPhysicsProperties::Response &res);

  bool setJointProperties(gazebo_msgs::SetJointProperties::Request &req,gazebo_msgs::SetJointProperties::Response &res);

  bool setModelState(gazebo_msgs::SetModelState::Request &req,gazebo_msgs::SetModelState::Response &res);

  void updateModelState(const gazebo_msgs::ModelState::ConstPtr& model_state);

  bool applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req,gazebo_msgs::ApplyJointEffort::Response &res);

  bool resetSimulation(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);

  bool resetWorld(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);

  bool pausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);

  bool unpausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);

  bool clearJointForces(gazebo_msgs::JointRequest::Request &req,gazebo_msgs::JointRequest::Response &res);
  bool clearJointForces(std::string joint_name);

  bool clearBodyWrenches(gazebo_msgs::BodyRequest::Request &req,gazebo_msgs::BodyRequest::Response &res);
  bool clearBodyWrenches(std::string body_name);

  bool setModelConfiguration(gazebo_msgs::SetModelConfiguration::Request &req,gazebo_msgs::SetModelConfiguration::Response &res);

  bool setLinkState(gazebo_msgs::SetLinkState::Request &req,gazebo_msgs::SetLinkState::Response &res);

  void updateLinkState(const gazebo_msgs::LinkState::ConstPtr& link_state);

  bool applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &req,gazebo_msgs::ApplyBodyWrench::Response &res);

private:

  void wrenchBodySchedulerSlot();

  void forceJointSchedulerSlot();

  void publishSimTime(const boost::shared_ptr<gazebo::msgs::WorldStatistics const> &msg);
  void publishSimTime();

  void publishLinkStates();

  void publishModelStates();

  void stripXmlDeclaration(std::string &model_xml);

  void updateSDFAttributes(TiXmlDocument &gazebo_model_xml,
                           const std::string &model_name,
                           const ignition::math::Vector3d &initial_xyz,
                           const ignition::math::Quaterniond &initial_q);

  void updateURDFModelPose(TiXmlDocument &gazebo_model_xml,
                           const ignition::math::Vector3d &initial_xyz,
                           const ignition::math::Quaterniond &initial_q);

  void updateURDFName(TiXmlDocument &gazebo_model_xml, const std::string &model_name);

  void walkChildAddRobotNamespace(TiXmlNode* model_xml);

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

  void transformWrench(ignition::math::Vector3d &target_force, ignition::math::Vector3d &target_torque,
                       const ignition::math::Vector3d &reference_force,
                       const ignition::math::Vector3d &reference_torque,
                       const ignition::math::Pose3d &target_to_reference );

  void physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level);

  void physicsReconfigureThread();

  void onResponse(ConstResponsePtr &response);

#ifdef GAZEBO_ROS_HAS_PERFORMANCE_METRICS
  void onPerformanceMetrics(const boost::shared_ptr<gazebo::msgs::PerformanceMetrics const> &msg);
#endif
  bool isURDF(std::string model_xml);

  bool isSDF(std::string model_xml);

  void loadGazeboRosApiPlugin(std::string world_name);

  ignition::math::Pose3d parsePose(const std::string &str);

  ignition::math::Vector3d parseVector3(const std::string &str);

  // track if the desconstructor event needs to occur
  bool plugin_loaded_;

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

  std::string robot_namespace_;

  gazebo::transport::NodePtr gazebonode_;
  gazebo::transport::SubscriberPtr stat_sub_;
  gazebo::transport::PublisherPtr factory_pub_;
  gazebo::transport::PublisherPtr factory_light_pub_;
  gazebo::transport::PublisherPtr light_modify_pub_;
  gazebo::transport::SubscriberPtr performance_metric_sub_;
  gazebo::transport::PublisherPtr request_pub_;
  gazebo::transport::SubscriberPtr response_sub_;

  boost::shared_ptr<ros::NodeHandle> nh_;
  ros::CallbackQueue gazebo_queue_;
  boost::shared_ptr<boost::thread> gazebo_callback_queue_thread_;

  gazebo::physics::WorldPtr world_;
  gazebo::event::ConnectionPtr wrench_update_event_;
  gazebo::event::ConnectionPtr force_update_event_;
  gazebo::event::ConnectionPtr time_update_event_;
  gazebo::event::ConnectionPtr pub_link_states_event_;
  gazebo::event::ConnectionPtr pub_model_states_event_;
  gazebo::event::ConnectionPtr load_gazebo_ros_api_plugin_event_;

  ros::ServiceServer spawn_sdf_model_service_;
  ros::ServiceServer spawn_urdf_model_service_;
  ros::ServiceServer delete_model_service_;
  ros::ServiceServer delete_light_service_;
  ros::ServiceServer get_model_state_service_;
  ros::ServiceServer get_model_properties_service_;
  ros::ServiceServer get_world_properties_service_;
  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_;
  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 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_;
  ros::Publisher     pub_performance_metrics_;
  int                pub_link_states_connection_count_;
  int                pub_model_states_connection_count_;
  int                pub_performance_metrics_connection_count_;

  // ROS comm
  boost::shared_ptr<ros::AsyncSpinner> async_ros_spin_;

  // physics dynamic reconfigure
  boost::shared_ptr<boost::thread> physics_reconfigure_thread_;
  bool physics_reconfigure_initialized_;
  ros::ServiceClient physics_reconfigure_set_client_;
  ros::ServiceClient physics_reconfigure_get_client_;
  boost::shared_ptr< dynamic_reconfigure::Server<gazebo_ros::PhysicsConfig> > physics_reconfigure_srv_;
  dynamic_reconfigure::Server<gazebo_ros::PhysicsConfig>::CallbackType physics_reconfigure_callback_;

  ros::Publisher     pub_clock_;
  int pub_clock_frequency_;
  gazebo::common::Time last_pub_clock_time_;

  boost::mutex lock_;

  bool world_created_;

  class WrenchBodyJob
  {
  public:
    gazebo::physics::LinkPtr body;
    ignition::math::Vector3d force;
    ignition::math::Vector3d torque;
    ros::Time start_time;
    ros::Duration duration;
  };

  class ForceJointJob
  {
  public:
    gazebo::physics::JointPtr joint;
    double force; // should this be a array?
    ros::Time start_time;
    ros::Duration duration;
  };

  std::vector<GazeboRosApiPlugin::WrenchBodyJob*> wrench_body_jobs_;
  std::vector<GazeboRosApiPlugin::ForceJointJob*> force_joint_jobs_;

  std::map<std::string, unsigned int> access_count_get_model_state_;
};
}
#endif