pr2_controller_manager: pr2_controller_manager: controller

00001 
00002 // Copyright (C) 2008-2009, Willow Garage, Inc.
00003 //
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * Neither the name of Stanford University nor the names of its
00012 //     contributors may be used to endorse or promote products derived from
00013 //     this software without specific prior written permission.
00014 //
00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025 // POSSIBILITY OF SUCH DAMAGE.
00027 
00028 /*
00029  * Author: Stuart Glaser, Wim Meeussen
00030  */
00031 #ifndef MECHANISM_CONTROL_H
00032 #define MECHANISM_CONTROL_H
00033 
00034 #include <pthread.h>
00035 #include <map>
00036 #include <string>
00037 #include <vector>
00038 #include "ros/ros.h"
00039 #include "pr2_controller_manager/controller_spec.h"
00040 #include <tinyxml.h>
00041 #include <pr2_hardware_interface/hardware_interface.h>
00042 #include <pr2_mechanism_model/robot.h>
00043 #include <realtime_tools/realtime_publisher.h>
00044 #include <ros/node_handle.h>
00045 #include <pr2_controller_interface/controller_provider.h>
00046 #include "pluginlib/class_loader.h"
00047 #include <pr2_mechanism_msgs/ListControllerTypes.h>
00048 #include <pr2_mechanism_msgs/ListControllers.h>
00049 #include <pr2_mechanism_msgs/ReloadControllerLibraries.h>
00050 #include <pr2_mechanism_msgs/LoadController.h>
00051 #include <pr2_mechanism_msgs/UnloadController.h>
00052 #include <pr2_mechanism_msgs/SwitchController.h>
00053 #include <pr2_mechanism_msgs/MechanismStatistics.h>
00054 #include <sensor_msgs/JointState.h>
00055 #include <boost/thread/condition.hpp>
00056 
00057 
00058 namespace pr2_controller_manager{
00059 
00060 class ControllerManager: public pr2_controller_interface::ControllerProvider {
00061 
00062 public:
00063   ControllerManager(pr2_hardware_interface::HardwareInterface *hw, 
00064                    const ros::NodeHandle& nh=ros::NodeHandle());
00065   virtual ~ControllerManager();
00066 
00067   // Real-time functions
00068   void update();
00069 
00070   // Non real-time functions
00071   bool initXml(TiXmlElement* config);
00072   bool loadController(const std::string& name);
00073   bool unloadController(const std::string &name);
00074   bool switchController(const std::vector<std::string>& start_controllers,
00075                         const std::vector<std::string>& stop_controllers,
00076                         const int strictness);
00077 
00078   // controllers_lock_ must be locked before calling
00079   virtual pr2_controller_interface::Controller* getControllerByName(const std::string& name);
00080 
00081   pr2_mechanism_model::Robot model_;
00082   pr2_mechanism_model::RobotState *state_;
00083 
00084 private:
00085   void getControllerNames(std::vector<std::string> &v);
00086   void getControllerSchedule(std::vector<size_t> &schedule);
00087 
00088   ros::NodeHandle controller_node_, cm_node_;
00089   boost::shared_ptr<pluginlib::ClassLoader<pr2_controller_interface::Controller> > controller_loader_;
00090 
00091   // for controller switching
00092   std::vector<pr2_controller_interface::Controller*> start_request_, stop_request_;
00093   bool please_switch_;
00094   int switch_strictness_;
00095 
00096   // controller lists
00097   boost::mutex controllers_lock_;
00098   std::vector<ControllerSpec> controllers_lists_[2];
00099   std::vector<size_t>   controllers_scheduling_[2];
00100   int current_controllers_list_, used_by_realtime_;
00101 
00102   // for controller statistics
00103   Statistics pre_update_stats_;
00104   Statistics update_stats_;
00105   Statistics post_update_stats_;
00106 
00107   // for publishing constroller state
00108   void publishJointState();
00109   void publishMechanismStatistics();
00110   realtime_tools::RealtimePublisher<sensor_msgs::JointState> pub_joint_state_;
00111   realtime_tools::RealtimePublisher<pr2_mechanism_msgs::MechanismStatistics> pub_mech_stats_;
00112   ros::Duration publish_period_joint_state_, publish_period_mechanism_stats_;
00113   ros::Time last_published_joint_state_, last_published_mechanism_stats_;
00114 
00115   // services to work with controllers
00116   bool listControllerTypesSrv(pr2_mechanism_msgs::ListControllerTypes::Request &req,
00117                               pr2_mechanism_msgs::ListControllerTypes::Response &resp);
00118   bool listControllersSrv(pr2_mechanism_msgs::ListControllers::Request &req,
00119                           pr2_mechanism_msgs::ListControllers::Response &resp);
00120   bool switchControllerSrv(pr2_mechanism_msgs::SwitchController::Request &req,
00121                            pr2_mechanism_msgs::SwitchController::Response &resp);
00122   bool loadControllerSrv(pr2_mechanism_msgs::LoadController::Request &req,
00123                           pr2_mechanism_msgs::LoadController::Response &resp);
00124   bool unloadControllerSrv(pr2_mechanism_msgs::UnloadController::Request &req,
00125                          pr2_mechanism_msgs::UnloadController::Response &resp);
00126   bool reloadControllerLibrariesSrv(pr2_mechanism_msgs::ReloadControllerLibraries::Request &req,
00127                                     pr2_mechanism_msgs::ReloadControllerLibraries::Response &resp);
00128   boost::mutex services_lock_;
00129   ros::ServiceServer srv_list_controllers_, srv_list_controller_types_, srv_load_controller_;
00130   ros::ServiceServer srv_unload_controller_, srv_switch_controller_, srv_reload_libraries_;
00131   bool motors_previously_halted_;
00132 };
00133 
00134 }
00135 #endif /* MECHANISM_CONTROL_H */
controller_manager.h
