gazebo_ros_actuator_array.h

Go to the documentation of this file.

/*
 *  Copyright (c) 2011, A.M.Howard, S.Williams
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *      * Redistributions of source code must retain the above copyright
 *        notice, this list of conditions and the following disclaimer.
 *      * Redistributions in binary form must reproduce the above copyright
 *        notice, this list of conditions and the following disclaimer in the
 *        documentation and/or other materials provided with the distribution.
 *      * Neither the name of the <organization> nor the
 *        names of its contributors may be used to endorse or promote products
 *        derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 *  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 *  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 *  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 *  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 *  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 *  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
/*
 * gazebo_ros_actuator_array.h
 *
 *  Created on: Feb 6, 2011
 *      Author: Stephen Williams
 */

#ifndef GAZEBO_ROS_ACTUATORARRAY_HH
#define GAZEBO_ROS_ACTUATORARRAY_HH

#include <vector>

#include <gazebo/Param.hh>
#include <gazebo/Controller.hh>
#include <gazebo/Entity.hh>
#include <gazebo/Model.hh>
#include <gazebo/Body.hh>
#include <gazebo/Joint.hh>

#include "boost/thread/mutex.hpp"
#include <ros/ros.h>
#include <control_toolbox/pid.h>

#define USE_CBQ
#ifdef USE_CBQ
#include <ros/callback_queue.h>
#include <ros/advertise_options.h>
#include <ros/advertise_service_options.h>
#include <ros/subscribe_options.h>
#endif

#include <actuator_array_driver/actuator_array_driver.h>


namespace gazebo
{

struct GazeboJointProperties : public actuator_array_driver::JointProperties
{
  int joint_index;
  gazebo::Joint* joint_ptr;
  control_toolbox::Pid pid;
  double position;
  double home_position;
};

struct MimicJointProperties : GazeboJointProperties
{
  std::string master_joint_name;
  unsigned int master_joint_index;
  double multiplier;
  double offset;
};

class GazeboRosActuatorArray : public Controller, public actuator_array_driver::ActuatorArrayDriver<GazeboJointProperties>
{
public:
  GazeboRosActuatorArray(Entity *parent);
  virtual ~GazeboRosActuatorArray();

protected:
  // Inherited from gazebo::Controller
  virtual void LoadChild(XMLConfigNode *node);
  virtual void InitChild();
  virtual void UpdateChild();
  virtual void FiniChild();

private:
  ParamT<std::string> *robotNamespaceP;
  std::string robot_namespace_;

  ParamT<std::string> *robotParamP;

  std::map<std::string, MimicJointProperties>  mimic_joints_;

  Model *myParent;

  gazebo::Time last_time_;

  boost::mutex lock_;

#ifdef USE_CBQ
  private: ros::CallbackQueue queue_;
  private: void QueueThread();
  private: boost::thread callback_queue_thread_;
#endif


  void parse_mimic_joints(const ros::NodeHandle& node);
  void update_joint(GazeboJointProperties& gazebo_joint, double command_position, double command_velocity, double command_effort, double dt);


  bool command_();

  bool stop_();

  bool home_();

  bool read_(ros::Time ts = ros::Time::now());

};




}

#endif