controller_updater.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Modified BSD License)
00003  *
00004  *  Copyright (c) 2013, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of PAL Robotics, S.L. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #include "play_motion/controller_updater.h"
00038 
00039 #include <boost/foreach.hpp>
00040 #include <controller_manager_msgs/ListControllers.h>
00041 
00042 #define foreach BOOST_FOREACH
00043 
00044 namespace play_motion
00045 {
00046 
00047   static ros::ServiceClient initCmClient(ros::NodeHandle nh)
00048   {
00049     return nh.serviceClient<controller_manager_msgs::ListControllers>
00050         ("controller_manager/list_controllers", true);
00051   }
00052 
00053   ControllerUpdater::ControllerUpdater(ros::NodeHandle nh) : nh_(nh)
00054   {
00055     cm_client_ = initCmClient(nh_);
00056     main_thread_ = boost::thread(&ControllerUpdater::mainLoop, this);
00057   }
00058 
00059   ControllerUpdater::~ControllerUpdater()
00060   {}
00061 
00062   static bool isJointTrajectoryController(const std::string& name)
00063   {
00064     std::string tofind = "JointTrajectoryController"; //XXX: magic value
00065     size_t pos = name.find(tofind);
00066     if (pos == std::string::npos)
00067       return false;
00068     if (pos != name.length() - tofind.length())
00069       return false;
00070 
00071     return true;
00072   }
00073 
00074   void ControllerUpdater::mainLoop()
00075   {
00076     ros::Rate r(1.0); //XXX: magic value
00077     for (; ros::ok(); r.sleep())
00078     {
00079       if (!update_cb_)
00080         continue;
00081 
00082       controller_manager_msgs::ListControllers srv;
00083 
00084       if (!cm_client_.isValid())
00085         cm_client_ = initCmClient(nh_);
00086       if(!cm_client_.call(srv))
00087       {
00088         ROS_WARN_THROTTLE(5.0, "Could not get list of controllers from controller manager.");
00089         continue;
00090       }
00091 
00092       ControllerStates states;
00093       ControllerJoints joints;
00094       typedef controller_manager_msgs::ControllerState cstate_t;
00095       foreach (const cstate_t& cs, srv.response.controller)
00096       {
00097         if (!isJointTrajectoryController(cs.type))
00098           continue;
00099         states[cs.name] = (cs.state == "running" ? RUNNING : STOPPED);
00100         joints[cs.name] = cs.resources;
00101       }
00102 
00103       if (states == last_cstates_)
00104         continue;
00105 
00106       ROS_INFO("The set of running joint trajectory controllers has changed, updating it.");
00107       update_timer_ = nh_.createTimer(ros::Duration(0), boost::bind(update_cb_, states, joints), true);
00108       last_cstates_ = states;
00109     }
00110   }
00111 
00112 }


play_motion
Author(s): Paul Mathieu
autogenerated on Wed Aug 26 2015 15:29:25