handle_layer.cpp
Go to the documentation of this file.
1 
3 #include "interface_mapping.h"
4 
5 using namespace canopen;
6 
7 
8 template<typename T > class LimitsHandle {
10 public:
11  LimitsHandle(const T &handle) : limits_handle_(handle) {}
12  virtual void enforce(const ros::Duration &period) { limits_handle_.enforceLimits(period); }
13  virtual void reset() {}
14 };
15 
18 
20  CommandMap::iterator it = commands_.find(m);
21  if(it == commands_.end()) return false;
22  jh_ = it->second;
23  return true;
24 }
25 
26 HandleLayer::HandleLayer(const std::string &name, const MotorBaseSharedPtr & motor, const ObjectStorageSharedPtr storage, XmlRpc::XmlRpcValue & options)
27 : HandleLayerBase(name + " Handle"), motor_(motor), variables_(storage), jsh_(name, &pos_, &vel_, &eff_), jph_(jsh_, &cmd_pos_), jvh_(jsh_, &cmd_vel_), jeh_(jsh_, &cmd_eff_), jh_(0), forward_command_(false),
28  filter_pos_("double"), filter_vel_("double"), filter_eff_("double"), options_(options), enable_limits_(true)
29 {
31 
32  std::string p2d("rint(rad2deg(pos)*1000)"), v2d("rint(rad2deg(vel)*1000)"), e2d("rint(eff)");
33  std::string p2r("deg2rad(obj6064)/1000"), v2r("deg2rad(obj606C)/1000"), e2r("0");
34 
35  if(options.hasMember("pos_unit_factor") || options.hasMember("vel_unit_factor") || options.hasMember("eff_unit_factor")){
36  const std::string reason("*_unit_factor parameters are not supported anymore, please migrate to conversion functions.");
37  ROS_FATAL_STREAM(reason);
38  throw std::invalid_argument(reason);
39  }
40 
41  if(options.hasMember("pos_to_device")) p2d = (const std::string&) options["pos_to_device"];
42  if(options.hasMember("pos_from_device")) p2r = (const std::string&) options["pos_from_device"];
43 
44  if(options.hasMember("vel_to_device")) v2d = (const std::string&) options["vel_to_device"];
45  if(options.hasMember("vel_from_device")) v2r = (const std::string&) options["vel_from_device"];
46 
47  if(options.hasMember("eff_to_device")) e2d = (const std::string&) options["eff_to_device"];
48  if(options.hasMember("eff_from_device")) e2r = (const std::string&) options["eff_from_device"];
49 
50  conv_target_pos_.reset(new UnitConverter(p2d, boost::bind(assignVariable, "pos", &cmd_pos_, _1)));
51  conv_target_vel_.reset(new UnitConverter(v2d, boost::bind(assignVariable, "vel", &cmd_vel_, _1)));
52  conv_target_eff_.reset(new UnitConverter(e2d, boost::bind(assignVariable, "eff", &cmd_eff_, _1)));
53 
54  conv_pos_.reset(new UnitConverter(p2r, boost::bind(&ObjectVariables::getVariable, &variables_, _1)));
55  conv_vel_.reset(new UnitConverter(v2r, boost::bind(&ObjectVariables::getVariable, &variables_, _1)));
56  conv_eff_.reset(new UnitConverter(e2r, boost::bind(&ObjectVariables::getVariable, &variables_, _1)));
57 }
58 
60  if(!motor_->isModeSupported(m) || commands_.find(m) == commands_.end()){
61  return NotSupported;
62  }else if(motor_->getMode() == m){
63  return NoNeedToSwitch;
64  }else if(motor_->getLayerState() == Ready){
65  return ReadyToSwitch;
66  }else{
67  return NotReadyToSwitch;
68  }
69 }
70 
72  if(motor_->getMode() != m){
73  forward_command_ = false;
74  jh_ = 0; // disconnect handle
75  if(!motor_->enterModeAndWait(m)){
76  ROS_ERROR_STREAM(jsh_.getName() << "could not enter mode " << (int)m);
77  LayerStatus s;
78  motor_->halt(s);
79  return false;
80  }
81  }
82  return select(m);
83 }
84 
86  if(motor_->getMode() == m){
87  forward_command_ = true;
88  return true;
89  }
90  return false;
91 }
92 
93 
94 template<typename T> void addLimitsHandle(std::vector<LimitsHandleBaseSharedPtr> &limits, const T &t) {
95  limits.push_back(LimitsHandleBaseSharedPtr( (LimitsHandleBase *) new LimitsHandle<T> (t) ));
96 }
97 
100  const joint_limits_interface::SoftJointLimits *soft_limits){
101  hardware_interface::JointHandle* h = addHandle(iface, &jph_, g_interface_mapping.getInterfaceModes("hardware_interface::PositionJointInterface"));
102  if(h && limits.has_position_limits){
104  if(soft_limits){
106  }
107  }
108  return h;
109 }
110 
113  const joint_limits_interface::SoftJointLimits *soft_limits){
114  hardware_interface::JointHandle* h = addHandle(iface, &jvh_, g_interface_mapping.getInterfaceModes("hardware_interface::VelocityJointInterface"));
115  if(h && limits.has_velocity_limits){
117  if(soft_limits){
119  }
120  }
121  return h;
122 }
123 
126  const joint_limits_interface::SoftJointLimits *soft_limits){
127  hardware_interface::JointHandle* h = addHandle(iface, &jeh_, g_interface_mapping.getInterfaceModes("hardware_interface::EffortJointInterface"));
128  if(h && limits.has_effort_limits){
130  if(soft_limits){
132  }
133  }
134  return h;
135 }
136 
137 void HandleLayer::handleRead(LayerStatus &status, const LayerState &current_state) {
138  if(current_state > Shutdown){
139  variables_.sync();
140  filter_pos_.update(conv_pos_->evaluate(), pos_);
141  filter_vel_.update(conv_vel_->evaluate(), vel_);
142  filter_eff_.update(conv_eff_->evaluate(), eff_);
143  }
144 }
145 void HandleLayer::handleWrite(LayerStatus &status, const LayerState &current_state) {
146  if(current_state == Ready){
148  if(forward_command_) jh = jh_;
149 
150  if(jh == &jph_){
151  motor_->setTarget(conv_target_pos_->evaluate());
152  cmd_vel_ = vel_;
153  cmd_eff_ = eff_;
154  }else if(jh == &jvh_){
155  motor_->setTarget(conv_target_vel_->evaluate());
156  cmd_pos_ = pos_;
157  cmd_eff_ = eff_;
158  }else if(jh == &jeh_){
159  motor_->setTarget(conv_target_eff_->evaluate());
160  cmd_pos_ = pos_;
161  cmd_vel_ = vel_;
162  }else{
163  cmd_pos_ = pos_;
164  cmd_vel_ = vel_;
165  cmd_eff_ = eff_;
166  if(jh) status.warn("unsupported mode active");
167  }
168  }
169 }
170 
171 bool prepareFilter(const std::string& joint_name, const std::string& filter_name, filters::FilterChain<double> &filter, XmlRpc::XmlRpcValue & options, canopen::LayerStatus &status){
172  filter.clear();
173  if(options.hasMember(filter_name)){
174  if(!filter.configure(options[filter_name],joint_name + "/" + filter_name)){
175  status.error("could not configure " + filter_name+ " for " + joint_name);
176  return false;
177  }
178  }
179 
180  return true;
181 }
182 
184  return prepareFilter(jsh_.getName(), "position_filters", filter_pos_, options_, status) &&
185  prepareFilter(jsh_.getName(), "velocity_filters", filter_vel_, options_, status) &&
186  prepareFilter(jsh_.getName(), "effort_filters", filter_eff_, options_, status);
187 }
188 
190  // TODO: implement proper init
191  conv_pos_->reset();
192  conv_vel_->reset();
193  conv_eff_->reset();
194  conv_target_pos_->reset();
195  conv_target_vel_->reset();
196  conv_target_eff_->reset();
197 
198 
199  if(prepareFilters(status))
200  {
201  handleRead(status, Layer::Ready);
202  }
203 }
204 
205 void HandleLayer::enforceLimits(const ros::Duration &period, bool reset){
206  for(std::vector<LimitsHandleBaseSharedPtr>::iterator it = limits_.begin(); it != limits_.end(); ++it){
207  if(reset) (*it)->reset();
208  if(enable_limits_) (*it)->enforce(period);
209  }
210 }
211 
212 void HandleLayer::enableLimits(bool enable){
213  enable_limits_ = enable;
214 }
CanSwitchResult canSwitch(const canopen::MotorBase::OperationMode &m)
filters::FilterChain< double > filter_pos_
Definition: handle_layer.h:94
bool switchMode(const canopen::MotorBase::OperationMode &m)
virtual void reset()
const void warn(const std::string &r)
virtual void handleWrite(canopen::LayerStatus &status, const LayerState &current_state)
XmlRpcServer s
hardware_interface::JointHandle * addHandle(T &iface, hardware_interface::JointHandle *jh, const std::vector< MotorBase::OperationMode > &modes)
Definition: handle_layer.h:105
std::vector< LimitsHandleBaseSharedPtr > limits_
Definition: handle_layer.h:125
void enforceLimits(const ros::Duration &period, bool reset)
void addLimitsHandle(std::vector< LimitsHandleBaseSharedPtr > &limits, const T &t)
boost::scoped_ptr< UnitConverter > conv_vel_
Definition: handle_layer.h:92
static double * assignVariable(const std::string &name, double *ptr, const std::string &req)
Definition: handle_layer.h:129
boost::scoped_ptr< UnitConverter > conv_pos_
Definition: handle_layer.h:92
std::vector< canopen::MotorBase::OperationMode > getInterfaceModes(const std::string &interface)
XmlRpc::XmlRpcValue options_
Definition: handle_layer.h:95
void registerHandle(hardware_interface::JointStateInterface &iface)
Definition: handle_layer.h:135
filters::FilterChain< double > filter_vel_
Definition: handle_layer.h:94
hardware_interface::JointHandle jvh_
Definition: handle_layer.h:98
virtual void handleRead(canopen::LayerStatus &status, const LayerState &current_state)
canopen::MotorBaseSharedPtr motor_
Definition: handle_layer.h:85
HandleLayer(const std::string &name, const canopen::MotorBaseSharedPtr &motor, const canopen::ObjectStorageSharedPtr storage, XmlRpc::XmlRpcValue &options)
#define ROS_FATAL_STREAM(args)
LimitsHandle(const T &handle)
hardware_interface::JointHandle jph_
Definition: handle_layer.h:98
boost::scoped_ptr< UnitConverter > conv_target_eff_
Definition: handle_layer.h:91
boost::shared_ptr< LimitsHandleBase > LimitsHandleBaseSharedPtr
Definition: handle_layer.h:28
InterfaceMapping g_interface_mapping
Definition: robot_layer.cpp:16
boost::scoped_ptr< UnitConverter > conv_eff_
Definition: handle_layer.h:92
virtual void handleInit(canopen::LayerStatus &status)
boost::atomic< hardware_interface::JointHandle * > jh_
Definition: handle_layer.h:99
bool prepareFilters(canopen::LayerStatus &status)
bool hasMember(const std::string &name) const
boost::scoped_ptr< UnitConverter > conv_target_vel_
Definition: handle_layer.h:91
hardware_interface::JointHandle jeh_
Definition: handle_layer.h:98
boost::atomic< bool > forward_command_
Definition: handle_layer.h:100
bool update(const T &data_in, T &data_out)
hardware_interface::JointStateHandle jsh_
Definition: handle_layer.h:97
bool configure(std::string param_name, ros::NodeHandle node=ros::NodeHandle())
const void error(const std::string &r)
void enableLimits(bool enable)
boost::scoped_ptr< UnitConverter > conv_target_pos_
Definition: handle_layer.h:91
bool select(const canopen::MotorBase::OperationMode &m)
#define ROS_ERROR_STREAM(args)
double * getVariable(const std::string &n)
Definition: handle_layer.h:61
filters::FilterChain< double > filter_eff_
Definition: handle_layer.h:94
bool forwardForMode(const canopen::MotorBase::OperationMode &m)
bool prepareFilter(const std::string &joint_name, const std::string &filter_name, filters::FilterChain< double > &filter, XmlRpc::XmlRpcValue &options, canopen::LayerStatus &status)
ObjectVariables variables_
Definition: handle_layer.h:90
virtual void enforce(const ros::Duration &period)


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