diff --git a/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h index 707ddc807..8e496f979 100644 --- a/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h +++ b/kuka_eki_hw_interface/include/kuka_eki_hw_interface/kuka_eki_hw_interface.h @@ -65,6 +65,7 @@ class KukaEkiHardwareInterface : public hardware_interface::RobotHW // EKI std::string eki_server_address_; std::string eki_server_port_; + int eki_cmd_id_count_; int eki_cmd_buff_len_; int eki_max_cmd_buff_len_ = 5; // by default, limit command buffer to 5 (size of advance run in KRL) @@ -81,8 +82,9 @@ class KukaEkiHardwareInterface : public hardware_interface::RobotHW int eki_read_state_timeout_ = 5; // [s]; settable by parameter (default = 5) boost::asio::io_service ios_; boost::asio::deadline_timer deadline_; - boost::asio::ip::udp::endpoint eki_server_endpoint_; - boost::asio::ip::udp::socket eki_server_socket_; + boost::asio::ip::tcp::endpoint eki_server_endpoint_; + boost::asio::ip::tcp::socket eki_server_socket_; + boost::asio::ip::tcp::acceptor eki_acceptor_; void eki_check_read_state_deadline(); static void eki_handle_receive(const boost::system::error_code &ec, size_t length, boost::system::error_code* out_ec, size_t* out_length); diff --git a/kuka_eki_hw_interface/krl/EkiHwInterface+.xml b/kuka_eki_hw_interface/krl/EkiHwInterface+.xml new file mode 100644 index 000000000..80cbf33a5 --- /dev/null +++ b/kuka_eki_hw_interface/krl/EkiHwInterface+.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/kuka_eki_hw_interface/krl/EkiHwInterface.xml b/kuka_eki_hw_interface/krl/EkiHwInterface.xml index 4aea0aadc..90fc2b6e0 100644 --- a/kuka_eki_hw_interface/krl/EkiHwInterface.xml +++ b/kuka_eki_hw_interface/krl/EkiHwInterface.xml @@ -1,71 +1,11 @@ - - - - Client - - - Program - - - address.of.robot.controller - 54600 - UDP - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + \ No newline at end of file diff --git a/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat index f47f888f3..37faa815d 100644 --- a/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat +++ b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.dat @@ -1,4 +1,6 @@ -&ACCESS RVO -defdat kuka_eki_hw_interface +&ACCESS RVP +DEFDAT KUKA_EKI_HW_INTERFACE +EXT bas (BAS_COMMAND :IN,REAL :IN ) ext bas(bas_command :in,real :in ) -enddat \ No newline at end of file + INT id_count +ENDDAT \ No newline at end of file diff --git a/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src index 5a4edf7b9..945deab73 100644 --- a/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src +++ b/kuka_eki_hw_interface/krl/kuka_eki_hw_interface.src @@ -1,45 +1,14 @@ -&ACCESS RVO +&ACCESS RVP +&REL 5 def kuka_eki_hw_interface() - - ; Software License Agreement (BSD License) - ; - ; Copyright (c) 2018, 3M - ; All rights reserved. - ; - ; Redistribution and use in source and binary forms, with or without - ; modification, are permitted provided that the following conditions are met: - ; - ; * Redistributions of source code must retain the above copyright - ; notice, this list of conditions and the following disclaimer. - ; * Redistributions in binary form must reproduce the above copyright - ; notice, this list of conditions and the following disclaimer in the - ; documentation and/or other materials provided with the distribution. - ; * Neither the name of the copyright holder, nor the names of its - ; contributors may be used to endorse or promote products derived - ; from this software without specific prior written permission. - ; - ; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - ; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - ; IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ; ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - ; LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - ; CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - ; SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - ; INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - ; CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - ; ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - ; POSSIBILITY OF SUCH DAMAGE. - - ; Author: Brett Hemes (3M) - - ; Declarations decl axis joint_pos_tgt decl int elements_read ; INI - bas(#initmov, 0) ; Basic initializasion of axes + bas(#initmov, 0) ; Basic initialization of axes +;FOLD description ; Initialize eki_hw_interface server ; Config located in C:/ROBOTER/Config/User/Common/EthernetKRL/EkiHwInterface.xml ; Starts a TCP state sever on xml-specified IP address/port. @@ -59,11 +28,12 @@ def kuka_eki_hw_interface() ; Interrupts: ; 15: Calls eki_hw_iface_reset() on falling edge of $flag[1] ; 16: Calls eki_hw_iface_send() on rising edge of $timer_flag[1] +;ENDFOLD eki_hw_iface_init() - ; BCO (Block COincidence) run to current position - ; (requied for below loop continue before first incoming command) - joint_pos_tgt = $axis_act_meas + ; BCO (Block Coincidence) run to current position + ; (required for below loop continue before first incoming command) + joint_pos_tgt = $axis_act ptp joint_pos_tgt ; Loop forever @@ -73,15 +43,17 @@ def kuka_eki_hw_interface() ptp joint_pos_tgt c_ptp ; PTP to most recent commanded position endloop +;FOLD note ; Note: EKI channels delete on reset or deselect of this program ; See Program EKI config element +;ENDFOLD end def eki_hw_iface_init() - decl eki_status eki_ret - + decl int ekx_ret + id_count = 0 ; Setup interrupts ; Interrupt 15 - Connection cleanup on client disconnect global interrupt decl 15 when $flag[1]==false do eki_hw_iface_reset() @@ -94,46 +66,63 @@ def eki_hw_iface_init() $timer_stop[1] = false ; Start timer 1 ; Create and open EKI interface - eki_ret = eki_init("EkiHwInterface") - eki_ret = eki_open("EkiHwInterface") + ; ekx_ret = eki_init("EkiHwInterface") + ekx_ret = EKX_Open("EkiHwInterface") + EKX_HandleError(ekx_ret) + + $flag[1] = true end def eki_hw_iface_send() - decl eki_status eki_ret - decl real vel_percent + decl int ekx_ret + decl real vel_percent if $flag[1] then ; If connection alive ; Load state values into xml structure + ; position - eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A1", $axis_act_meas.a1) - eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A2", $axis_act_meas.a2) - eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A3", $axis_act_meas.a3) - eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A4", $axis_act_meas.a4) - eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A5", $axis_act_meas.a5) - eki_ret = eki_setreal("EkiHwInterface", "RobotState/Pos/@A6", $axis_act_meas.a6) - ; velocity - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A1", $vel_axis_act[1] * $vel_axis_ma[1] / 100.0) - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A2", $vel_axis_act[2] * $vel_axis_ma[2] / 100.0) - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A3", $vel_axis_act[3] * $vel_axis_ma[3] / 100.0) - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A4", $vel_axis_act[4] * $vel_axis_ma[4] / 100.0) - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A5", $vel_axis_act[5] * $vel_axis_ma[5] / 100.0) - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Vel/@A6", $vel_axis_act[6] * $vel_axis_ma[6] / 100.0) - ; effort - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A1", $torque_axis_act[1]) - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A2", $torque_axis_act[2]) - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A3", $torque_axis_act[3]) - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A4", $torque_axis_act[4]) - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A5", $torque_axis_act[5]) - ;eki_ret = eki_setreal("EkiHwInterface", "RobotState/Eff/@A6", $torque_axis_act[6]) + ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A1", $axis_act.a1) + EKX_HandleError(ekx_ret) + ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A2", $axis_act.a2) + EKX_HandleError(ekx_ret) + ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A3", $axis_act.a3) + EKX_HandleError(ekx_ret) + ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A4", $axis_act.a4) + EKX_HandleError(ekx_ret) + ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A5", $axis_act.a5) + EKX_HandleError(ekx_ret) + ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Pos.A6", $axis_act.a6) + EKX_HandleError(ekx_ret) + +;FOLD velocity + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A1", $vel_axis_act[1] * $vel_axis_ma[1] / 100.0) + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A2", $vel_axis_act[2] * $vel_axis_ma[2] / 100.0) + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A3", $vel_axis_act[3] * $vel_axis_ma[3] / 100.0) + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A4", $vel_axis_act[4] * $vel_axis_ma[4] / 100.0) + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A5", $vel_axis_act[5] * $vel_axis_ma[5] / 100.0) + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Vel.A6", $vel_axis_act[6] * $vel_axis_ma[6] / 100.0) +;ENDFOLD +;FOLD effort + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A1", $torque_axis_act[1]) + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A2", $torque_axis_act[2]) + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A3", $torque_axis_act[3]) + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A4", $torque_axis_act[4]) + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A5", $torque_axis_act[5]) + ;ekx_ret = EKX_WriteReal("EkiHwInterface.RobotState.Eff.A6", $torque_axis_act[6]) +;ENDFOLD ; interface state - eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1") - eki_ret = eki_setint("EkiHwInterface", "RobotState/RobotCommand/@Size", eki_ret.buff) + ; ekx_ret = eki_checkbuffer("EkiHwInterface.RobotCommand.Pos.A1") + ; ekx_ret = EKX_WriteInteger("EkiHwInterface.RobotState.RobotCommand.Size", ekx_ret.buff) + + ekx_ret = EKX_WriteInteger("EkiHwInterface.RobotState.RobotCommand.ID", id_count) + EKX_HandleError(ekx_ret) ; Send xml structure if $flag[1] then ; Make sure connection hasn't died while updating xml structure - eki_ret = eki_send("EkiHwInterface", "RobotState") + ekx_ret = EKX_Send("EkiHwInterface.RobotState") + EKX_HandleError(ekx_ret) endif endif @@ -141,52 +130,50 @@ def eki_hw_iface_send() $timer[1] = -10 ; ~10 ms for above send + 10 ms interrupt timer -> ~50 Hz state transmission end - - -deffct int eki_hw_iface_available() - decl eki_status eki_ret - - if not $flag[1] then - return 0 - endif - - eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1") - return eki_ret.buff -endfct - - - ; eki_hw_iface_get -; Tries to read most recent elemnt from buffer. q left unchanged if empty. +; Tries to read most recent element from buffer. q left unchanged if empty. ; Returns number of elements read. deffct int eki_hw_iface_get(joint_pos_cmd :out) - decl eki_status eki_ret + decl bool ekx_ret decl axis joint_pos_cmd + decl bool is_new + decl int rec_id + + ; ekx_ret = eki_checkbuffer("EkiHwInterface.RobotCommand.Pos.A1") + ; if ekx_ret.buff <= 0 then + ; return 0 + ; endif + + ekx_ret = EKX_WaitForSensorData(1, "EkiHwInterface.RobotCommand.ID", 10000) + ekx_ret = EKX_GetIntegerElement(1, "EkiHwInterface.RobotCommand.ID", rec_id, is_new) + ; if id_count == rec_id then + ; return 0 + ; endif + + ; if not ekx_ret then + ; return 0 + ; endif + + ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A1", joint_pos_cmd.a1, is_new) + ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A2", joint_pos_cmd.a2, is_new) + ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A3", joint_pos_cmd.a3, is_new) + ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A4", joint_pos_cmd.a4, is_new) + ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A5", joint_pos_cmd.a5, is_new) + ekx_ret = EKX_GetRealElement(1, "EkiHwInterface.RobotCommand.Pos.A6", joint_pos_cmd.a6, is_new) + + id_count = rec_id - if not $flag[1] then - return 0 - endif - - eki_ret = eki_checkbuffer("EkiHwInterface", "RobotCommand/Pos/@A1") - if eki_ret.buff <= 0 then - return 0 - endif - - eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A1", joint_pos_cmd.a1) - eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A2", joint_pos_cmd.a2) - eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A3", joint_pos_cmd.a3) - eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A4", joint_pos_cmd.a4) - eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A5", joint_pos_cmd.a5) - eki_ret = eki_getreal("EkiHwInterface", "RobotCommand/Pos/@A6", joint_pos_cmd.a6) return 1 endfct def eki_hw_iface_reset() - decl eki_status eki_ret + decl int ekx_ret_int + decl bool ekx_ret_bool - eki_ret = eki_clear("EkiHwInterface") - eki_ret = eki_init("EkiHwInterface") - eki_ret = eki_open("EkiHwInterface") + ekx_ret_bool = EKX_Close("EkiHwInterface") + ekx_ret_int = EKX_Open("EkiHwInterface") + EKX_HandleError(ekx_ret_int) + $flag[1] = true end diff --git a/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp b/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp index 9a59a03f1..d98f8a37a 100644 --- a/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp +++ b/kuka_eki_hw_interface/src/kuka_eki_hw_interface.cpp @@ -48,7 +48,7 @@ namespace kuka_eki_hw_interface KukaEkiHardwareInterface::KukaEkiHardwareInterface() : joint_position_(n_dof_, 0.0), joint_velocity_(n_dof_, 0.0), joint_effort_(n_dof_, 0.0), joint_position_command_(n_dof_, 0.0), joint_names_(n_dof_), deadline_(ios_), - eki_server_socket_(ios_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0)) + eki_server_socket_(ios_, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0)), eki_cmd_id_count_(0), eki_cmd_buff_len_(0) { } @@ -87,12 +87,10 @@ bool KukaEkiHardwareInterface::eki_read_state(std::vector &joint_positio static boost::array in_buffer; // Read socket buffer (with timeout) - // Based off of Boost documentation example: doc/html/boost_asio/example/timeouts/blocking_udp_client.cpp deadline_.expires_from_now(boost::posix_time::seconds(eki_read_state_timeout_)); // set deadline boost::system::error_code ec = boost::asio::error::would_block; size_t len = 0; - eki_server_socket_.async_receive(boost::asio::buffer(in_buffer), - boost::bind(&KukaEkiHardwareInterface::eki_handle_receive, _1, _2, &ec, &len)); + eki_server_socket_.async_receive(boost::asio::buffer(in_buffer), boost::bind(&KukaEkiHardwareInterface::eki_handle_receive, _1, _2, &ec, &len)); do ios_.run_one(); while (ec == boost::asio::error::would_block); @@ -133,8 +131,15 @@ bool KukaEkiHardwareInterface::eki_read_state(std::vector &joint_positio axis_name[1]++; } - // Extract number of command elements buffered on robot - robot_command->Attribute("Size", &cmd_buff_len); + // Extract last command id that was received/read by the robot + int id_count; + robot_command->Attribute("ID", &id_count); + + bool wrap_around = id_count > eki_cmd_id_count_; + if(!wrap_around) + cmd_buff_len = eki_cmd_id_count_ - id_count; + else + cmd_buff_len = (INT32_MAX - id_count) + eki_cmd_id_count_; return true; } @@ -142,6 +147,11 @@ bool KukaEkiHardwareInterface::eki_read_state(std::vector &joint_positio bool KukaEkiHardwareInterface::eki_write_command(const std::vector &joint_position_command) { + if( (eki_cmd_id_count_ + 1) > INT32_MAX ) + eki_cmd_id_count_ = 0; + else + ++eki_cmd_id_count_; + TiXmlDocument xml_out; TiXmlElement* robot_command = new TiXmlElement("RobotCommand"); TiXmlElement* pos = new TiXmlElement("Pos"); @@ -149,6 +159,9 @@ bool KukaEkiHardwareInterface::eki_write_command(const std::vector &join robot_command->LinkEndChild(pos); pos->LinkEndChild(empty_text); // force format (vs ) char axis_name[] = "A1"; + + robot_command->SetAttribute("ID", std::to_string(eki_cmd_id_count_)); + for (int i = 0; i < n_dof_; ++i) { pos->SetAttribute(axis_name, std::to_string(angles::to_degrees(joint_position_command[i])).c_str()); @@ -160,8 +173,7 @@ bool KukaEkiHardwareInterface::eki_write_command(const std::vector &join xml_printer.SetStreamPrinting(); // no linebreaks xml_out.Accept(&xml_printer); - size_t len = eki_server_socket_.send_to(boost::asio::buffer(xml_printer.CStr(), xml_printer.Size()), - eki_server_endpoint_); + size_t len = eki_server_socket_.send(boost::asio::buffer(xml_printer.CStr(), xml_printer.Size())); return true; } @@ -248,10 +260,34 @@ void KukaEkiHardwareInterface::start() // Start client ROS_INFO_NAMED("kuka_eki_hw_interface", "... connecting to robot's EKI server..."); - boost::asio::ip::udp::resolver resolver(ios_); - eki_server_endpoint_ = *resolver.resolve({boost::asio::ip::udp::v4(), eki_server_address_, eki_server_port_}); - boost::array ini_buf = { 0 }; - eki_server_socket_.send_to(boost::asio::buffer(ini_buf), eki_server_endpoint_); // initiate contact to start server + eki_server_endpoint_ = boost::asio::ip::tcp::endpoint(boost::asio::ip::address_v4::any(), std::stoi(eki_server_port_)); + + ROS_WARN_STREAM("IP: " << eki_server_endpoint_.address().to_string()); + ROS_WARN_STREAM("PORT: " << eki_server_endpoint_.port()); + + eki_acceptor_ = boost::asio::ip::tcp::acceptor(ios_, eki_server_endpoint_.protocol()); + + boost::system::error_code ec; + + eki_acceptor_.bind(eki_server_endpoint_, ec); + while(ec.value() == 98) + { + ROS_WARN_STREAM("[ERROR] Code: " << ec.value() << " MSG: " << ec.message()); + ROS_WARN("Retrying..."); + eki_acceptor_.bind(eki_server_endpoint_, ec); + ros::Duration(0.1).sleep(); + } + if(ec != 0) + { + ROS_WARN_STREAM("[ERROR] Code: " << ec.value() << " MSG: " << ec.message()); + throw boost::system::system_error(ec ? ec : boost::asio::error::operation_aborted); + } + ROS_WARN("Connected!"); + eki_acceptor_.listen(1); + + eki_acceptor_.accept(eki_server_socket_, eki_server_endpoint_); + + eki_acceptor_.close(); // Start persistent actor to check for eki_read_state timeouts deadline_.expires_at(boost::posix_time::pos_infin); // do nothing unit a read is invoked (deadline_ = +inf)