actuator_array_driver.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.
 */
/*
 * actuator_array_driver.h
 *
 *  Created on: Feb 24, 2011
 *      Author: swilliams8
 */

#ifndef ACTUATOR_ARRAY_DRIVER_H_
#define ACTUATOR_ARRAY_DRIVER_H_

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_srvs/Empty.h>
#include <urdf/model.h>

namespace actuator_array_driver
{

/*
 * A structure that holds standard information about each joint. If additional information is needed, a derived structure
 * can be provided to the ActuatorArrayDriver base class.
 */
struct JointProperties
{
  bool has_position_limits;
  double min_position;
  double max_position;
  bool has_velocity_limits;
  double max_velocity;
  bool has_acceleration_limits;
  double max_acceleration;
  bool has_effort_limits;
  double max_effort;
  double friction;
  double damping;
};

/*
 * This package contains a base class for an Actuator Array Driver. This is intended for use with chains of R/C Servos or other
 * similar devices where position (and velocity) commands are sent at irregular intervals, as opposed to the tight, real-time
 * control loop of the PR2 Controller Manager system.
 *
 * This base class performs some standard functionality, such as parsing joint limits out of the robot_description, subscribing to
 * a 'command' topic, publishing on the 'joint_states' topic, and setting up a 'stop' and 'home' service call. This is designed to
 * work as a stand-alone driver for controlling/tele-operating a chain of servos, or in conjunction with the Actuatory Array
 * Joint Trajectory Action. This class is provided as a convenience only, and is not required for successful operation with the
 * Actuator Array Joint Trajectory Action.
 *
 * The base class is templated on a JOINT structure. If no additional data is needed, the provided JointProperties can be used
 * as the template argument. If additional information needs to be stored on a per-joint basis, then the JointProperties class
 * can be used as a base for a custom JOINT structure.
 */
template<class JOINT>
class ActuatorArrayDriver
{
public:
  ActuatorArrayDriver();
  virtual ~ActuatorArrayDriver();

  void read_and_publish();

  void spin();


protected:
  ros::Subscriber command_sub_;

  sensor_msgs::JointState command_msg_;

  void command_callback(const sensor_msgs::JointState::ConstPtr& command_msg);

  ros::ServiceServer stop_srv_;

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

  ros::ServiceServer home_srv_;

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

  ros::Publisher joint_state_pub_;

  sensor_msgs::JointState joint_state_msg_;

  std::string robot_description_parameter_;

  urdf::Model urdf_model_;

  std::map<std::string, JOINT> joints_;

  void parse_urdf(const ros::NodeHandle& node);

  void update_joint_from_urdf(const std::string& joint_name, JOINT& joint_properties);

  void parse_actuator_list(const ros::NodeHandle& node);

  void advertise_and_subscribe(ros::NodeHandle& node);

  void init();

  // a convenience function that allows a single YAML configuration file to both
  virtual bool init_actuator_(const std::string& joint_name, JOINT& joint_properties, XmlRpc::XmlRpcValue& joint_data) {return true;};

  virtual bool read_(ros::Time ts = ros::Time::now()) {return true;};

  virtual bool command_() {return true;};

  virtual bool stop_() {return true;};

  virtual bool home_() {return true;};

};

}

#include <actuator_array_driver/actuator_array_driver_impl.h>

#endif /* ACTUATOR_ARRAY_DRIVER_H_ */