00001 /* 00002 * Copyright (c) 2011, A.M.Howard, S.Williams 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 * * Redistributions of source code must retain the above copyright 00008 * notice, this list of conditions and the following disclaimer. 00009 * * Redistributions in binary form must reproduce the above copyright 00010 * notice, this list of conditions and the following disclaimer in the 00011 * documentation and/or other materials provided with the distribution. 00012 * * Neither the name of the <organization> nor the 00013 * names of its contributors may be used to endorse or promote products 00014 * derived from this software without specific prior written permission. 00015 * 00016 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00017 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00018 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00019 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY 00020 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00021 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00022 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00023 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00024 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00025 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00026 */ 00027 /* 00028 * gazebo_ros_actuator_array.h 00029 * 00030 * Created on: Feb 6, 2011 00031 * Author: Stephen Williams 00032 */ 00033 00034 #ifndef GAZEBO_ROS_ACTUATORARRAY_HH 00035 #define GAZEBO_ROS_ACTUATORARRAY_HH 00036 00037 #include <vector> 00038 00039 #include <gazebo/Param.hh> 00040 #include <gazebo/Controller.hh> 00041 #include <gazebo/Entity.hh> 00042 #include <gazebo/Model.hh> 00043 #include <gazebo/Body.hh> 00044 #include <gazebo/Joint.hh> 00045 00046 #include "boost/thread/mutex.hpp" 00047 #include <ros/ros.h> 00048 #include <control_toolbox/pid.h> 00049 00050 #define USE_CBQ 00051 #ifdef USE_CBQ 00052 #include <ros/callback_queue.h> 00053 #include <ros/advertise_options.h> 00054 #include <ros/advertise_service_options.h> 00055 #include <ros/subscribe_options.h> 00056 #endif 00057 00058 #include <actuator_array_driver/actuator_array_driver.h> 00059 00060 00061 namespace gazebo 00062 { 00065 00175 struct GazeboJointProperties : public actuator_array_driver::JointProperties 00176 { 00177 int joint_index; 00178 gazebo::Joint* joint_ptr; 00179 control_toolbox::Pid pid; 00180 double position; 00181 double home_position; 00182 }; 00183 00184 struct MimicJointProperties : GazeboJointProperties 00185 { 00186 std::string master_joint_name; 00187 unsigned int master_joint_index; 00188 double multiplier; 00189 double offset; 00190 }; 00191 00192 class GazeboRosActuatorArray : public Controller, public actuator_array_driver::ActuatorArrayDriver<GazeboJointProperties> 00193 { 00195 public: 00196 GazeboRosActuatorArray(Entity *parent); 00197 virtual ~GazeboRosActuatorArray(); 00198 00199 protected: 00200 // Inherited from gazebo::Controller 00201 virtual void LoadChild(XMLConfigNode *node); 00202 virtual void InitChild(); 00203 virtual void UpdateChild(); 00204 virtual void FiniChild(); 00205 00206 private: 00208 ParamT<std::string> *robotNamespaceP; 00209 std::string robot_namespace_; 00210 00212 ParamT<std::string> *robotParamP; 00213 00215 std::map<std::string, MimicJointProperties> mimic_joints_; 00216 00218 Model *myParent; 00219 00221 gazebo::Time last_time_; 00222 00224 boost::mutex lock_; 00225 00226 #ifdef USE_CBQ 00227 private: ros::CallbackQueue queue_; 00228 private: void QueueThread(); 00229 private: boost::thread callback_queue_thread_; 00230 #endif 00231 00232 00233 void parse_mimic_joints(const ros::NodeHandle& node); 00234 void update_joint(GazeboJointProperties& gazebo_joint, double command_position, double command_velocity, double command_effort, double dt); 00235 00236 00240 bool command_(); 00241 00244 bool stop_(); 00245 00248 bool home_(); 00249 00252 bool read_(ros::Time ts = ros::Time::now()); 00253 00254 }; 00255 00257 00258 00259 00260 } 00261 00262 #endif 00263