Trajectory interface

CMakeLists.txt

@@ -21,6 +21,7 @@ add_library(urcl SHARED
     src/comm/tcp_server.cpp
     src/control/reverse_interface.cpp
     src/control/script_sender.cpp
+    src/control/trajectory_point_interface.cpp
     src/primary/primary_package.cpp
     src/primary/robot_message.cpp
     src/primary/robot_state.cpp

include/ur_client_library/comm/control_mode.h

@@ -27,7 +27,6 @@
 
 #ifndef UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED
 #define UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED
-#endif  // ifndef UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED
 
 namespace urcl
 {
@@ -42,7 +41,10 @@ enum class ControlMode : int32_t
   MODE_UNINITIALIZED = -1,  ///< Startup default until another mode is sent to the script.
   MODE_IDLE = 0,            ///< Set when no controller is currently active controlling the robot.
   MODE_SERVOJ = 1,          ///< Set when servoj control is active.
-  MODE_SPEEDJ = 2           ///< Set when speedj control is active.
+  MODE_SPEEDJ = 2,          ///< Set when speedj control is active.
+  MODE_FORWARD = 3          ///< Set when trajectory forwarding is active.
 };
 }  // namespace comm
 }  // namespace urcl
+
+#endif  // ifndef UR_CLIENT_LIBRARY_CONTROL_MODE_H_INCLUDED

include/ur_client_library/comm/tcp_server.h

@@ -90,7 +90,7 @@ class TCPServer
    * \param func Function handling the event information. The file client's file_descriptor will be
    * passed to the function as well as the actual message received from the client.
    */
-  void setMessageCallback(std::function<void(const int, char*)> func)
+  void setMessageCallback(std::function<void(const int, char*, int)> func)
   {
     message_callback_ = func;
   }
@@ -184,7 +184,7 @@ class TCPServer
 
   std::function<void(const int)> new_connection_callback_;
   std::function<void(const int)> disconnect_callback_;
-  std::function<void(const int, char* buffer)> message_callback_;
+  std::function<void(const int, char* buffer, int nbytesrecv)> message_callback_;
 };
 
 }  // namespace comm

include/ur_client_library/control/reverse_interface.h

@@ -40,13 +40,25 @@ namespace urcl
 {
 namespace control
 {
+/*!
+ * \brief Control messages for forwarding and aborting trajectories.
+ */
+enum class TrajectoryControlMessage : int32_t
+{
+  TRAJECTORY_CANCEL = -1,  ///< Represents command to cancel currently active trajectory.
+  TRAJECTORY_NOOP = 0,     ///< Represents no new control command.
+  TRAJECTORY_START = 1,    ///< Represents command to start a new trajectory.
+};
+
 /*!
  * \brief The ReverseInterface class handles communication to the robot. It starts a server and
  * waits for the robot to connect via its URCaps program.
  */
 class ReverseInterface
 {
 public:
+  static const int32_t MULT_JOINTSTATE = 1000000;
+
   ReverseInterface() = delete;
   /*!
    * \brief Creates a ReverseInterface object including a TCPServer.
@@ -59,7 +71,7 @@ class ReverseInterface
   /*!
    * \brief Disconnects possible clients so the reverse interface object can be safely destroyed.
    */
-  ~ReverseInterface() = default;
+  virtual ~ReverseInterface() = default;
 
   /*!
    * \brief Writes needed information to the robot to be read by the URCaps program.
@@ -70,30 +82,38 @@ class ReverseInterface
    *
    * \returns True, if the write was performed successfully, false otherwise.
    */
-  bool write(const vector6d_t* positions, const comm::ControlMode control_mode = comm::ControlMode::MODE_IDLE);
+  virtual bool write(const vector6d_t* positions, const comm::ControlMode control_mode = comm::ControlMode::MODE_IDLE);
+
+  /*!
+   * \brief Writes needed information to the robot to be read by the URScript program.
+   *
+   * \param trajectory_action 1 if a trajectory is to be started, -1 if it should be stopped
+   * \param point_number The number of points of the trajectory to be executed
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, const int point_number = 0);
 
   /*!
    * \brief Set the Keepalive count. This will set the number of allowed timeout reads on the robot.
    *
    * \param count Number of allowed timeout reads on the robot.
    */
-  void setKeepaliveCount(const uint32_t& count)
+  virtual void setKeepaliveCount(const uint32_t& count)
   {
     keepalive_count_ = count;
   }
 
-private:
-  void connectionCallback(const int filedescriptor);
+protected:
+  virtual void connectionCallback(const int filedescriptor);
 
-  void disconnectionCallback(const int filedescriptor);
+  virtual void disconnectionCallback(const int filedescriptor);
 
-  void messageCallback(const int filedescriptor, char* buffer);
+  virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv);
 
   int client_fd_;
   comm::TCPServer server_;
 
-  static const int32_t MULT_JOINTSTATE = 1000000;
-
   template <typename T>
   size_t append(uint8_t* buffer, T& val)
   {

include/ur_client_library/control/trajectory_point_interface.h

@@ -0,0 +1,107 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2021 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 Exner mauch@fzi.de
+ * \date    2021-06-01
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED
+#define UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED
+
+#include "ur_client_library/control/reverse_interface.h"
+#include "ur_client_library/comm/control_mode.h"
+#include "ur_client_library/types.h"
+#include "ur_client_library/log.h"
+
+namespace urcl
+{
+namespace control
+{
+/*!
+ * \brief Types for encoding trajectory execution result.
+ */
+enum class TrajectoryResult : int32_t
+{
+
+  TRAJECTORY_RESULT_SUCCESS = 0,   ///< Successful execution
+  TRAJECTORY_RESULT_CANCELED = 1,  ///< Canceled by user
+  TRAJECTORY_RESULT_FAILURE = 2    ///< Aborted due to error during execution
+};
+
+/*!
+ * \brief The TrajectoryPointInterface class handles trajectory forwarding to the robot. Full
+ * trajectories are forwarded to the robot controller and are executed there.
+ */
+class TrajectoryPointInterface : public ReverseInterface
+{
+public:
+  static const int32_t MULT_TIME = 1000;
+  static const int32_t JOINT_POINT = 0;
+  static const int32_t CARTESIAN_POINT = 1;
+
+  TrajectoryPointInterface() = delete;
+  /*!
+   * \brief Creates a TrajectoryPointInterface object including a TCPServer.
+   *
+   * \param port Port the Server is started on
+   */
+  TrajectoryPointInterface(uint32_t port);
+
+  /*!
+   * \brief Disconnects possible clients so the reverse interface object can be safely destroyed.
+   */
+  virtual ~TrajectoryPointInterface() = default;
+
+  /*!
+   * \brief Writes needed information to the robot to be read by the URScript program.
+   *
+   * \param positions A vector of joint or cartesian targets for the robot
+   * \param time The goal time to reach the target
+   * \param blend_radius The radius to be used for blending between control points
+   * \param cartesian True, if the written point is specified in cartesian space, false if in joint space
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, const float blend_radius,
+                            const bool cartesian);
+
+  void setTrajectoryEndCallback(std::function<void(TrajectoryResult)> callback)
+  {
+    handle_trajectory_end_ = callback;
+  }
+
+protected:
+  virtual void connectionCallback(const int filedescriptor) override;
+
+  virtual void disconnectionCallback(const int filedescriptor) override;
+
+  virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) override;
+
+private:
+  std::function<void(TrajectoryResult)> handle_trajectory_end_;
+};
+
+}  // namespace control
+}  // namespace urcl
+
+#endif  // UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED

include/ur_client_library/ur/ur_driver.h

@@ -31,6 +31,7 @@
 
 #include "ur_client_library/rtde/rtde_client.h"
 #include "ur_client_library/control/reverse_interface.h"
+#include "ur_client_library/control/trajectory_point_interface.h"
 #include "ur_client_library/control/script_sender.h"
 #include "ur_client_library/ur/tool_communication.h"
 #include "ur_client_library/ur/version_information.h"
@@ -84,12 +85,14 @@ class UrDriver
    * \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
    * \param reverse_ip IP address that the reverse_port will get bound to. If not specified, the IP
    * address of the interface that is used for connecting to the robot's RTDE port will be used.
+   * \param trajectory_port Port used for sending trajectory points to the robot in case of
+   * trajectory forwarding.
    */
   UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
            const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
            std::unique_ptr<ToolCommSetup> tool_comm_setup, const uint32_t reverse_port = 50001,
            const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03,
-           bool non_blocking_read = false, const std::string& reverse_ip = "");
+           bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003);
 
   /*!
    * \brief Constructs a new UrDriver object.
@@ -113,12 +116,15 @@ class UrDriver
    * \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
    * \param reverse_ip IP address that the reverse_port will get bound to. If not specified, the IP
    * address of the interface that is used for connecting to the robot's RTDE port will be used.
+   * \param trajectory_port Port used for sending trajectory points to the robot in case of
+   * trajectory forwarding.
    */
   UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
            const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
            std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum = "",
            const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000,
-           double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "");
+           double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "",
+           const uint32_t trajectory_port = 50003);
   /*!
    * \brief Constructs a new UrDriver object.
    *
@@ -141,12 +147,14 @@ class UrDriver
    * \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
    * \param reverse_ip IP address that the reverse_port will get bound to. If not specified, the IP
    * address of the interface that is used for connecting to the robot's RTDE port will be used.
+   * \param trajectory_port Port used for sending trajectory points to the robot in case of
+   * trajectory forwarding.
    */
   UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
            const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
            const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001,
            const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03,
-           bool non_blocking_read = false, const std::string& reverse_ip = "")
+           bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003)
     : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode,
                std::unique_ptr<ToolCommSetup>{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain,
                servoj_lookahead_time, non_blocking_read, reverse_ip)
@@ -180,6 +188,30 @@ class UrDriver
    */
   bool writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode);
 
+  /*!
+   * \brief Writes a trajectory point onto the dedicated socket.
+   *
+   * \param values Desired joint or cartesian positions
+   * \param cartesian True, if the point sent is cartesian, false if joint-based
+   * \param goal_time Time for the robot to reach this point
+   * \param blend_radius  The radius to be used for blending between control points
+   *
+   * \returns True on successful write.
+   */
+  bool writeTrajectoryPoint(const vector6d_t& values, const bool cartesian, const float goal_time = 0.0,
+                            const float blend_radius = 0.052);
+
+  /*!
+   * \brief Writes a control message in trajectory forward mode.
+   *
+   * \param trajectory_action The action to be taken, such as starting a new trajectory
+   * \param point_number The number of points of a new trajectory to be sent
+   *
+   * \returns True on successful write.
+   */
+  bool writeTrajectoryControlMessage(const control::TrajectoryControlMessage trajectory_action,
+                                     const int point_number = 0);
+
   /*!
    * \brief Write a keepalive signal only.
    *
@@ -265,13 +297,28 @@ class UrDriver
    */
   void setKeepaliveCount(const uint32_t& count);
 
+  /*!
+   * \brief Register a callback for the robot-based trajectory execution completion.
+   *
+   * One mode of robot control is to forward a complete trajectory to the robot for execution.
+   * When the execution is done, the callback function registered here will be triggered.
+   *
+   * \param trajectory_done_cb Callback function that will be triggered in the event of finishing
+   * a trajectory execution
+   */
+  void registerTrajectoryDoneCallback(std::function<void(control::TrajectoryResult)> trajectory_done_cb)
+  {
+    trajectory_interface_->setTrajectoryEndCallback(trajectory_done_cb);
+  }
+
 private:
   std::string readScriptFile(const std::string& filename);
 
   int rtde_frequency_;
   comm::INotifier notifier_;
   std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
   std::unique_ptr<control::ReverseInterface> reverse_interface_;
+  std::unique_ptr<control::TrajectoryPointInterface> trajectory_interface_;
   std::unique_ptr<control::ScriptSender> script_sender_;
   std::unique_ptr<comm::URStream<primary_interface::PrimaryPackage>> primary_stream_;
   std::unique_ptr<comm::URStream<primary_interface::PrimaryPackage>> secondary_stream_;