00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #ifndef GAZEBO_CONTROLLER_MANAGER_H 00031 #define GAZEBO_CONTROLLER_MANAGER_H 00032 00033 #include <vector> 00034 00035 #include <gazebo/physics/World.hh> 00036 #include <gazebo/physics/Model.hh> 00037 #include <gazebo/physics/physics.hh> 00038 #include <gazebo/common/Time.hh> 00039 #include <gazebo/common/Plugin.hh> 00040 00041 #include "ros_ethercat_model/hardware_interface.hpp" 00042 #include "ros_ethercat_model/ros_ethercat.hpp" 00043 #include "controller_manager/controller_manager.h" 00044 #include "ros_ethercat_model/robot_state.hpp" 00045 #include <tinyxml.h> 00046 #include <ros/ros.h> 00047 #undef USE_CBQ 00048 #ifdef USE_CBQ 00049 #include <ros/callback_queue.h> 00050 #endif 00051 00052 #include "boost/thread/mutex.hpp" 00053 00054 #include <sr_self_test/sr_self_test.hpp> 00055 00056 namespace gazebo 00057 { 00058 class GazeboRosControllerManager : public ModelPlugin 00059 { 00060 public: 00061 GazeboRosControllerManager(); 00062 virtual ~GazeboRosControllerManager(); 00063 void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); 00064 00065 protected: 00066 // Inherited from gazebo::Controller 00067 virtual void UpdateChild(); 00068 00069 virtual void ResetChild(); 00070 00071 boost::shared_ptr<shadow_robot::SrSelfTest> self_test_; 00072 private: 00073 00074 gazebo::physics::ModelPtr parent_model_; 00075 RosEthercat *state_; 00076 controller_manager::ControllerManager *cm_; 00077 00081 ros_ethercat_model::RobotState *fake_state_; 00082 std::vector<gazebo::physics::JointPtr> joints_; 00083 00084 /* 00085 * \brief read pr2.xml for actuators, and pass tinyxml node to mechanism control node's initXml. 00086 */ 00087 void ReadPr2Xml(); 00088 00089 /* 00090 * \brief pointer to ros node 00091 */ 00092 ros::NodeHandle* rosnode_; 00093 00095 /* 00096 * \brief tmp vars for performance checking 00097 */ 00098 double wall_start_, sim_start_; 00099 00101 std::string robotParam; 00102 std::string robotNamespace; 00103 00104 bool fake_calibration_; 00105 00106 00107 #ifdef USE_CBQ 00108 private: ros::CallbackQueue controller_manager_queue_; 00109 private: void ControllerManagerQueueThread(); 00110 private: boost::thread controller_manager_callback_queue_thread_; 00111 #endif 00112 void ControllerManagerROSThread(); 00113 boost::thread ros_spinner_thread_; 00114 bool stop_; 00115 00116 // Pointer to the model 00117 physics::WorldPtr world; 00118 00119 // Pointer to the update event connection 00120 event::ConnectionPtr updateConnection; 00121 00122 // subscribe to world stats 00123 transport::NodePtr node; 00124 transport::SubscriberPtr statsSub; 00125 common::Time simTime; 00126 00127 // Timing 00128 ros::Duration control_period_; 00129 ros::Time last_update_sim_time_ros_; 00130 ros::Time last_write_sim_time_ros_; 00131 }; 00132 00134 00135 00136 } 00137 00138 #endif 00139