test_moveit_controller_manager.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef TEST_MOVEIT_CONTROLLER_MANAGER_
38 #define TEST_MOVEIT_CONTROLLER_MANAGER_
39 
41 
43 {
45 {
46 public:
47  TestMoveItControllerHandle(const std::string& name) : MoveItControllerHandle(name)
48  {
49  }
50 
51  virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory)
52  {
53  return true;
54  }
55 
56  virtual bool cancelExecution()
57  {
58  return true;
59  }
60 
61  virtual bool waitForExecution(const ros::Duration& timeout = ros::Duration(0))
62  {
63  return false;
64  }
65 
67  {
69  }
70 };
71 
73 {
74 public:
75  static const int ACTIVE = 1;
76  static const int DEFAULT = 2;
77 
79  {
80  controllers_["right_arm"] = DEFAULT;
81  controllers_["left_arm"] = ACTIVE + DEFAULT;
82  controllers_["arms"] = 0;
83  controllers_["base"] = DEFAULT;
84  controllers_["head"] = 0;
85  controllers_["left_arm_head"] = 0;
86 
87  controller_joints_["right_arm"].push_back("rj1");
88  controller_joints_["right_arm"].push_back("rj2");
89 
90  controller_joints_["left_arm"].push_back("lj1");
91  controller_joints_["left_arm"].push_back("lj2");
92  controller_joints_["left_arm"].push_back("lj3");
93 
94  controller_joints_["arms"].insert(controller_joints_["arms"].end(), controller_joints_["left_arm"].begin(),
95  controller_joints_["left_arm"].end());
96  controller_joints_["arms"].insert(controller_joints_["arms"].end(), controller_joints_["right_arm"].begin(),
97  controller_joints_["right_arm"].end());
98 
99  controller_joints_["base"].push_back("basej");
100  controller_joints_["head"].push_back("headj");
101 
102  controller_joints_["left_arm_head"].insert(controller_joints_["left_arm_head"].end(),
103  controller_joints_["left_arm"].begin(),
104  controller_joints_["left_arm"].end());
105  controller_joints_["left_arm_head"].insert(controller_joints_["left_arm_head"].end(),
106  controller_joints_["head"].begin(), controller_joints_["head"].end());
107  }
108 
109  virtual moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name)
110  {
111  return moveit_controller_manager::MoveItControllerHandlePtr(new TestMoveItControllerHandle(name));
112  }
113 
114  virtual void getControllersList(std::vector<std::string>& names)
115  {
116  names.clear();
117  for (std::map<std::string, int>::const_iterator it = controllers_.begin(); it != controllers_.end(); ++it)
118  names.push_back(it->first);
119  }
120 
121  virtual void getActiveControllers(std::vector<std::string>& names)
122  {
123  names.clear();
124  for (std::map<std::string, int>::const_iterator it = controllers_.begin(); it != controllers_.end(); ++it)
125  if (it->second & ACTIVE)
126  names.push_back(it->first);
127  }
128 
129  virtual void getControllerJoints(const std::string& name, std::vector<std::string>& joints)
130  {
131  joints = controller_joints_[name];
132  }
133 
135  getControllerState(const std::string& name)
136  {
138  state.active_ = controllers_[name] & ACTIVE;
139  state.default_ = false;
140  return state;
141  }
142 
143  virtual bool switchControllers(const std::vector<std::string>& activate, const std::vector<std::string>& deactivate)
144  {
145  for (std::size_t i = 0; i < deactivate.size(); ++i)
146  {
147  controllers_[deactivate[i]] &= ~ACTIVE;
148  std::cout << "Deactivated controller " << deactivate[i] << std::endl;
149  }
150  for (std::size_t i = 0; i < activate.size(); ++i)
151  {
152  controllers_[activate[i]] |= ACTIVE;
153  std::cout << "Activated controller " << activate[i] << std::endl;
154  }
155  return true;
156  }
157 
158 protected:
159  std::map<std::string, int> controllers_;
160  std::map<std::string, std::vector<std::string> > controller_joints_;
161 };
162 }
163 #endif
virtual bool switchControllers(const std::vector< std::string > &activate, const std::vector< std::string > &deactivate)
virtual void getControllersList(std::vector< std::string > &names)
std::map< std::string, std::vector< std::string > > controller_joints_
virtual moveit_controller_manager::ExecutionStatus getLastExecutionStatus()
virtual void getControllerJoints(const std::string &name, std::vector< std::string > &joints)
virtual void getActiveControllers(std::vector< std::string > &names)
virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory &trajectory)
virtual moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string &name)
virtual moveit_controller_manager::MoveItControllerManager::ControllerState getControllerState(const std::string &name)
virtual bool waitForExecution(const ros::Duration &timeout=ros::Duration(0))


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:17:33