KukaLBRiiwaROSPlugin_BACKUP_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 #include <geometry_msgs/Wrench.h>
20 
21 #include <cartesian_impedance_msgs/SetCartesianImpedance.h>
22 #include <cartesian_impedance_msgs/SetCartesianForceCtrl.h>
23 
24 #include "grl/kuka/KukaDriver.hpp"
28 <<<<<<< HEAD
29 =======
30 
31 //this is for conversion fom meters to mm
32 #define meters_to_mm 1000
33 >>>>>>> force_control
34 
35 
36 /// @todo move elsewhere, because it will conflict with others' implementations of outputting vectors
37 template<typename T>
38 inline boost::log::formatting_ostream& operator<<(boost::log::formatting_ostream& out, std::vector<T>& v)
39 {
40  out << "[";
41  size_t last = v.size() - 1;
42  for(size_t i = 0; i < v.size(); ++i) {
43  out << v[i];
44  if (i != last)
45  out << ", ";
46  }
47  out << "]";
48  return out;
49 }
50 
51 namespace grl {
52 
53  namespace ros {
54 
55  /**
56  *
57  * This class contains code to offer a simple communication layer between ROS and the KUKA LBR iiwa
58  *
59  * @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());
60  */
61  class KukaLBRiiwaROSPlugin : public std::enable_shared_from_this<KukaLBRiiwaROSPlugin>
62  {
63  public:
64 
65  enum ParamIndex {
66  RobotName,
67  RobotModel,
76  };
77 
78  typedef std::tuple<
79  std::string,
80  std::string,
81  std::string,
82  std::string,
83  std::string,
84  std::string,
85  std::string,
86  std::string,
87  std::string,
88  std::string
89  > Params;
90 
91 
92  static const Params defaultParams(){
93  return std::make_tuple(
94  "Robotiiwa" , // RobotName,
95  "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800)
96  "172.31.1.101" , // LocalZMQAddress // only insert ip, not the port
97  "tcp://172.31.1.147:30010", // RemoteZMQAddress
98  "192.170.10.100" , // LocalHostKukaKoniUDPAddress,
99  "30200" , // LocalHostKukaKoniUDPPort,
100  "192.170.10.2" , // RemoteHostKukaKoniUDPAddress,
101  "30200" , // RemoteHostKukaKoniUDPPort
102  "JAVA" , // KukaCommandMode (options are FRI, JAVA)
103  "FRI" // KukaMonitorMode (options are FRI, JAVA)
104  );
105  }
106 
108  ::ros::NodeHandle nh_tilde("~");
109 
110  nh_tilde.getParam("RobotName",std::get<RobotName>(params));
111  nh_tilde.getParam("RobotModel",std::get<RobotModel>(params));
112  nh_tilde.getParam("LocalZMQAddress",std::get<LocalZMQAddress>(params));
113  nh_tilde.getParam("RemoteZMQAddress",std::get<RemoteZMQAddress>(params));
114  nh_tilde.getParam("LocalHostKukaKoniUDPAddress",std::get<LocalHostKukaKoniUDPAddress>(params));
115  nh_tilde.getParam("LocalHostKukaKoniUDPPort",std::get<LocalHostKukaKoniUDPPort>(params));
116  nh_tilde.getParam("RemoteHostKukaKoniUDPAddress",std::get<RemoteHostKukaKoniUDPAddress>(params));
117  nh_tilde.getParam("RemoteHostKukaKoniUDPPort",std::get<RemoteHostKukaKoniUDPPort>(params));
118  nh_tilde.getParam("KukaCommandMode",std::get<KukaCommandMode>(params));
119  nh_tilde.getParam("KukaMonitorMode",std::get<KukaMonitorMode>(params));
120 
121  return params;
122  }
123 
124  /// unique tag type so State never
125  /// conflicts with a similar tuple
126  struct JointStateTag{};
127 
130  JointForce,
134  JointMatrix,
136  };
137 
138 
139  typedef std::vector<double> JointScalar;
140 
141  /// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simGetJointMatrix for data layout information
142  typedef std::array<double,12> TransformationMatrix;
143  typedef std::vector<TransformationMatrix> TransformationMatrices;
144 
145  typedef std::tuple<
146  JointScalar, // jointPosition
147  // JointScalar // JointVelocity // no velocity yet
148  JointScalar, // jointForce
149  JointScalar, // jointTargetPosition
150  JointScalar, // JointLowerPositionLimit
151  JointScalar, // JointUpperPositionLimit
152  TransformationMatrices, // jointTransformation
153  robot::arm::KukaJAVAdriver::JointStateTag // JointStateTag unique identifying type so tuple doesn't conflict
154  > State;
155 
156 
158  : debug(false),params_(params), nh_("")
159  {
160  loadRosParams(params_);
161  }
162 
163  void construct(){ construct(params_);}
164 
165  /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails
166  /// @warning getting the ik group is optional, so it does not throw an exception
167  void construct(Params params) {
168 
169  current_js_.name.resize(7);
170  current_js_.name[0] = "iiwa_joint_1";
171  current_js_.name[1] = "iiwa_joint_2";
172  current_js_.name[2] = "iiwa_joint_3";
173  current_js_.name[3] = "iiwa_joint_4";
174  current_js_.name[4] = "iiwa_joint_5";
175  current_js_.name[5] = "iiwa_joint_6";
176  current_js_.name[6] = "iiwa_joint_7";
177  current_js_.velocity.resize(7);
178  current_js_.velocity[0] = 0.;
179  current_js_.velocity[1] = 0.;
180  current_js_.velocity[2] = 0.;
181  current_js_.velocity[3] = 0.;
182  current_js_.velocity[4] = 0.;
183  current_js_.velocity[5] = 0.;
184  current_js_.velocity[6] = 0.;
185 
186  ::ros::NodeHandle nh;
187  js_pub_ = nh.advertise<sensor_msgs::JointState>("joint_states",100);
188  jt_sub_ = nh.subscribe<trajectory_msgs::JointTrajectory>("joint_traj_cmd", 1000, &KukaLBRiiwaROSPlugin::jt_callback, this);
189  jt_pt_sub_ = nh.subscribe<trajectory_msgs::JointTrajectoryPoint>("joint_traj_pt_cmd", 1000, &KukaLBRiiwaROSPlugin::jt_pt_callback, this);
190  wrench_pub_ = nh.advertise<geometry_msgs::Wrench>("slave/wrench_r",100);
191  mode_sub_ = nh.subscribe<std_msgs::String>("interaction_mode", 1000, &KukaLBRiiwaROSPlugin::mode_callback, this);
192 
193  dummy_sub_ = nh.subscribe<std_msgs::String>("dummy_topic", 1000, &KukaLBRiiwaROSPlugin::dummy_callback, this);
194  cart_imp_sub_ = nh.subscribe<cartesian_impedance_msgs::SetCartesianImpedance>("set_cartesian_impedance_params", 1000, &KukaLBRiiwaROSPlugin::set_cartesian_impedance_callback, this);
195  cart_ft_sub_ = nh.subscribe<cartesian_impedance_msgs::SetCartesianForceCtrl>("set_cartesian_ft_params", 1000, &KukaLBRiiwaROSPlugin::set_force_control_callback, this);
196 
197  ROS_INFO("done creating subscribers");
198  //jt_sub_ = nh.subscribe<trajectory_msgs::JointTrajectory>("joint_traj_cmd",1000,boost::bind(&KukaLBRiiwaROSPlugin::jt_callback, this, _1));
199 
200  params_ = params;
201  // keep driver threads from exiting immediately after creation, because they have work to do!
202  device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service));
203 
204  /// @todo properly support passing of io_service
205  KukaDriverP_.reset(
207  //device_driver_io_service,
208  params
209  // std::make_tuple(
210  // std::string(std::std::get<LocalHostKukaKoniUDPAddress > (params)),
211  // std::string(std::std::get<LocalHostKukaKoniUDPPort > (params)),
212  // std::string(std::std::get<RemoteHostKukaKoniUDPAddress> (params)),
213  // std::string(std::std::get<RemoteHostKukaKoniUDPPort > (params)),
214  // grl::robot::arm::KukaFRIClientDataDriver::run_automatically
215  // )
216  )
217 
218  );
219  KukaDriverP_->construct();
220  }
221 
222 
223 
224 
225  bool setState(State& state) { return true; }
226 
227 
228  const Params & getParams(){
229  return params_;
230  }
231 
232  /**
233  * ROS joint trajectory callback
234  * this code needs to execute the joint trajectory on the robot
235  */
236  void jt_callback(const trajectory_msgs::JointTrajectoryConstPtr &msg) {
237  boost::lock_guard<boost::mutex> lock(jt_mutex);
238 
239 
240  for(auto &pt: msg->points) {
241  // positions velocities ac
242  if (pt.positions.size() != KUKA::LBRState::NUM_DOF) {
243  BOOST_THROW_EXCEPTION(std::runtime_error("Malformed joint trajectory request! Wrong number of joints."));
244  }
245 
246  // handle
247  //simJointPosition
248  simJointPosition.clear();
249  boost::copy(pt.positions,std::back_inserter(simJointPosition));
250  //simJointVelocity
251  simJointVelocity.clear();
252  boost::copy(pt.velocities,std::back_inserter(simJointVelocity));
253  //simJointForce
254  simJointForce.clear();
255  boost::copy(pt.effort,std::back_inserter(simJointForce));
256 
257  ///@todo: execute the rest of the trajectory
258  break;
259  }
260 
261  }
262 
263  /// ROS callback to set current interaction mode; determines whether commands will be send in SERVO, TEACH, etc
264  void mode_callback(const std_msgs::StringConstPtr &msg) {
265  boost::lock_guard<boost::mutex> lock(jt_mutex);
266  simJointPosition.clear();
267  // boost::copy(current_js_.position,std::back_inserter(simJointPosition));
268  // if(simJointPosition.size()) KukaDriverP_->set( simJointPosition, grl::revolute_joint_angle_open_chain_command_tag());
269 
270  // simJointPosition.clear();
271  // //simJointVelocity
272  // simJointVelocity.clear();
273  // //simJointForce
274  // simJointForce.clear();
275 
276  //std::cerr << "Mode command = " << msg->data.c_str() << "\n";
277  if (debug) {
278  ROS_INFO("Receiving mode command: %s", msg->data.c_str());
279  }
280 
281  unsigned int ArmStateLen = 9;
282  for (unsigned int i = 0; i < ArmStateLen; ++i) {
283  if (msg->data == grl::flatbuffer::EnumNamesArmState()[i]) {
284 
285  if (debug) {
286  std::string info = std::string("Valid grl::flatbuffer::ArmState command received for ") + grl::flatbuffer::EnumNamesArmState()[i] + std::string(" mode");
287  ROS_INFO(info.c_str());
288  }
289 
290  interaction_mode = static_cast<grl::flatbuffer::ArmState>(i);
291  KukaDriverP_->set(interaction_mode);
292  break;
293  }
294  }
295 
296  }
297  void dummy_callback(const std_msgs::StringConstPtr &msg)
298  {
299  boost::lock_guard<boost::mutex> lock(jt_mutex);
300 
301  ROS_INFO("Dummy message set!");
302  KukaDriverP_->set(msg->data);
303  //KukaDriverP_->set();
304 
305  }
306  void set_cartesian_impedance_callback(const cartesian_impedance_msgs::SetCartesianImpedanceConstPtr &cartImpedance)
307  {
308  boost::lock_guard<boost::mutex> lock(jt_mutex);
309  ROS_INFO("Changing Cartesian Impedance Parameters");
310  //
311  // simJointPosition.clear();
312  // boost::copy(current_js_.position,std::back_inserter(simJointPosition));
313  // if(simJointPosition.size()) KukaDriverP_->set( simJointPosition, grl::revolute_joint_angle_open_chain_command_tag());
314 
315  //Cartesian stiffness msg values to the flatbuffers
316  grl::flatbuffer::Vector3d cart_stifness_trans(cartImpedance->stiffness.translational.x,cartImpedance->stiffness.translational.y,cartImpedance->stiffness.translational.z);
317  grl::flatbuffer::EulerRotation cart_stifness_rot(cartImpedance->stiffness.rotational.x,cartImpedance->stiffness.rotational.y,cartImpedance->stiffness.rotational.z,grl::flatbuffer::EulerOrder_xyz);
318 
319  KukaDriverP_->set(cart_stifness_trans,cart_stifness_rot,cart_stiffness_values());
320 
321 
322  //Cartesian damping values pass to the flatbuffers
323  grl::flatbuffer::Vector3d cart_damping_trans(cartImpedance->damping.translational.x,cartImpedance->damping.translational.y,cartImpedance->damping.translational.z);
324  grl::flatbuffer::EulerRotation cart_damping_rot(cartImpedance->damping.rotational.x,cartImpedance->damping.rotational.y,cartImpedance->damping.rotational.z,grl::flatbuffer::EulerOrder_xyz);
325 
326  KukaDriverP_->set(cart_damping_trans,cart_damping_rot,cart_damping_values());
327 
328  //Max Cartesian Path deviation when in cartesian impedance
329  //Cartesian max Cartesian velocities [m] for translation and [rad] for rotation
330  //iiwa expects the values to be in mm/s for *cart_max_path_deviation*
331  grl::flatbuffer::Vector3d cart_max_path_dev_trans(cartImpedance->max_path_deviation.translation.x*meters_to_mm,cartImpedance->max_path_deviation.translation.y*meters_to_mm,cartImpedance->max_path_deviation.translation.z*meters_to_mm);
332  grl::flatbuffer::EulerRotation cart_max_path_dev_vel_rot(cartImpedance->max_path_deviation.rotation.x,cartImpedance->max_path_deviation.rotation.y,cartImpedance->max_path_deviation.rotation.z,grl::flatbuffer::EulerOrder_xyz);
333 
334 
335  grl::flatbuffer::EulerPose cart_max_path_deviation(cart_max_path_dev_trans,cart_max_path_dev_vel_rot);
336  KukaDriverP_->set(cart_max_path_deviation,max_path_deviation());
337 
338 
339  //Max Cartesian velocities [m/s] for translation and [rad/s] for rotation
340  // iiwa expect the values to be mm/s for the *cart_max_ctrl_vel_trans*
341  grl::flatbuffer::Vector3d cart_max_ctrl_vel_trans(cartImpedance->max_cart_vel.set.linear.x*meters_to_mm,cartImpedance->max_cart_vel.set.linear.y*meters_to_mm,cartImpedance->max_cart_vel.set.linear.z*meters_to_mm);
342  grl::flatbuffer::EulerRotation cart_max_ctrl_vel_rot(cartImpedance->max_cart_vel.set.angular.x,cartImpedance->max_cart_vel.set.angular.y,cartImpedance->max_cart_vel.set.angular.z,grl::flatbuffer::EulerOrder_xyz);
343 
344  grl::flatbuffer::EulerPose cart_max_ctrl_vel(cart_max_ctrl_vel_trans,cart_max_ctrl_vel_rot);
345  KukaDriverP_->set(cart_max_ctrl_vel,max_cart_vel());
346 
347  //Max Control Force [N] for translation and [Nm] for rotation
348  grl::flatbuffer::Vector3d cart_max_ctrl_force_trans(cartImpedance->max_ctrl_force.set.force.x,cartImpedance->max_ctrl_force.set.force.y,cartImpedance->max_ctrl_force.set.force.z);
349  grl::flatbuffer::EulerRotation cart_max_ctrl_force_rot(cartImpedance->max_ctrl_force.set.torque.x,cartImpedance->max_ctrl_force.set.torque.y,cartImpedance->max_ctrl_force.set.torque.z,grl::flatbuffer::EulerOrder_xyz);
350 
351  grl::flatbuffer::EulerPose cart_max_ctrl_force(cart_max_ctrl_force_trans,cart_max_ctrl_force_rot);
352 
353  // //send position after state change
354  // simJointPosition.clear();
355  // boost::copy(current_js_.position,std::back_inserter(simJointPosition));
356  // if(simJointPosition.size()) KukaDriverP_->set( simJointPosition, grl::revolute_joint_angle_open_chain_command_tag());
357 
358  KukaDriverP_->set(cart_max_ctrl_force,max_ctrl_force());
359 
360  double nullspaceStiffness;
361  double nullspaceDamping;
362 
363  nullspaceDamping = cartImpedance->null_space_params.damping[0];
364  nullspaceStiffness = cartImpedance->null_space_params.stiffness[0];
365 
366  // simJointPosition.clear();
367  // boost::copy(current_js_.position,std::back_inserter(simJointPosition));
368  // if(simJointPosition.size()) KukaDriverP_->set( simJointPosition, grl::revolute_joint_angle_open_chain_command_tag());
369 
370  KukaDriverP_->set(nullspaceStiffness,nullspaceDamping,null_space_params());
371 
372  // simJointPosition.clear();
373  // boost::copy(current_js_.position,std::back_inserter(simJointPosition));
374  // if(simJointPosition.size()) KukaDriverP_->set( simJointPosition, grl::revolute_joint_angle_open_chain_command_tag());
375 
376 
377 
378  }
379  void set_force_control_callback(const cartesian_impedance_msgs::SetCartesianForceCtrlConstPtr &cartFTCtrl)
380  {
381  boost::lock_guard<boost::mutex> lock(jt_mutex);
382  std::string DOF;
383  double force;
384  double stiffness;
385 
386  // simJointPosition.clear();
387  // boost::copy(current_js_.position,std::back_inserter(simJointPosition));
388  // if(simJointPosition.size()) KukaDriverP_->set( simJointPosition, grl::revolute_joint_angle_open_chain_command_tag());
389 
390  KukaDriverP_->set(cartFTCtrl->DOF.c_str(),cartFTCtrl->force,cartFTCtrl->stiffness,set_const_ctrl_force());
391 
392 
393  // simJointPosition.clear();
394  // boost::copy(current_js_.position,std::back_inserter(simJointPosition));
395  // if(simJointPosition.size()) KukaDriverP_->set( simJointPosition, grl::revolute_joint_angle_open_chain_command_tag());
396 
397  //ROS_INFO("Set Cartesian Constant Force/torqe Control; %s",cartFTCtrl->DOF.c_str());
398  }
399 
400  /// ROS joint trajectory callback
401  /// this code needs to execute the joint trajectory on the robot
402  void jt_pt_callback(const trajectory_msgs::JointTrajectoryPointConstPtr &msg) {
403  boost::lock_guard<boost::mutex> lock(jt_mutex);
404 
405  // positions velocities ac
406  if (msg->positions.size() != KUKA::LBRState::NUM_DOF) {
407  BOOST_THROW_EXCEPTION(std::runtime_error("Malformed joint trajectory request! Wrong number of joints."));
408  }
409 
410  // handle
411  //simJointPosition
412  simJointPosition.clear();
413  boost::copy(msg->positions,std::back_inserter(simJointPosition));
414  //simJointVelocity
415  simJointVelocity.clear();
416  boost::copy(msg->velocities,std::back_inserter(simJointVelocity));
417  //simJointForce
418  simJointForce.clear();
419  boost::copy(msg->effort,std::back_inserter(simJointForce));
420 
421  ROS_INFO("Im in Joint trajectory msgs!!");
422 
423  ///@todo: execute the rest of the trajectory
424  }
425 
426 
428  device_driver_workP_.reset();
429 
430  if(driver_threadP){
432  driver_threadP->join();
433  }
434  }
435 
436  ///
437  /// @brief spin once, call this repeatedly to run the driver
438  ///
439  ///
440  bool run_one()
441  {
442 
443  bool haveNewData = false;
444 
445  if(KukaDriverP_){
446 
447 
448  switch(interaction_mode) {
450  if (debug) {
451  ROS_INFO("Arm is in SERVO Mode");
452  }
454  /// @todo setting joint position clears joint force in KukaDriverP_. Is this right or should position and force be configurable simultaeously?
455  //if(simJointForce.size()) KukaDriverP_->set( simJointForce, grl::revolute_joint_torque_open_chain_command_tag());
456  break;
458  if (debug) {
459  ROS_INFO("Arm is in TEACH mode");
460  }
461  break;
462  break;
464  break;
466  break;
468  if (debug) {
469  ROS_INFO("Sending start!");
470  }
471  break;
473  break;
474  default:
475  if (debug) {
476  ROS_INFO("KukaLBRiiwaROSPlugin in unsupported mode!");
477  }
478 
479  }
480 
481  haveNewData = KukaDriverP_->run_one();
482 
483  if(haveNewData)
484  {
485 
486  // We have the real kuka state read from the device now
487  // update real joint angle data
488  current_js_.position.clear();
489  KukaDriverP_->get(std::back_inserter(current_js_.position), grl::revolute_joint_angle_open_chain_state_tag());
490 
491  current_js_.effort.clear();
492  KukaDriverP_->get(std::back_inserter(current_js_.effort), grl::revolute_joint_torque_open_chain_state_tag());
493 
494  //current_js_.velocity.clear();
495  //grl::robot::arm::copy(friData_->monitoringMsg, std::back_inserter(current_js_.velocity), grl::revolute_joint_angle_open_chain_state_tag());
496  current_js_.header.stamp = ::ros::Time::now();
497  current_js_.header.seq += 1;
498  js_pub_.publish(current_js_);
499  }
500 
501  if(haveNewData)
502  {
503  std::vector<double> wrench_vector;
504  KukaDriverP_->getWrench(std::back_inserter(wrench_vector));
505  if (!wrench_vector.empty())
506  {
507  current_wrench.force.x = wrench_vector[0];
508  current_wrench.force.y = wrench_vector[1];
509  current_wrench.force.z = wrench_vector[2];
510  current_wrench.torque.x = wrench_vector[3];
511  current_wrench.torque.y = wrench_vector[4];
512  current_wrench.torque.z = wrench_vector[5];
513  }
514  wrench_pub_.publish(current_wrench);
515  }
516 
517  }
518 
519  return haveNewData;
520  }
521 
522  boost::asio::io_service device_driver_io_service;
523  std::unique_ptr<boost::asio::io_service::work> device_driver_workP_;
524  std::unique_ptr<std::thread> driver_threadP;
525 
526  /// @todo replace all these simJoint elements with simple KukaLBRiiwaROSPlugin::State
527  std::vector<double> simJointPosition;// = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
528  std::vector<double> simJointVelocity = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
529  std::vector<double> simJointForce;// = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
530  std::vector<double> simJointTargetPosition = {0.0,0.0,0.0,0.0,0.0,0.0,0.0};
532 
533  /// @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.
534  std::vector<double> realJointPosition = { 0, 0, 0, 0, 0, 0, 0 };
535  // does not exist
536  std::vector<double> realJointForce = { 0, 0, 0, 0, 0, 0, 0 };
537 
538  std::string dummy_message;
539 
540  private:
541 
542  bool debug;
543  grl::flatbuffer::ArmState interaction_mode;
544  // grl::flatbuffer::KUKAiiwaArmConfiguration cart_impedance;
545 
546  boost::mutex jt_mutex;
547  boost::shared_ptr<robot::arm::KukaDriver> KukaDriverP_;
548  Params params_;
549 
550  ::ros::Subscriber jt_sub_; // subscribes to joint state trajectories and executes them
551  ::ros::Subscriber jt_pt_sub_; // subscribes to joint state trajectories and executes them
552  ::ros::Subscriber mode_sub_; // subscribes to interaction mode messages (strings for now)
553  ::ros::Subscriber dummy_sub_;
554  ::ros::Subscriber cart_imp_sub_;
555  ::ros::Subscriber cart_ft_sub_;
556 
557  ::ros::Publisher js_pub_; // publish true joint states from the KUKA
558  ::ros::Publisher wrench_pub_;
559 
560  sensor_msgs::JointState current_js_;
561  geometry_msgs::Wrench current_wrench;
562 
563  ::ros::NodeHandle nh_;
564 
565 
566  };
567  }
568 }
569 
570 
571 #endif
void set_force_control_callback(const cartesian_impedance_msgs::SetCartesianForceCtrlConstPtr &cartFTCtrl)
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
#define meters_to_mm
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_
void set_cartesian_impedance_callback(const cartesian_impedance_msgs::SetCartesianImpedanceConstPtr &cartImpedance)
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
void dummy_callback(const std_msgs::StringConstPtr &msg)