moveit_controller_manager_example.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ioan A. Sucan
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 Ioan A. Sucan 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 #include <ros/ros.h>
00038 #include <moveit/controller_manager/controller_manager.h>
00039 #include <sensor_msgs/JointState.h>
00040 #include <pluginlib/class_list_macros.h>
00041 #include <map>
00042 
00043 namespace moveit_controller_manager_example
00044 {
00045 class ExampleControllerHandle : public moveit_controller_manager::MoveItControllerHandle
00046 {
00047 public:
00048   ExampleControllerHandle(const std::string& name) : moveit_controller_manager::MoveItControllerHandle(name)
00049   {
00050   }
00051 
00052   virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory& t)
00053   {
00054     // do whatever is needed to actually execute this trajectory
00055     return true;
00056   }
00057 
00058   virtual bool cancelExecution()
00059   {
00060     // do whatever is needed to cancel execution
00061     return true;
00062   }
00063 
00064   virtual bool waitForExecution(const ros::Duration&)
00065   {
00066     // wait for the current execution to finish
00067     return true;
00068   }
00069 
00070   virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus()
00071   {
00072     return moveit_controller_manager::ExecutionStatus(moveit_controller_manager::ExecutionStatus::SUCCEEDED);
00073   }
00074 };
00075 
00076 class MoveItControllerManagerExample : public moveit_controller_manager::MoveItControllerManager
00077 {
00078 public:
00079   MoveItControllerManagerExample()
00080   {
00081   }
00082 
00083   virtual ~MoveItControllerManagerExample()
00084   {
00085   }
00086 
00087   virtual moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name)
00088   {
00089     return moveit_controller_manager::MoveItControllerHandlePtr(new ExampleControllerHandle(name));
00090   }
00091 
00092   /*
00093    * Get the list of controller names.
00094    */
00095   virtual void getControllersList(std::vector<std::string>& names)
00096   {
00097     names.resize(1);
00098     names[0] = "my_example_controller";
00099   }
00100 
00101   /*
00102    * This plugin assumes that all controllers are already active -- and if they are not, well, it has no way to deal
00103    * with it anyways!
00104    */
00105   virtual void getActiveControllers(std::vector<std::string>& names)
00106   {
00107     getControllersList(names);
00108   }
00109 
00110   /*
00111    * Controller must be loaded to be active, see comment above about active controllers...
00112    */
00113   virtual void getLoadedControllers(std::vector<std::string>& names)
00114   {
00115     getControllersList(names);
00116   }
00117 
00118   /*
00119    * Get the list of joints that a controller can control.
00120    */
00121   virtual void getControllerJoints(const std::string& name, std::vector<std::string>& joints)
00122   {
00123     joints.clear();
00124     if (name == "my_example_controller")
00125     {
00126       // declare which joints this controller actuates
00127       joints.push_back("joint1");
00128       joints.push_back("joint2");
00129       joints.push_back("joint3");
00130       joints.push_back("joint4");
00131       // ...
00132     }
00133   }
00134 
00135   /*
00136    * Controllers are all active and default.
00137    */
00138   virtual moveit_controller_manager::MoveItControllerManager::ControllerState
00139   getControllerState(const std::string& name)
00140   {
00141     moveit_controller_manager::MoveItControllerManager::ControllerState state;
00142     state.active_ = true;
00143     state.default_ = true;
00144     return state;
00145   }
00146 
00147   /* Cannot switch our controllers */
00148   virtual bool switchControllers(const std::vector<std::string>& activate, const std::vector<std::string>& deactivate)
00149   {
00150     return false;
00151   }
00152 
00153 protected:
00154   ros::NodeHandle node_handle_;
00155   std::map<std::string, moveit_controller_manager::MoveItControllerHandlePtr> controllers_;
00156 };
00157 
00158 }  // end namespace moveit_controller_manager_example
00159 
00160 PLUGINLIB_EXPORT_CLASS(moveit_controller_manager_example::MoveItControllerManagerExample,
00161                        moveit_controller_manager::MoveItControllerManager);


moveit_controller_manager_example
Author(s): Ioan Sucan
autogenerated on Wed Jun 19 2019 19:24:01