kuka_eki_hw_interface.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, 3M
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the copyright holder, nor the names of its
16  * contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 // Author: Brett Hemes (3M) <brhemes@mmm.com>
33 
34 
35 #include <boost/array.hpp>
36 #include <boost/bind.hpp>
37 #include <boost/date_time/posix_time/posix_time_types.hpp>
38 
39 #include <angles/angles.h>
40 
41 #include <tinyxml.h>
42 
44 
45 
46 namespace kuka_eki_hw_interface
47 {
48 
49 KukaEkiHardwareInterface::KukaEkiHardwareInterface() : joint_position_(n_dof_, 0.0), joint_velocity_(n_dof_, 0.0),
50  joint_effort_(n_dof_, 0.0), joint_position_command_(n_dof_, 0.0), joint_names_(n_dof_), deadline_(ios_),
51  eki_server_socket_(ios_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0))
52 {
53 
54 }
55 
56 
58 
59 
61 {
62  // Check if deadline has already passed
63  if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now())
64  {
65  eki_server_socket_.cancel();
66  deadline_.expires_at(boost::posix_time::pos_infin);
67  }
68 
69  // Sleep until deadline exceeded
71 }
72 
73 
74 void KukaEkiHardwareInterface::eki_handle_receive(const boost::system::error_code &ec, size_t length,
75  boost::system::error_code* out_ec, size_t* out_length)
76 {
77  *out_ec = ec;
78  *out_length = length;
79 }
80 
81 
82 bool KukaEkiHardwareInterface::eki_read_state(std::vector<double> &joint_position,
83  std::vector<double> &joint_velocity,
84  std::vector<double> &joint_effort,
85  int &cmd_buff_len)
86 {
87  static boost::array<char, 2048> in_buffer;
88 
89  // Read socket buffer (with timeout)
90  // Based off of Boost documentation example: doc/html/boost_asio/example/timeouts/blocking_udp_client.cpp
91  deadline_.expires_from_now(boost::posix_time::seconds(eki_read_state_timeout_)); // set deadline
92  boost::system::error_code ec = boost::asio::error::would_block;
93  size_t len = 0;
94  eki_server_socket_.async_receive(boost::asio::buffer(in_buffer),
95  boost::bind(&KukaEkiHardwareInterface::eki_handle_receive, _1, _2, &ec, &len));
96  do
97  ios_.run_one();
98  while (ec == boost::asio::error::would_block);
99  if (ec)
100  return false;
101 
102  // Update joint positions from XML packet (if received)
103  if (len == 0)
104  return false;
105 
106  // Parse XML
107  TiXmlDocument xml_in;
108  in_buffer[len] = '\0'; // null-terminate data buffer for parsing (expects c-string)
109  xml_in.Parse(in_buffer.data());
110  TiXmlElement* robot_state = xml_in.FirstChildElement("RobotState");
111  if (!robot_state)
112  return false;
113  TiXmlElement* pos = robot_state->FirstChildElement("Pos");
114  TiXmlElement* vel = robot_state->FirstChildElement("Vel");
115  TiXmlElement* eff = robot_state->FirstChildElement("Eff");
116  TiXmlElement* robot_command = robot_state->FirstChildElement("RobotCommand");
117  if (!pos || !vel || !eff || !robot_command)
118  return false;
119 
120  // Extract axis positions
121  double joint_pos; // [deg]
122  double joint_vel; // [%max]
123  double joint_eff; // [Nm]
124  char axis_name[] = "A1";
125  for (int i = 0; i < n_dof_; ++i)
126  {
127  pos->Attribute(axis_name, &joint_pos);
128  joint_position[i] = angles::from_degrees(joint_pos); // convert deg to rad
129  vel->Attribute(axis_name, &joint_vel);
130  joint_velocity[i] = joint_vel;
131  eff->Attribute(axis_name, &joint_eff);
132  joint_effort[i] = joint_eff;
133  axis_name[1]++;
134  }
135 
136  // Extract number of command elements buffered on robot
137  robot_command->Attribute("Size", &cmd_buff_len);
138 
139  return true;
140 }
141 
142 
143 bool KukaEkiHardwareInterface::eki_write_command(const std::vector<double> &joint_position_command)
144 {
145  TiXmlDocument xml_out;
146  TiXmlElement* robot_command = new TiXmlElement("RobotCommand");
147  TiXmlElement* pos = new TiXmlElement("Pos");
148  TiXmlText* empty_text = new TiXmlText("");
149  robot_command->LinkEndChild(pos);
150  pos->LinkEndChild(empty_text); // force <Pos></Pos> format (vs <Pos />)
151  char axis_name[] = "A1";
152  for (int i = 0; i < n_dof_; ++i)
153  {
154  pos->SetAttribute(axis_name, std::to_string(angles::to_degrees(joint_position_command[i])).c_str());
155  axis_name[1]++;
156  }
157  xml_out.LinkEndChild(robot_command);
158 
159  TiXmlPrinter xml_printer;
160  xml_printer.SetStreamPrinting(); // no linebreaks
161  xml_out.Accept(&xml_printer);
162 
163  size_t len = eki_server_socket_.send_to(boost::asio::buffer(xml_printer.CStr(), xml_printer.Size()),
165 
166  return true;
167 }
168 
169 
171 {
172  // Get controller joint names from parameter server
173  if (!nh_.getParam("controller_joint_names", joint_names_))
174  {
175  ROS_ERROR("Cannot find required parameter 'controller_joint_names' on the parameter server.");
176  throw std::runtime_error("Cannot find required parameter 'controller_joint_names' on the parameter server.");
177  }
178 
179  // Get EKI parameters from parameter server
180  const std::string param_addr = "eki/robot_address";
181  const std::string param_port = "eki/robot_port";
182  const std::string param_socket_timeout = "eki/socket_timeout";
183  const std::string param_max_cmd_buf_len = "eki/max_cmd_buf_len";
184 
185  if (nh_.getParam(param_addr, eki_server_address_) &&
186  nh_.getParam(param_port, eki_server_port_))
187  {
188  ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Configuring Kuka EKI hardware interface on: "
189  << eki_server_address_ << ", " << eki_server_port_);
190  }
191  else
192  {
193  std::string msg = "Failed to get EKI address/port from parameter server (looking for '" + param_addr +
194  "', '" + param_port + "')";
195  ROS_ERROR_STREAM(msg);
196  throw std::runtime_error(msg);
197  }
198 
199  if (nh_.getParam(param_socket_timeout, eki_read_state_timeout_))
200  {
201  ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Configuring Kuka EKI hardware interface socket timeout to "
202  << eki_read_state_timeout_ << " seconds");
203  }
204  else
205  {
206  ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Failed to get EKI socket timeout from parameter server (looking "
207  "for '" + param_socket_timeout + "'), defaulting to " +
208  std::to_string(eki_read_state_timeout_) + " seconds");
209  }
210 
211  if (nh_.getParam(param_max_cmd_buf_len, eki_max_cmd_buff_len_))
212  {
213  ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Configuring Kuka EKI hardware interface maximum command buffer "
214  "length to " << eki_max_cmd_buff_len_);
215  }
216  else
217  {
218  ROS_INFO_STREAM_NAMED("kuka_eki_hw_interface", "Failed to get EKI hardware interface maximum command buffer length "
219  "from parameter server (looking for '" + param_max_cmd_buf_len + "'), defaulting to " +
220  std::to_string(eki_max_cmd_buff_len_));
221  }
222 
223  // Create ros_control interfaces (joint state and position joint for all dof's)
224  for (std::size_t i = 0; i < n_dof_; ++i)
225  {
226  // Joint state interface
229  &joint_effort_[i]));
230 
231  // Joint position control interface
235  }
236 
237  // Register interfaces
240 
241  ROS_INFO_STREAM_NAMED("kukw_eki_hw_interface", "Loaded Kuka EKI hardware interface");
242 }
243 
244 
246 {
247  ROS_INFO_NAMED("kuka_eki_hw_interface", "Starting Kuka EKI hardware interface...");
248 
249  // Start client
250  ROS_INFO_NAMED("kuka_eki_hw_interface", "... connecting to robot's EKI server...");
251  boost::asio::ip::udp::resolver resolver(ios_);
252  eki_server_endpoint_ = *resolver.resolve({boost::asio::ip::udp::v4(), eki_server_address_, eki_server_port_});
253  boost::array<char, 1> ini_buf = { 0 };
254  eki_server_socket_.send_to(boost::asio::buffer(ini_buf), eki_server_endpoint_); // initiate contact to start server
255 
256  // Start persistent actor to check for eki_read_state timeouts
257  deadline_.expires_at(boost::posix_time::pos_infin); // do nothing unit a read is invoked (deadline_ = +inf)
259 
260  // Initialize joint_position_command_ from initial robot state (avoid bad (null) commands before controllers come up)
262  {
263  std::string msg = "Failed to read from robot EKI server within alloted time of "
264  + std::to_string(eki_read_state_timeout_) + " seconds. Make sure eki_hw_interface is running "
265  "on the robot controller and all configurations are correct.";
266  ROS_ERROR_STREAM(msg);
267  throw std::runtime_error(msg);
268  }
270 
271  ROS_INFO_NAMED("kuka_eki_hw_interface", "... done. EKI hardware interface started!");
272 }
273 
274 
276 {
278  {
279  std::string msg = "Failed to read from robot EKI server within alloted time of "
280  + std::to_string(eki_read_state_timeout_) + " seconds. Make sure eki_hw_interface is running "
281  "on the robot controller and all configurations are correct.";
282  ROS_ERROR_STREAM(msg);
283  throw std::runtime_error(msg);
284  }
285 }
286 
287 
289 {
290  // only write if max will not be exceeded
293 
294  // underflow/overflow checking
295  // NOTE: this is commented as it results in a lot of logging output and the use of ROS_*
296  // logging macros breaks incurs (quite) some overhead. Uncomment and rebuild this
297  // if you'd like to use this anyway.
298  //if (eki_cmd_buff_len_ >= eki_max_cmd_buff_len_)
299  // ROS_WARN_STREAM("eki_hw_iface RobotCommand buffer overflow (curent size " << eki_cmd_buff_len_
300  // << " greater than or equal max allowed " << eki_max_cmd_buff_len_ << ")");
301  //else if (eki_cmd_buff_len_ == 0)
302  // ROS_WARN_STREAM("eki_hw_iface RobotCommand buffer empty");
303 }
304 
305 } // namespace kuka_eki_hw_interface
#define ROS_INFO_NAMED(name,...)
bool eki_read_state(std::vector< double > &joint_position, std::vector< double > &joint_velocity, std::vector< double > &joint_effort, int &cmd_buff_len)
#define ROS_INFO_STREAM_NAMED(name, args)
void read(const ros::Time &time, const ros::Duration &period)
static double from_degrees(double degrees)
static double to_degrees(double radians)
static void eki_handle_receive(const boost::system::error_code &ec, size_t length, boost::system::error_code *out_ec, size_t *out_length)
void registerHandle(const JointStateHandle &handle)
hardware_interface::JointStateInterface joint_state_interface_
bool eki_write_command(const std::vector< double > &joint_position)
JointStateHandle getHandle(const std::string &name)
void write(const ros::Time &time, const ros::Duration &period)
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR(...)
hardware_interface::PositionJointInterface position_joint_interface_


kuka_eki_hw_interface
Author(s): Brett Hemes (3M)
autogenerated on Tue Oct 15 2019 03:33:29