multi_joint_controller.h
Go to the documentation of this file.
00001 /*
00002     Copyright (c) 2011, Antons Rebguns <email>
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 Antons Rebguns <email> ''AS IS'' AND ANY
00017     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 Antons Rebguns <email> 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 
00029 #ifndef DYNAMIXEL_HARDWARE_INTERFACE_MULTI_JOINT_CONTROLLER_H
00030 #define DYNAMIXEL_HARDWARE_INTERFACE_MULTI_JOINT_CONTROLLER_H
00031 
00032 #include <map>
00033 #include <string>
00034 #include <cmath>
00035 
00036 #include <dynamixel_hardware_interface/dynamixel_io.h>
00037 #include <dynamixel_hardware_interface/single_joint_controller.h>
00038 #include <dynamixel_hardware_interface/JointState.h>
00039 
00040 #include <ros/ros.h>
00041 #include <std_msgs/Float64.h>
00042 
00043 namespace controller
00044 {
00045 
00046 class MultiJointController
00047 {
00048 public:
00049     MultiJointController() {};
00050     virtual ~MultiJointController() {};
00051     
00052     virtual bool initialize(std::string name, std::vector<SingleJointController*> deps)
00053     {
00054         name_ = name;
00055         deps_ = deps;
00056         
00057         try
00058         {
00059             c_nh_ = ros::NodeHandle(nh_, name_);
00060         }
00061         catch(std::exception &e)
00062         {
00063             ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s':\n%s", name_.c_str(), e.what());
00064             return false;
00065         }
00066         catch(...)
00067         {
00068             ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s'", name_.c_str());
00069             return false;
00070         }
00071 
00072         num_joints_ = deps_.size();
00073         joint_names_.resize(num_joints_);
00074         
00075         for (size_t i = 0; i < num_joints_; ++i)
00076         {
00077             std::string joint_name = deps_[i]->getJointName();
00078             std::string port_namespace = deps_[i]->getPortNamespace();
00079             
00080             joint_names_[i] = joint_name;
00081             joint_to_idx_[joint_name] = i;
00082             joint_to_controller_[joint_name] = deps_[i];
00083             joint_states_[joint_name] = &deps_[i]->getJointState();
00084             
00085             port_to_joints_[port_namespace].push_back(joint_name);
00086             port_to_io_[port_namespace] = deps_[i]->getPort();
00087             
00088             ROS_DEBUG("Adding joint %s to controller %s on port %s", joint_name.c_str(), name.c_str(), port_namespace.c_str());
00089         }
00090 
00091         return true;
00092     }
00093     
00094     virtual void start() = 0;
00095     virtual void stop() = 0;
00096     
00097     const std::vector<SingleJointController*>& getDependencies() { return deps_; }
00098     
00099 protected:
00100     ros::NodeHandle nh_;
00101     ros::NodeHandle c_nh_;
00102     
00103     std::string name_;
00104     std::vector<SingleJointController*> deps_;
00105     
00106     size_t num_joints_;
00107     
00108     std::vector<std::string> joint_names_;
00109     std::map<std::string, int> joint_to_idx_;
00110     
00111     std::map<std::string, SingleJointController*> joint_to_controller_;
00112     std::map<std::string, std::vector<std::string> > port_to_joints_;
00113     std::map<std::string, dynamixel_hardware_interface::DynamixelIO*> port_to_io_;
00114     std::map<std::string, const dynamixel_hardware_interface::JointState*> joint_states_;
00115     
00116 };
00117 
00118 }
00119 
00120 #endif  // DYNAMIXEL_HARDWARE_INTERFACE_MULTI_JOINT_CONTROLLER_H


dynamixel_hardware_interface
Author(s): Antons Rebguns
autogenerated on Sun Oct 5 2014 23:33:10