Merge remote-tracking branch 'origin/master' into limit_check

examples/full_driver.cpp

@@ -180,7 +180,7 @@ int main(int argc, char* argv[])
         URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
         return 1;
       }
-      URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString());
+      URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());
     }
     else
     {

include/ur_client_library/rtde/rtde_writer.h

@@ -34,6 +34,7 @@
 #include "ur_client_library/rtde/data_package.h"
 #include "ur_client_library/comm/stream.h"
 #include "ur_client_library/queue/readerwriterqueue.h"
+#include "ur_client_library/ur/datatypes.h"
 #include <thread>
 #include <mutex>
 
@@ -118,10 +119,14 @@ class RTDEWriter
    *
    * \param output_pin The pin to change
    * \param value The new value, it should be between 0 and 1, where 0 is 4mA and 1 is 20mA.
+   * \param type The domain for the output can be eitherAnalogOutputType::CURRENT or  AnalogOutputType::VOLTAGE or
+   * AnalogOutputType::SET_ON_TEACH_PENDANT. In the latter case the domain is left untouched and the domain configured
+   * on the teach pendant will be used.
    *
    * \returns Success of the package creation
    */
-  bool sendStandardAnalogOutput(uint8_t output_pin, double value);
+  bool sendStandardAnalogOutput(uint8_t output_pin, double value,
+                                const AnalogOutputType type = AnalogOutputType::SET_ON_TEACH_PENDANT);
 
   /*!
    * \brief Creates a package to request setting a new value for an input_bit_register

include/ur_client_library/ur/datatypes.h

@@ -79,6 +79,13 @@ enum class SafetyStatus : int8_t  // Only available on 3.10/5.4
   SYSTEM_THREE_POSITION_ENABLING_STOP = 13
 };
 
+enum class AnalogOutputType : int8_t
+{
+  SET_ON_TEACH_PENDANT = -1,
+  CURRENT = 0,
+  VOLTAGE = 1
+};
+
 inline std::string robotModeString(const RobotMode& mode)
 {
   switch (mode)

src/rtde/rtde_writer.cpp

@@ -204,7 +204,7 @@ bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value)
   return success;
 }
 
-bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value)
+bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value, const AnalogOutputType type)
 {
   if (output_pin > 1)
   {
@@ -216,18 +216,22 @@ bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value)
   if (value > 1.0 || value < 0.0)
   {
     std::stringstream ss;
-    ss << "Analog output value should be between 0 and 1. The value is " << static_cast<int>(value);
+    ss << "Analog output value should be between 0 and 1. The value is " << static_cast<double>(value);
     URCL_LOG_ERROR(ss.str().c_str());
     return false;
   }
 
   std::lock_guard<std::mutex> guard(package_mutex_);
   uint8_t mask = pinToMask(output_pin);
-  // default to current for now, as no functionality to choose included in set io service
-  uint8_t output_type = 0;
+
   bool success = true;
   success = package_.setData("standard_analog_output_mask", mask);
-  success = success && package_.setData("standard_analog_output_type", output_type);
+  if (type != AnalogOutputType::SET_ON_TEACH_PENDANT)
+  {
+    auto output_type_bits = [](const uint8_t pin, const uint8_t type) { return type << pin; };
+    uint8_t output_type = output_type_bits(output_pin, toUnderlying(type));
+    success = success && package_.setData("standard_analog_output_type", output_type);
+  }
   success = success && package_.setData("standard_analog_output_0", value);
   success = success && package_.setData("standard_analog_output_1", value);
 

tests/test_rtde_writer.cpp

@@ -260,9 +260,12 @@ TEST_F(RTDEWriterTest, send_tool_digital_output)
   EXPECT_FALSE(writer_->sendToolDigitalOutput(pin, send_pin_value));
 }
 
-TEST_F(RTDEWriterTest, send_standard_analog_output)
+TEST_F(RTDEWriterTest, send_standard_analog_output_unknown_domain)
 {
+  waitForMessageCallback(1000);
+
   uint8_t expected_standard_analog_output_mask = 1;
+
   uint8_t pin = 0;
   double send_analog_output = 1;
   EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output));
@@ -272,26 +275,100 @@ TEST_F(RTDEWriterTest, send_standard_analog_output)
   ASSERT_TRUE(dataFieldExist("standard_analog_output_0"));
   ASSERT_TRUE(dataFieldExist("standard_analog_output_1"));
   ASSERT_TRUE(dataFieldExist("standard_analog_output_mask"));
+  ASSERT_TRUE(dataFieldExist("standard_analog_output_type"));
+
+  double received_analog_output = std::get<double>(parsed_data_["standard_analog_output_0"]);
+  uint8_t received_standard_analog_output_mask = std::get<uint8_t>(parsed_data_["standard_analog_output_mask"]);
+  uint8_t received_standard_analog_output_type = std::get<uint8_t>(parsed_data_["standard_analog_output_type"]);
+
+  EXPECT_EQ(send_analog_output, received_analog_output);
+  EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask);
+  // The test server sets this to 0 if not given
+  EXPECT_EQ(0, received_standard_analog_output_type);
+}
+
+TEST_F(RTDEWriterTest, send_standard_analog_output_voltage)
+{
+  uint8_t pin = 0;
+  AnalogOutputType type = AnalogOutputType::VOLTAGE;
+  double send_analog_output = 1;
+
+  uint8_t expected_standard_analog_output_mask = 1;
+  uint8_t expected_standard_analog_output_type = 1;
+
+  EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output, type));
+
+  waitForMessageCallback(1000);
 
   double received_analog_output = std::get<double>(parsed_data_["standard_analog_output_0"]);
   uint8_t received_standard_analog_output_mask = std::get<uint8_t>(parsed_data_["standard_analog_output_mask"]);
+  uint8_t received_standard_analog_output_type = std::get<uint8_t>(parsed_data_["standard_analog_output_type"]);
 
   EXPECT_EQ(send_analog_output, received_analog_output);
   EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask);
+  EXPECT_EQ(expected_standard_analog_output_type, received_standard_analog_output_type);
 
   pin = 1;
   expected_standard_analog_output_mask = 2;
-  EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output));
+  expected_standard_analog_output_type = 2;
+
+  EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output, type));
 
   waitForMessageCallback(1000);
 
   received_analog_output = std::get<double>(parsed_data_["standard_analog_output_1"]);
   received_standard_analog_output_mask = std::get<uint8_t>(parsed_data_["standard_analog_output_mask"]);
+  received_standard_analog_output_type = std::get<uint8_t>(parsed_data_["standard_analog_output_type"]);
 
   EXPECT_EQ(send_analog_output, received_analog_output);
   EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask);
+  EXPECT_EQ(expected_standard_analog_output_type, received_standard_analog_output_type);
+}
 
-  // Changing pins above 1, should return false.
+TEST_F(RTDEWriterTest, send_standard_analog_output_current)
+{
+  uint8_t pin = 0;
+  AnalogOutputType type = AnalogOutputType::CURRENT;
+  double send_analog_output = 1;
+
+  uint8_t expected_standard_analog_output_mask = 1;
+  uint8_t expected_standard_analog_output_type = 0;
+
+  EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output, type));
+
+  waitForMessageCallback(1000);
+
+  double received_analog_output = std::get<double>(parsed_data_["standard_analog_output_0"]);
+  uint8_t received_standard_analog_output_mask = std::get<uint8_t>(parsed_data_["standard_analog_output_mask"]);
+  uint8_t received_standard_analog_output_type = std::get<uint8_t>(parsed_data_["standard_analog_output_type"]);
+
+  EXPECT_EQ(send_analog_output, received_analog_output);
+  EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask);
+  EXPECT_EQ(expected_standard_analog_output_type, received_standard_analog_output_type);
+
+  pin = 1;
+  expected_standard_analog_output_mask = 2;
+  expected_standard_analog_output_type = 0;
+
+  EXPECT_TRUE(writer_->sendStandardAnalogOutput(pin, send_analog_output, type));
+
+  waitForMessageCallback(1000);
+
+  received_analog_output = std::get<double>(parsed_data_["standard_analog_output_1"]);
+  received_standard_analog_output_mask = std::get<uint8_t>(parsed_data_["standard_analog_output_mask"]);
+  received_standard_analog_output_type = std::get<uint8_t>(parsed_data_["standard_analog_output_type"]);
+
+  EXPECT_EQ(send_analog_output, received_analog_output);
+  EXPECT_EQ(expected_standard_analog_output_mask, received_standard_analog_output_mask);
+  EXPECT_EQ(expected_standard_analog_output_type, received_standard_analog_output_type);
+}
+
+TEST_F(RTDEWriterTest, send_standard_analog_output_illegal_input_fails)
+{
+  uint8_t pin = 0;
+  double send_analog_output = 1;
+
+  // Pin should be either 0 or 1
   pin = 2;
   EXPECT_FALSE(writer_->sendStandardAnalogOutput(pin, send_analog_output));