implemented initialization of rs485 interface.

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -26,6 +26,7 @@
 //----------------------------------------------------------------------
 
 #include "ur_rtde_driver/ros/hardware_interface.h"
+#include "ur_rtde_driver/ur/tool_communication.h"
 
 #include <Eigen/Geometry>
 
@@ -66,9 +67,67 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   tcp_link_ = robot_hw_nh.param<std::string>("tcp_link", "tool0");
   program_state_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("robot_program_running", 10, true);
 
+  bool use_tool_communication = robot_hw_nh.param<bool>("use_tool_communication", "false");
+  std::unique_ptr<ToolCommSetup> tool_comm_setup;
+  if (use_tool_communication)
+  {
+    tool_comm_setup.reset(new ToolCommSetup());
+
+    using ToolVoltageT = std::underlying_type<ToolVoltage>::type;
+    ToolVoltageT tool_voltage;
+    if (!robot_hw_nh.getParam("tool_voltage", tool_voltage))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_voltage") << " not given.");
+      return false;
+    }
+    tool_comm_setup->setToolVoltage(static_cast<ToolVoltage>(tool_voltage));
+
+    using ParityT = std::underlying_type<Parity>::type;
+    ParityT parity;
+    if (!robot_hw_nh.getParam("tool_parity", parity))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_parity") << " not given.");
+      return false;
+    }
+
+    int baud_rate;
+    if (!robot_hw_nh.getParam("tool_baud_rate", baud_rate))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_baud_rate") << " not given.");
+      return false;
+    }
+    tool_comm_setup->setBaudRate(baud_rate);
+
+    int stop_bits;
+    if (!robot_hw_nh.getParam("tool_stop_bits", stop_bits))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_stop_bits") << " not given.");
+      return false;
+    }
+    tool_comm_setup->setStopBits(stop_bits);
+
+    int rx_idle_chars;
+    if (!robot_hw_nh.getParam("tool_rx_idle_chars", rx_idle_chars))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_rx_idle_chars") << " not given.");
+      return false;
+    }
+    tool_comm_setup->setRxIdleChars(rx_idle_chars);
+    tool_comm_setup->setParity(static_cast<Parity>(parity));
+
+    int tx_idle_chars;
+    if (!robot_hw_nh.getParam("tool_tx_idle_chars", tx_idle_chars))
+    {
+      ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_tx_idle_chars") << " not given.");
+      return false;
+    }
+    tool_comm_setup->setTxIdleChars(tx_idle_chars);
+  }
+
   ROS_INFO_STREAM("Initializing urdriver");
   ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename,
-                                std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1)));
+                                std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
+                                std::move(tool_comm_setup)));
 
   if (!root_nh.getParam("hardware_interface/joints", joint_names_))
   {

ur_rtde_driver/src/ur/tool_communication.cpp

@@ -0,0 +1,53 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2019 FZI Forschungszentrum Informatik
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-06-06
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_rtde_driver/ur/tool_communication.h"
+
+namespace ur_driver
+{
+ToolCommSetup::ToolCommSetup()
+  : tool_voltage_(ToolVoltage::OFF)
+  , parity_(Parity::ODD)
+  , baud_rate_(9600)
+  , stop_bits_(1, 2)
+  , rx_idle_chars_(1.0, 40.0)
+  , tx_idle_chars_(0.0, 40.0)
+{
+}
+
+void ToolCommSetup::setBaudRate(const uint32_t baud_rate)
+{
+  if (baud_rates_.find(baud_rate) != baud_rates_.end())
+  {
+    baud_rate_ = baud_rate;
+  }
+  else
+  {
+    throw std::runtime_error("Provided baud rate is not allowed");
+  }
+}
+}  // namespace ur_driver

ur_rtde_driver/src/ur/ur_driver.cpp

@@ -38,13 +38,15 @@
 namespace ur_driver
 {
 static const int32_t MULT_JOINTSTATE = 1000000;
+static const std::string BEGIN_REPLACE("{{BEGIN_REPLACE}}");
 static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
 static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
 static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
 static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
 
 ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file,
-                              const std::string& recipe_file, std::function<void(bool)> handle_program_state)
+                              const std::string& recipe_file, std::function<void(bool)> handle_program_state,
+                              std::unique_ptr<ToolCommSetup> tool_comm_setup)
   : servoj_time_(0.008)
   , servoj_gain_(2000)
   , servoj_lookahead_time_(0.03)
@@ -80,6 +82,20 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
   prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip);
   prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
 
+  std::stringstream begin_replace;
+  if (tool_comm_setup != nullptr)
+  {
+    begin_replace << "set_tool_voltage("
+                  << static_cast<std::underlying_type<ToolVoltage>::type>(tool_comm_setup->getToolVoltage()) << ")\n";
+    begin_replace << "set_tool_communication("
+                  << "True"
+                  << ", " << tool_comm_setup->getBaudRate() << ", "
+                  << static_cast<std::underlying_type<Parity>::type>(tool_comm_setup->getParity()) << ", "
+                  << tool_comm_setup->getStopBits() << ", " << tool_comm_setup->getRxIdleChars() << ", "
+                  << tool_comm_setup->getTxIdleChars() << ")";
+  }
+  prog.replace(prog.find(BEGIN_REPLACE), BEGIN_REPLACE.length(), begin_replace.str());
+
   script_sender_.reset(new comm::ScriptSender(script_sender_port, prog));
   script_sender_->start();
   LOG_INFO("Created script sender");