test_moveit_controller_manager.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
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 Willow Garage 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 
00035 /* Author: Ioan Sucan */
00036 
00037 #ifndef TEST_MOVEIT_CONTROLLER_MANAGER_
00038 #define TEST_MOVEIT_CONTROLLER_MANAGER_
00039 
00040 #include <moveit/controller_manager/controller_manager.h>
00041 
00042 namespace test_moveit_controller_manager
00043 {
00044 class TestMoveItControllerHandle : public moveit_controller_manager::MoveItControllerHandle
00045 {
00046 public:
00047   TestMoveItControllerHandle(const std::string& name) : MoveItControllerHandle(name)
00048   {
00049   }
00050 
00051   virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory)
00052   {
00053     return true;
00054   }
00055 
00056   virtual bool cancelExecution()
00057   {
00058     return true;
00059   }
00060 
00061   virtual bool waitForExecution(const ros::Duration& timeout = ros::Duration(0))
00062   {
00063     return false;
00064   }
00065 
00066   virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus()
00067   {
00068     return moveit_controller_manager::ExecutionStatus::SUCCEEDED;
00069   }
00070 };
00071 
00072 class TestMoveItControllerManager : public moveit_controller_manager::MoveItControllerManager
00073 {
00074 public:
00075   static const int ACTIVE = 1;
00076   static const int DEFAULT = 2;
00077 
00078   TestMoveItControllerManager()
00079   {
00080     controllers_["right_arm"] = DEFAULT;
00081     controllers_["left_arm"] = ACTIVE + DEFAULT;
00082     controllers_["arms"] = 0;
00083     controllers_["base"] = DEFAULT;
00084     controllers_["head"] = 0;
00085     controllers_["left_arm_head"] = 0;
00086 
00087     controller_joints_["right_arm"].push_back("rj1");
00088     controller_joints_["right_arm"].push_back("rj2");
00089 
00090     controller_joints_["left_arm"].push_back("lj1");
00091     controller_joints_["left_arm"].push_back("lj2");
00092     controller_joints_["left_arm"].push_back("lj3");
00093 
00094     controller_joints_["arms"].insert(controller_joints_["arms"].end(), controller_joints_["left_arm"].begin(),
00095                                       controller_joints_["left_arm"].end());
00096     controller_joints_["arms"].insert(controller_joints_["arms"].end(), controller_joints_["right_arm"].begin(),
00097                                       controller_joints_["right_arm"].end());
00098 
00099     controller_joints_["base"].push_back("basej");
00100     controller_joints_["head"].push_back("headj");
00101 
00102     controller_joints_["left_arm_head"].insert(controller_joints_["left_arm_head"].end(),
00103                                                controller_joints_["left_arm"].begin(),
00104                                                controller_joints_["left_arm"].end());
00105     controller_joints_["left_arm_head"].insert(controller_joints_["left_arm_head"].end(),
00106                                                controller_joints_["head"].begin(), controller_joints_["head"].end());
00107   }
00108 
00109   virtual moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name)
00110   {
00111     return moveit_controller_manager::MoveItControllerHandlePtr(new TestMoveItControllerHandle(name));
00112   }
00113 
00114   virtual void getControllersList(std::vector<std::string>& names)
00115   {
00116     names.clear();
00117     for (std::map<std::string, int>::const_iterator it = controllers_.begin(); it != controllers_.end(); ++it)
00118       names.push_back(it->first);
00119   }
00120 
00121   virtual void getActiveControllers(std::vector<std::string>& names)
00122   {
00123     names.clear();
00124     for (std::map<std::string, int>::const_iterator it = controllers_.begin(); it != controllers_.end(); ++it)
00125       if (it->second & ACTIVE)
00126         names.push_back(it->first);
00127   }
00128 
00129   virtual void getControllerJoints(const std::string& name, std::vector<std::string>& joints)
00130   {
00131     joints = controller_joints_[name];
00132   }
00133 
00134   virtual moveit_controller_manager::MoveItControllerManager::ControllerState
00135   getControllerState(const std::string& name)
00136   {
00137     moveit_controller_manager::MoveItControllerManager::ControllerState state;
00138     state.active_ = controllers_[name] & ACTIVE;
00139     state.default_ = false;
00140     return state;
00141   }
00142 
00143   virtual bool switchControllers(const std::vector<std::string>& activate, const std::vector<std::string>& deactivate)
00144   {
00145     for (std::size_t i = 0; i < deactivate.size(); ++i)
00146     {
00147       controllers_[deactivate[i]] &= ~ACTIVE;
00148       std::cout << "Deactivated controller " << deactivate[i] << std::endl;
00149     }
00150     for (std::size_t i = 0; i < activate.size(); ++i)
00151     {
00152       controllers_[activate[i]] |= ACTIVE;
00153       std::cout << "Activated controller " << activate[i] << std::endl;
00154     }
00155     return true;
00156   }
00157 
00158 protected:
00159   std::map<std::string, int> controllers_;
00160   std::map<std::string, std::vector<std::string> > controller_joints_;
00161 };
00162 }
00163 #endif


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jun 19 2019 19:24:16