1 #ifndef GRL_ROS_BRIDGE_HPP_ 2 #define GRL_ROS_BRIDGE_HPP_ 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> 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> 21 #include <cartesian_impedance_msgs/SetCartesianImpedance.h> 22 #include <cartesian_impedance_msgs/SetCartesianForceCtrl.h> 32 #define meters_to_mm 1000 38 inline boost::log::formatting_ostream& operator<<(boost::log::formatting_ostream& out, std::vector<T>& v)
41 size_t last = v.size() - 1;
42 for(
size_t i = 0; i < v.size(); ++i) {
61 class KukaLBRiiwaROSPlugin :
public std::enable_shared_from_this<KukaLBRiiwaROSPlugin>
93 return std::make_tuple(
95 "KUKA_LBR_IIWA_14_R820" ,
97 "tcp://172.31.1.147:30010",
108 ::ros::NodeHandle nh_tilde(
"~");
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));
158 : debug(false),params_(params), nh_(
"")
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.;
186 ::ros::NodeHandle nh;
187 js_pub_ = nh.advertise<sensor_msgs::JointState>(
"joint_states",100);
190 wrench_pub_ = nh.advertise<geometry_msgs::Wrench>(
"slave/wrench_r",100);
197 ROS_INFO(
"done creating subscribers");
219 KukaDriverP_->construct();
236 void jt_callback(
const trajectory_msgs::JointTrajectoryConstPtr &msg) {
237 boost::lock_guard<boost::mutex> lock(jt_mutex);
240 for(
auto &pt: msg->points) {
243 BOOST_THROW_EXCEPTION(std::runtime_error(
"Malformed joint trajectory request! Wrong number of joints."));
265 boost::lock_guard<boost::mutex> lock(jt_mutex);
278 ROS_INFO(
"Receiving mode command: %s", msg->data.c_str());
281 unsigned int ArmStateLen = 9;
282 for (
unsigned int i = 0; i < ArmStateLen; ++i) {
287 ROS_INFO(info.c_str());
291 KukaDriverP_->set(interaction_mode);
299 boost::lock_guard<boost::mutex> lock(jt_mutex);
301 ROS_INFO(
"Dummy message set!");
302 KukaDriverP_->set(msg->data);
308 boost::lock_guard<boost::mutex> lock(jt_mutex);
309 ROS_INFO(
"Changing Cartesian Impedance Parameters");
316 grl::flatbuffer::Vector3d cart_stifness_trans(cartImpedance->stiffness.translational.x,cartImpedance->stiffness.translational.y,cartImpedance->stiffness.translational.z);
319 KukaDriverP_->set(cart_stifness_trans,cart_stifness_rot,cart_stiffness_values());
323 grl::flatbuffer::Vector3d cart_damping_trans(cartImpedance->damping.translational.x,cartImpedance->damping.translational.y,cartImpedance->damping.translational.z);
326 KukaDriverP_->set(cart_damping_trans,cart_damping_rot,cart_damping_values());
336 KukaDriverP_->set(cart_max_path_deviation,max_path_deviation());
345 KukaDriverP_->set(cart_max_ctrl_vel,max_cart_vel());
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);
358 KukaDriverP_->set(cart_max_ctrl_force,max_ctrl_force());
360 double nullspaceStiffness;
361 double nullspaceDamping;
363 nullspaceDamping = cartImpedance->null_space_params.damping[0];
364 nullspaceStiffness = cartImpedance->null_space_params.stiffness[0];
370 KukaDriverP_->set(nullspaceStiffness,nullspaceDamping,null_space_params());
381 boost::lock_guard<boost::mutex> lock(jt_mutex);
390 KukaDriverP_->set(cartFTCtrl->DOF.c_str(),cartFTCtrl->force,cartFTCtrl->stiffness,set_const_ctrl_force());
403 boost::lock_guard<boost::mutex> lock(jt_mutex);
407 BOOST_THROW_EXCEPTION(std::runtime_error(
"Malformed joint trajectory request! Wrong number of joints."));
421 ROS_INFO(
"Im in Joint trajectory msgs!!");
443 bool haveNewData =
false;
448 switch(interaction_mode) {
451 ROS_INFO(
"Arm is in SERVO Mode");
459 ROS_INFO(
"Arm is in TEACH mode");
469 ROS_INFO(
"Sending start!");
476 ROS_INFO(
"KukaLBRiiwaROSPlugin in unsupported mode!");
481 haveNewData = KukaDriverP_->run_one();
488 current_js_.position.clear();
491 current_js_.effort.clear();
496 current_js_.header.stamp = ::ros::Time::now();
497 current_js_.header.seq += 1;
498 js_pub_.publish(current_js_);
503 std::vector<double> wrench_vector;
504 KukaDriverP_->getWrench(std::back_inserter(wrench_vector));
505 if (!wrench_vector.empty())
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];
514 wrench_pub_.publish(current_wrench);
546 boost::mutex jt_mutex;
547 boost::shared_ptr<robot::arm::KukaDriver> KukaDriverP_;
550 ::ros::Subscriber jt_sub_;
551 ::ros::Subscriber jt_pt_sub_;
552 ::ros::Subscriber mode_sub_;
553 ::ros::Subscriber dummy_sub_;
554 ::ros::Subscriber cart_imp_sub_;
555 ::ros::Subscriber cart_ft_sub_;
557 ::ros::Publisher js_pub_;
558 ::ros::Publisher wrench_pub_;
560 sensor_msgs::JointState current_js_;
561 geometry_msgs::Wrench current_wrench;
563 ::ros::NodeHandle nh_;
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
bool setState(State &state)
std::string dummy_message
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
boost::asio::io_service device_driver_io_service
const char ** EnumNamesArmState()
std::vector< double > JointScalar
std::vector< double > realJointPosition
static const Params defaultParams()
Kuka LBR iiwa Primary Multi Mode Driver, supports communication over FRI and JAVA interfaces...
std::tuple< JointScalar, JointScalar, JointScalar, JointScalar, JointScalar, TransformationMatrices, robot::arm::KukaJAVAdriver::JointStateTag > State
std::array< double, 12 > TransformationMatrix
void mode_callback(const std_msgs::StringConstPtr &msg)
ROS callback to set current interaction mode; determines whether commands will be send in SERVO...
void construct(Params params)
const Params & getParams()
std::vector< double > simJointPosition
KukaLBRiiwaROSPlugin(Params params=defaultParams())
std::vector< double > realJointForce
std::vector< double > simJointVelocity
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
std::vector< double > simJointForce
Params & loadRosParams(Params ¶ms)
void dummy_callback(const std_msgs::StringConstPtr &msg)