controller_interface_base.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 
00018 #ifndef COB_TWIST_CONTROLLER_CONTROLLER_INTERFACES_CONTROLLER_INTERFACE_BASE_H
00019 #define COB_TWIST_CONTROLLER_CONTROLLER_INTERFACES_CONTROLLER_INTERFACE_BASE_H
00020 
00021 #include <vector>
00022 #include "ros/ros.h"
00023 
00024 #include "cob_twist_controller/cob_twist_controller_data_types.h"
00025 #include "cob_twist_controller/utils/simpson_integrator.h"
00026 
00027 namespace cob_twist_controller
00028 {
00029 
00031 class ControllerInterfaceBase
00032 {
00033     public:
00034         virtual ~ControllerInterfaceBase() {}
00035 
00036         virtual void initialize(ros::NodeHandle& nh,
00037                                 const TwistControllerParams& params) = 0;
00038         virtual void processResult(const KDL::JntArray& q_dot_ik,
00039                                    const KDL::JntArray& current_q) = 0;
00040 
00041     protected:
00042         ControllerInterfaceBase() {}
00043 
00044         TwistControllerParams params_;
00045         ros::NodeHandle nh_;
00046         ros::Publisher pub_;
00047 };
00048 
00050 class ControllerInterfacePositionBase : public ControllerInterfaceBase
00051 {
00052     public:
00053         ~ControllerInterfacePositionBase() {}
00054 
00055         bool updateIntegration(const KDL::JntArray& q_dot_ik,
00056                                const KDL::JntArray& current_q)
00057         {
00058             ros::Time now = ros::Time::now();
00059             period_ = now - last_update_time_;
00060             last_update_time_ = now;
00061             return integrator_->updateIntegration(q_dot_ik, current_q, pos_, vel_);
00062         }
00063 
00064     protected:
00065         ControllerInterfacePositionBase() {}
00066 
00067         boost::shared_ptr<SimpsonIntegrator> integrator_;
00068         std::vector<double> pos_, vel_;
00069         ros::Time last_update_time_;
00070         ros::Duration period_;
00071 };
00072 
00073 }
00074 
00075 #endif  // COB_TWIST_CONTROLLER_CONTROLLER_INTERFACES_CONTROLLER_INTERFACE_BASE_H


cob_twist_controller
Author(s): Felix Messmer , Marco Bezzon , Christoph Mark , Francisco Moreno
autogenerated on Thu Jun 6 2019 21:19:26