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> 27 inline boost::log::formatting_ostream& operator<<(boost::log::formatting_ostream& out, std::vector<T>& v)
30 size_t last = v.size() - 1;
31 for(
size_t i = 0; i < v.size(); ++i) {
50 class KukaLBRiiwaROSPlugin :
public std::enable_shared_from_this<KukaLBRiiwaROSPlugin>
82 return std::make_tuple(
84 "KUKA_LBR_IIWA_14_R820" ,
85 "tcp://0.0.0.0:30010" ,
86 "tcp://172.31.1.147:30010",
97 ::ros::NodeHandle nh_tilde(
"~");
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));
147 : debug(false),params_(params), nh_(
"")
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.;
175 ::ros::NodeHandle nh;
176 js_pub_ = nh.advertise<sensor_msgs::JointState>(
"joint_states",100);
180 ROS_INFO(
"done creating subscribers");
202 KukaDriverP_->construct();
219 void jt_callback(
const trajectory_msgs::JointTrajectoryConstPtr &msg) {
220 boost::lock_guard<boost::mutex> lock(jt_mutex);
223 for(
auto &pt: msg->points) {
226 BOOST_THROW_EXCEPTION(std::runtime_error(
"Malformed joint trajectory request! Wrong number of joints."));
249 boost::lock_guard<boost::mutex> lock(jt_mutex);
253 ROS_INFO(
"Receiving mode command: %s", msg->data.c_str());
256 unsigned int ArmStateLen = 9;
257 for (
unsigned int i = 0; i < ArmStateLen; ++i) {
262 ROS_INFO(info.c_str());
266 KukaDriverP_->set(interaction_mode);
276 boost::lock_guard<boost::mutex> lock(jt_mutex);
281 BOOST_THROW_EXCEPTION(std::runtime_error(
"Malformed joint trajectory request! Wrong number of joints."));
315 bool haveNewData =
false;
320 switch(interaction_mode) {
323 ROS_INFO(
"Arm is in SERVO Mode");
331 ROS_INFO(
"Arm is in TEACH mode");
341 ROS_INFO(
"Sending start!");
347 ROS_INFO(
"KukaLBRiiwaROSPlugin in unsupported mode!");
350 haveNewData = KukaDriverP_->run_one();
357 current_js_.position.clear();
360 current_js_.effort.clear();
366 current_js_.header.stamp = ::ros::Time::now();
367 current_js_.header.seq += 1;
368 js_pub_.publish(current_js_);
397 boost::mutex jt_mutex;
398 boost::shared_ptr<robot::arm::KukaDriver> KukaDriverP_;
401 ::ros::Subscriber jt_sub_;
402 ::ros::Subscriber jt_pt_sub_;
403 ::ros::Subscriber mode_sub_;
405 ::ros::Publisher js_pub_;
408 sensor_msgs::JointState current_js_;
410 ::ros::NodeHandle nh_;
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::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_
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)