robot_layer.cpp
Go to the documentation of this file.
1 
2 
6 
8 #include <controller_manager_msgs/SwitchController.h>
9 
11 
12 #include "interface_mapping.h"
13 
14 using namespace canopen;
15 
17 
18 void RobotLayer::stopControllers(const std::vector<std::string> controllers){
19  controller_manager_msgs::SwitchController srv;
20  srv.request.stop_controllers = controllers;
21  srv.request.strictness = srv.request.BEST_EFFORT;
22  boost::thread call(boost::bind(ros::service::call<controller_manager_msgs::SwitchController>, "controller_manager/switch_controller", srv));
23  call.detach();
24 }
25 
26 void RobotLayer::add(const std::string &name, HandleLayerBaseSharedPtr handle){
27  LayerGroupNoDiag::add(handle);
28  handles_.insert(std::make_pair(name, handle));
29 }
30 
32 {
37 
38  urdf_.initParam("robot_description");
39 }
40 
42  if(first_init_){
43  for(HandleMap::iterator it = handles_.begin(); it != handles_.end(); ++it){
46 
47  urdf::JointConstSharedPtr joint = getJoint(it->first);
48 
49  if(!joint){
50  status.error("joint " + it->first + " not found");
51  return;
52  }
53 
54  bool has_joint_limits = joint_limits_interface::getJointLimits(joint, limits);
55 
56  has_joint_limits = joint_limits_interface::getJointLimits(it->first, nh_, limits) || has_joint_limits;
57 
58  bool has_soft_limits = has_joint_limits && joint_limits_interface::getSoftJointLimits(joint, soft_limits);
59 
60  if(!has_joint_limits){
61  ROS_WARN_STREAM("No limits found for " << it->first);
62  }
63 
64  it->second->registerHandle(state_interface_);
65 
67 
68  it->second->registerHandle(pos_interface_, limits, has_soft_limits ? &soft_limits : 0);
69  it->second->registerHandle(vel_interface_, limits, has_soft_limits ? &soft_limits : 0);
70  it->second->registerHandle(eff_interface_, limits, has_soft_limits ? &soft_limits : 0);
71  }
72  first_init_ = false;
73  }
75 }
76 
77 void RobotLayer::enforce(const ros::Duration &period, bool reset){
78  for(HandleMap::iterator it = handles_.begin(); it != handles_.end(); ++it){
79  it->second->enforceLimits(period, reset);
80  }
81 }
82 class ModeLookup {
85  std::map<std::string, int> lookup_;
87 public:
89  has_default_mode_ = nh_c.getParam("required_drive_mode", default_mode_);
90  has_lookup_ = nh_c.getParam("required_drive_modes", lookup_);
91  }
92  bool hasModes() { return has_default_mode_ || has_lookup_; }
93  bool hasMixedModes() { return has_lookup_; }
94  bool getMode(MotorBase::OperationMode &om, const std::string &key) {
95  std::map<std::string, int>::iterator f = lookup_.find(key);
96  if(f != lookup_.end()){
97  om = MotorBase::OperationMode(f->second);
98  return true;
99  }else if (has_default_mode_) {
100  om = MotorBase::OperationMode(default_mode_);
101  return true;
102  }
103  return false;
104  }
105 };
106 bool RobotLayer::prepareSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list) {
107  // compile-time check for mode switching support in ros_control
108  (void) &hardware_interface::RobotHW::prepareSwitch; // please upgrade to ros_control/contoller_manager 0.9.4 or newer
109 
110  // stop handles
111  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); ++controller_it){
112 
113  if(switch_map_.find(controller_it->name) == switch_map_.end()){
114  ROS_ERROR_STREAM(controller_it->name << " was not started before");
115  return false;
116  }
117  }
118 
119  // start handles
120  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){
121  SwitchContainer to_switch;
122  ros::NodeHandle nh(nh_, controller_it->name);
123  ModeLookup ml(nh);
124 
125  std::set<std::string> claimed_interfaces;
126 
127  if(controller_it->claimed_resources.size() > 0){
128  for (std::vector<hardware_interface::InterfaceResources>::const_iterator cres_it = controller_it->claimed_resources.begin(); cres_it != controller_it->claimed_resources.end(); ++cres_it){
129  for (std::set<std::string>::const_iterator res_it = cres_it->resources.begin(); res_it != cres_it->resources.end(); ++res_it){
130  claimed_interfaces.insert(cres_it->hardware_interface);
131  if(!ml.hasModes()){
132  ROS_ERROR_STREAM("Please set required_drive_mode(s) for controller " << controller_it->name);
133  return false;
134  }
135  if(claimed_interfaces.size() > 1 && !ml.hasMixedModes()){
136  ROS_ERROR_STREAM("controller "<< controller_it->name << " has mixed interfaces, please set required_drive_modes.");
137  return false;
138  }
139 
140  boost::unordered_map< std::string, HandleLayerBaseSharedPtr >::const_iterator h_it = handles_.find(*res_it);
141 
142  const std::string & joint = *res_it;
143 
144  if(h_it == handles_.end()){
145  ROS_ERROR_STREAM(joint << " not found");
146  return false;
147  }
148  SwitchData sd;
149  sd.enforce_limits = nh.param("enforce_limits", true);
150 
151  if(!ml.getMode(sd.mode, joint)){
152  ROS_ERROR_STREAM("could not determine drive mode for " << joint);
153  return false;
154  }
155 
156  if(g_interface_mapping.hasConflict(cres_it->hardware_interface, sd.mode)){
157  ROS_ERROR_STREAM(cres_it->hardware_interface << " cannot be provided in mode " << sd.mode);
158  return false;
159  }
160 
161  HandleLayerBase::CanSwitchResult res = h_it->second->canSwitch(sd.mode);
162 
163  switch(res){
165  ROS_ERROR_STREAM("Mode " << sd.mode << " is not available for " << joint);
166  return false;
168  ROS_ERROR_STREAM(joint << " is not ready to switch mode");
169  return false;
172  sd.handle = h_it->second;
173  to_switch.push_back(sd);
174  }
175  }
176  }
177  }
178  switch_map_.insert(std::make_pair(controller_it->name, to_switch));
179  }
180 
181  // perform mode switches
182  boost::unordered_set<HandleLayerBaseSharedPtr > to_stop;
183  std::vector<std::string> failed_controllers;
184  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); ++controller_it){
185  SwitchContainer &to_switch = switch_map_.at(controller_it->name);
186  for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
187  to_stop.insert(it->handle);
188  }
189  }
190  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){
191  SwitchContainer &to_switch = switch_map_.at(controller_it->name);
192  bool okay = true;
193  for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
194  it->handle->switchMode(MotorBase::No_Mode); // stop all
195  }
196  for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
197  if(!it->handle->switchMode(it->mode)){
198  failed_controllers.push_back(controller_it->name);
199  ROS_ERROR_STREAM("Could not switch one joint for " << controller_it->name << ", will stop all related joints and the controller.");
200  for(RobotLayer::SwitchContainer::iterator stop_it = to_switch.begin(); stop_it != to_switch.end(); ++stop_it){
201  to_stop.insert(stop_it->handle);
202  }
203  okay = false;
204  break;
205  }else{
206  it->handle->enableLimits(it->enforce_limits);
207  }
208  to_stop.erase(it->handle);
209  }
210  }
211  for(boost::unordered_set<HandleLayerBaseSharedPtr >::iterator it = to_stop.begin(); it != to_stop.end(); ++it){
212  (*it)->switchMode(MotorBase::No_Mode);
213  }
214  if(!failed_controllers.empty()){
215  stopControllers(failed_controllers);
216  // will not return false here since this would prevent other controllers to be started and therefore lead to an inconsistent state
217  }
218 
219  return true;
220 }
221 
222 void RobotLayer::doSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list) {
223  std::vector<std::string> failed_controllers;
224  for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){
225  try{
226  SwitchContainer &to_switch = switch_map_.at(controller_it->name);
227  for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
228  if(!it->handle->forwardForMode(it->mode)){
229  failed_controllers.push_back(controller_it->name);
230  ROS_ERROR_STREAM("Could not switch one joint for " << controller_it->name << ", will stop all related joints and the controller.");
231  for(RobotLayer::SwitchContainer::iterator stop_it = to_switch.begin(); stop_it != to_switch.end(); ++stop_it){
232  it->handle->switchMode(MotorBase::No_Mode);
233  }
234  break;
235  }
236  }
237 
238  }catch(const std::out_of_range&){
239  ROS_ERROR_STREAM("Conttroller " << controller_it->name << "not found, will stop it");
240  failed_controllers.push_back(controller_it->name);
241  }
242  }
243  if(!failed_controllers.empty()){
244  stopControllers(failed_controllers);
245  }
246 }
virtual void add(const VectorMemberSharedPtr &l)
hardware_interface::PositionJointInterface pos_interface_
Definition: robot_layer.h:20
f
urdf::JointConstSharedPtr getJoint(const std::string &n) const
Definition: robot_layer.h:51
std::map< std::string, int > lookup_
Definition: robot_layer.cpp:85
bool hasModes()
Definition: robot_layer.cpp:92
void add(const std::string &name, HandleLayerBaseSharedPtr handle)
Definition: robot_layer.cpp:26
HandleLayerBaseSharedPtr handle
Definition: robot_layer.h:37
void enforce(const ros::Duration &period, bool reset)
Definition: robot_layer.cpp:77
ModeLookup(ros::NodeHandle &nh_c)
Definition: robot_layer.cpp:88
virtual void handleInit(LayerStatus &status)
bool hasMixedModes()
Definition: robot_layer.cpp:93
virtual bool prepareSwitch(const std::list< ControllerInfo > &start_list, const std::list< ControllerInfo > &stop_list)
bool hasConflict(const std::string &interface, canopen::MotorBase::OperationMode mode)
SwitchMap switch_map_
Definition: robot_layer.h:43
virtual void handleInit(canopen::LayerStatus &status)
Definition: robot_layer.cpp:41
bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits &soft_limits)
hardware_interface::JointStateInterface state_interface_
Definition: robot_layer.h:19
bool param(const std::string &param_name, T &param_val, const T &default_val) const
canopen::MotorBase::OperationMode mode
Definition: robot_layer.h:38
bool has_default_mode_
Definition: robot_layer.cpp:84
const std::string name
URDF_EXPORT bool initParam(const std::string &param)
#define ROS_WARN_STREAM(args)
urdf::Model urdf_
Definition: robot_layer.h:32
bool getMode(MotorBase::OperationMode &om, const std::string &key)
Definition: robot_layer.cpp:94
boost::atomic< bool > first_init_
Definition: robot_layer.h:45
vector_type::iterator call(FuncType func, Data &status)
InterfaceMapping g_interface_mapping
Definition: robot_layer.cpp:16
int default_mode_
Definition: robot_layer.cpp:83
hardware_interface::VelocityJointInterface vel_interface_
Definition: robot_layer.h:21
bool getParam(const std::string &key, std::string &s) const
hardware_interface::EffortJointInterface eff_interface_
Definition: robot_layer.h:22
bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits &limits)
virtual void doSwitch(const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &stop_list)
const void error(const std::string &r)
std::vector< SwitchData > SwitchContainer
Definition: robot_layer.h:41
#define ROS_ERROR_STREAM(args)
bool has_lookup_
Definition: robot_layer.cpp:86
HandleMap handles_
Definition: robot_layer.h:35
ros::NodeHandle nh_
Definition: robot_layer.h:31
virtual bool prepareSwitch(const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &stop_list)
RobotLayer(ros::NodeHandle nh)
Definition: robot_layer.cpp:31
void stopControllers(const std::vector< std::string > controllers)
Definition: robot_layer.cpp:18


canopen_motor_node
Author(s): Mathias Lüdtke
autogenerated on Sat May 4 2019 02:40:47