KukaLBRiiwaROSPlugin_BASE_26696.hpp
Go to the documentation of this file.
1 #ifndef GRL_ROS_BRIDGE_HPP_
2 #define GRL_ROS_BRIDGE_HPP_
3 
4 #include <iostream>
5 #include <memory>
6 #include <array>
7 
8 #include <boost/log/trivial.hpp>
9 #include <boost/exception/all.hpp>
10 #include <boost/algorithm/string.hpp>
11 #include <boost/thread/mutex.hpp>
12 #include <boost/thread/locks.hpp>
13 
14 #include <ros/ros.h>
15 #include <trajectory_msgs/JointTrajectory.h>
16 #include <trajectory_msgs/JointTrajectoryPoint.h>
17 #include <sensor_msgs/JointState.h>
18 #include <std_msgs/String.h>
19 
20 #include "grl/kuka/KukaDriver.hpp"
23 
24 
25 /// @todo move elsewhere, because it will conflict with others' implementations of outputting vectors
26 template<typename T>
27 inline boost::log::formatting_ostream& operator<<(boost::log::formatting_ostream& out, std::vector<T>& v)
28 {
29  out << "[";
30  size_t last = v.size() - 1;
31  for(size_t i = 0; i < v.size(); ++i) {
32  out << v[i];
33  if (i != last)
34  out << ", ";
35  }
36  out << "]";
37  return out;
38 }
39 
40 namespace grl {
41 
42  namespace ros {
43 
44  /**
45  *
46  * This class contains code to offer a simple communication layer between ROS and the KUKA LBR iiwa
47  *
48  * @todo Main Loop Update Rate must be supplied to underlying Driver for FRI mode. see KukaLBRiiwaVrepPlugin for reference, particularly kukaDriverP_->set(simulationTimeStep_,time_duration_command_tag());
49  */
50  class KukaLBRiiwaROSPlugin : public std::enable_shared_from_this<KukaLBRiiwaROSPlugin>
51  {
52  public:
53 
54  enum ParamIndex {
55  RobotName,
56  RobotModel,
65  };
66 
67  typedef std::tuple<
68  std::string,
69  std::string,
70  std::string,
71  std::string,
72  std::string,
73  std::string,
74  std::string,
75  std::string,
76  std::string,
77  std::string
78  > Params;
79 
80 
81  static const Params defaultParams(){
82  return std::make_tuple(
83  "Robotiiwa" , // RobotName,
84  "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800)
85  "tcp://0.0.0.0:30010" , // LocalZMQAddress
86  "tcp://172.31.1.147:30010", // RemoteZMQAddress
87  "192.170.10.100" , // LocalHostKukaKoniUDPAddress,
88  "30200" , // LocalHostKukaKoniUDPPort,
89  "192.170.10.2" , // RemoteHostKukaKoniUDPAddress,
90  "30200" , // RemoteHostKukaKoniUDPPort
91  "JAVA" , // KukaCommandMode (options are FRI, JAVA)
92  "FRI" // KukaMonitorMode (options are FRI, JAVA)
93  );
94  }
95 
97  ::ros::NodeHandle nh_tilde("~");
98 
99  nh_tilde.getParam("RobotName",std::get<RobotName>(params));
100  nh_tilde.getParam("RobotModel",std::get<RobotModel>(params));
101  nh_tilde.getParam("LocalZMQAddress",std::get<LocalZMQAddress>(params));
102  nh_tilde.getParam("RemoteZMQAddress",std::get<RemoteZMQAddress>(params));
103  nh_tilde.getParam("LocalHostKukaKoniUDPAddress",std::get<LocalHostKukaKoniUDPAddress>(params));
104  nh_tilde.getParam("LocalHostKukaKoniUDPPort",std::get<LocalHostKukaKoniUDPPort>(params));
105  nh_tilde.getParam("RemoteHostKukaKoniUDPAddress",std::get<RemoteHostKukaKoniUDPAddress>(params));
106  nh_tilde.getParam("RemoteHostKukaKoniUDPPort",std::get<RemoteHostKukaKoniUDPPort>(params));
107  nh_tilde.getParam("KukaCommandMode",std::get<KukaCommandMode>(params));
108  nh_tilde.getParam("KukaMonitorMode",std::get<KukaMonitorMode>(params));
109 
110  return params;
111  }
112 
113  /// unique tag type so State never
114  /// conflicts with a similar tuple
115  struct JointStateTag{};
116 
119  JointForce,
123  JointMatrix,
125  };
126 
127 
128  typedef std::vector<double> JointScalar;
129 
130  /// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simGetJointMatrix for data layout information
131  typedef std::array<double,12> TransformationMatrix;
132  typedef std::vector<TransformationMatrix> TransformationMatrices;
133 
134  typedef std::tuple<
135  JointScalar, // jointPosition
136  // JointScalar // JointVelocity // no velocity yet
137  JointScalar, // jointForce
138  JointScalar, // jointTargetPosition
139  JointScalar, // JointLowerPositionLimit
140  JointScalar, // JointUpperPositionLimit
141  TransformationMatrices, // jointTransformation
142  robot::arm::KukaJAVAdriver::JointStateTag // JointStateTag unique identifying type so tuple doesn't conflict
143  > State;
144 
145 
147  : debug(false),params_(params), nh_("")
148  {
149  loadRosParams(params_);
150  }
151 
152  void construct(){ construct(params_);}
153 
154  /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails
155  /// @warning getting the ik group is optional, so it does not throw an exception
156  void construct(Params params) {
157 
158  current_js_.name.resize(7);
159  current_js_.name[0] = "iiwa_joint_1";
160  current_js_.name[1] = "iiwa_joint_2";
161  current_js_.name[2] = "iiwa_joint_3";
162  current_js_.name[3] = "iiwa_joint_4";
163  current_js_.name[4] = "iiwa_joint_5";
164  current_js_.name[5] = "iiwa_joint_6";
165  current_js_.name[6] = "iiwa_joint_7";
166  current_js_.velocity.resize(7);
167  current_js_.velocity[0] = 0.;
168  current_js_.velocity[1] = 0.;
169  current_js_.velocity[2] = 0.;
170  current_js_.velocity[3] = 0.;
171  current_js_.velocity[4] = 0.;
172  current_js_.velocity[5] = 0.;
173  current_js_.velocity[6] = 0.;
174 
175  ::ros::NodeHandle nh;
176  js_pub_ = nh.advertise<sensor_msgs::JointState>("joint_states",100);
177  jt_sub_ = nh.subscribe<trajectory_msgs::JointTrajectory>("joint_traj_cmd", 1000, &KukaLBRiiwaROSPlugin::jt_callback, this);
178  jt_pt_sub_ = nh.subscribe<trajectory_msgs::JointTrajectoryPoint>("joint_traj_pt_cmd", 1000, &KukaLBRiiwaROSPlugin::jt_pt_callback, this);
179  mode_sub_ = nh.subscribe<std_msgs::String>("interaction_mode", 1000, &KukaLBRiiwaROSPlugin::mode_callback, this);
180  ROS_INFO("done creating subscribers");
181  //jt_sub_ = nh.subscribe<trajectory_msgs::JointTrajectory>("joint_traj_cmd",1000,boost::bind(&KukaLBRiiwaROSPlugin::jt_callback, this, _1));
182 
183  params_ = params;
184  // keep driver threads from exiting immediately after creation, because they have work to do!
185  device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service));
186 
187  /// @todo properly support passing of io_service
188  KukaDriverP_.reset(
190  //device_driver_io_service,
191  params
192  // std::make_tuple(
193  // std::string(std::std::get<LocalHostKukaKoniUDPAddress > (params)),
194  // std::string(std::std::get<LocalHostKukaKoniUDPPort > (params)),
195  // std::string(std::std::get<RemoteHostKukaKoniUDPAddress> (params)),
196  // std::string(std::std::get<RemoteHostKukaKoniUDPPort > (params)),
197  // grl::robot::arm::KukaFRIClientDataDriver::run_automatically
198  // )
199  )
200 
201  );
202  KukaDriverP_->construct();
203  }
204 
205 
206 
207 
208  bool setState(State& state) { return true; }
209 
210 
211  const Params & getParams(){
212  return params_;
213  }
214 
215  /**
216  * ROS joint trajectory callback
217  * this code needs to execute the joint trajectory on the robot
218  */
219  void jt_callback(const trajectory_msgs::JointTrajectoryConstPtr &msg) {
220  boost::lock_guard<boost::mutex> lock(jt_mutex);
221 
222 
223  for(auto &pt: msg->points) {
224  // positions velocities ac
225  if (pt.positions.size() != KUKA::LBRState::NUM_DOF) {
226  BOOST_THROW_EXCEPTION(std::runtime_error("Malformed joint trajectory request! Wrong number of joints."));
227  }
228 
229  // handle
230  //simJointPosition
231  simJointPosition.clear();
232  boost::copy(pt.positions,std::back_inserter(simJointPosition));
233  //simJointVelocity
234  simJointVelocity.clear();
235  boost::copy(pt.velocities,std::back_inserter(simJointVelocity));
236  //simJointForce
237  simJointForce.clear();
238  boost::copy(pt.effort,std::back_inserter(simJointForce));
239 
240  ///@todo: execute the rest of the trajectory
241  break;
242  }
243 
244  }
245 
246 
247  /// ROS callback to set current interaction mode; determines whether commands will be send in SERVO, TEACH, etc
248  void mode_callback(const std_msgs::StringConstPtr &msg) {
249  boost::lock_guard<boost::mutex> lock(jt_mutex);
250 
251  //std::cerr << "Mode command = " << msg->data.c_str() << "\n";
252  if (debug) {
253  ROS_INFO("Receiving mode command: %s", msg->data.c_str());
254  }
255 
256  unsigned int ArmStateLen = 9;
257  for (unsigned int i = 0; i < ArmStateLen; ++i) {
258  if (msg->data == grl::flatbuffer::EnumNamesArmState()[i]) {
259 
260  if (debug) {
261  std::string info = std::string("Valid grl::flatbuffer::ArmState command received for ") + grl::flatbuffer::EnumNamesArmState()[i] + std::string(" mode");
262  ROS_INFO(info.c_str());
263  }
264 
265  interaction_mode = static_cast<grl::flatbuffer::ArmState>(i);
266  KukaDriverP_->set(interaction_mode);
267  break;
268  }
269  }
270 
271  }
272 
273  /// ROS joint trajectory callback
274  /// this code needs to execute the joint trajectory on the robot
275  void jt_pt_callback(const trajectory_msgs::JointTrajectoryPointConstPtr &msg) {
276  boost::lock_guard<boost::mutex> lock(jt_mutex);
277 
278 
279  // positions velocities ac
280  if (msg->positions.size() != KUKA::LBRState::NUM_DOF) {
281  BOOST_THROW_EXCEPTION(std::runtime_error("Malformed joint trajectory request! Wrong number of joints."));
282  }
283 
284  // handle
285  //simJointPosition
286  simJointPosition.clear();
287  boost::copy(msg->positions,std::back_inserter(simJointPosition));
288  //simJointVelocity
289  simJointVelocity.clear();
290  boost::copy(msg->velocities,std::back_inserter(simJointVelocity));
291  //simJointForce
292  simJointForce.clear();
293  boost::copy(msg->effort,std::back_inserter(simJointForce));
294 
295  ///@todo: execute the rest of the trajectory
296  }
297 
298 
300  device_driver_workP_.reset();
301 
302  if(driver_threadP){
304  driver_threadP->join();
305  }
306  }
307 
308  ///
309  /// @brief spin once, call this repeatedly to run the driver
310  ///
311  ///
312  bool run_one()
313  {
314 
315  bool haveNewData = false;
316 
317  if(KukaDriverP_){
318 
319 
320  switch(interaction_mode) {
322  if (debug) {
323  ROS_INFO("Arm is in SERVO Mode");
324  }
326  /// @todo setting joint position clears joint force in KukaDriverP_. Is this right or should position and force be configurable simultaeously?
327  //if(simJointForce.size()) KukaDriverP_->set( simJointForce, grl::revolute_joint_torque_open_chain_command_tag());
328  break;
330  if (debug) {
331  ROS_INFO("Arm is in TEACH mode");
332  }
333  break;
334  break;
336  break;
338  break;
340  if (debug) {
341  ROS_INFO("Sending start!");
342  }
343  break;
345  break;
346  default:
347  ROS_INFO("KukaLBRiiwaROSPlugin in unsupported mode!");
348  }
349 
350  haveNewData = KukaDriverP_->run_one();
351 
352  if(haveNewData)
353  {
354 
355  // We have the real kuka state read from the device now
356  // update real joint angle data
357  current_js_.position.clear();
358  KukaDriverP_->get(std::back_inserter(current_js_.position), grl::revolute_joint_angle_open_chain_state_tag());
359 
360  current_js_.effort.clear();
361  KukaDriverP_->get(std::back_inserter(current_js_.effort), grl::revolute_joint_torque_open_chain_state_tag());
362 
363  //current_js_.velocity.clear();
364  //grl::robot::arm::copy(friData_->monitoringMsg, std::back_inserter(current_js_.velocity), grl::revolute_joint_angle_open_chain_state_tag());
365 
366  current_js_.header.stamp = ::ros::Time::now();
367  current_js_.header.seq += 1;
368  js_pub_.publish(current_js_);
369  }
370 
371  }
372 
373  return haveNewData;
374  }
375 
376  boost::asio::io_service device_driver_io_service;
377  std::unique_ptr<boost::asio::io_service::work> device_driver_workP_;
378  std::unique_ptr<std::thread> driver_threadP;
379 
380  /// @todo replace all these simJoint elements with simple KukaLBRiiwaROSPlugin::State
381  std::vector<double> simJointPosition;// = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
382  std::vector<double> simJointVelocity = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
383  std::vector<double> simJointForce;// = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
384  std::vector<double> simJointTargetPosition = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
386 
387  /// @note loss of precision! kuka sends double values, if you write custom code don't use these float values. Vrep uses floats internally which is why they are used here.
388  std::vector<double> realJointPosition = { 0, 0, 0, 0, 0, 0, 0 };
389  // does not exist
390  std::vector<double> realJointForce = { 0, 0, 0, 0, 0, 0, 0 };
391 
392  private:
393 
394  bool debug;
395  grl::flatbuffer::ArmState interaction_mode;
396 
397  boost::mutex jt_mutex;
398  boost::shared_ptr<robot::arm::KukaDriver> KukaDriverP_;
399  Params params_;
400 
401  ::ros::Subscriber jt_sub_; // subscribes to joint state trajectories and executes them
402  ::ros::Subscriber jt_pt_sub_; // subscribes to joint state trajectories and executes them
403  ::ros::Subscriber mode_sub_; // subscribes to interaction mode messages (strings for now)
404 
405  ::ros::Publisher js_pub_; // publish true joint states from the KUKA
406 
407 
408  sensor_msgs::JointState current_js_;
409 
410  ::ros::NodeHandle nh_;
411 
412  };
413  }
414 }
415 
416 
417 #endif
void jt_pt_callback(const trajectory_msgs::JointTrajectoryPointConstPtr &msg)
bool run_one()
spin once, call this repeatedly to run the driver
std::vector< double > simJointTargetPosition
void jt_callback(const trajectory_msgs::JointTrajectoryConstPtr &msg)
OutputIterator copy(std::string model, OutputIterator it, grl::revolute_joint_velocity_open_chain_state_constraint_tag)
copy vector of joint velocity limits in radians/s
Definition: Kuka.hpp:131
boost::asio::io_service device_driver_io_service
const char ** EnumNamesArmState()
Kuka LBR iiwa Primary Multi Mode Driver, supports communication over FRI and JAVA interfaces...
Definition: KukaDriver.hpp:41
std::tuple< JointScalar, JointScalar, JointScalar, JointScalar, JointScalar, TransformationMatrices, robot::arm::KukaJAVAdriver::JointStateTag > State
const int NUM_DOF
Definition: Kuka.hpp:19
void mode_callback(const std_msgs::StringConstPtr &msg)
ROS callback to set current interaction mode; determines whether commands will be send in SERVO...
KukaLBRiiwaROSPlugin(Params params=defaultParams())
std::unique_ptr< boost::asio::io_service::work > device_driver_workP_
std::unique_ptr< std::thread > driver_threadP
std::tuple< std::string, std::string, std::string, std::string, std::string, std::string, std::string, std::string, std::string, std::string > Params
KukaLBRiiwaROSPlugin::TransformationMatrices simJointTransformationMatrix
std::vector< TransformationMatrix > TransformationMatrices