WIP: Create an interface for registering a trajectoryDone callback fo…

…r passthrough

examples/full_driver.cpp

@@ -53,12 +53,16 @@ void handleRobotProgramState(bool program_running)
   // Print the text in green so we see it better
   std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
 }
+void trajectoryDoneCb(urcl::control::TrajectoryResult result)
+{
+  std::cout << "Trajectory finished in state " << toUnderlying(result) << std::endl;
+}
 
 int main(int argc, char* argv[])
 {
   std::unique_ptr<ToolCommSetup> tool_comm_setup;
-  g_my_driver.reset(new UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, false,
-                                 std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
+  g_my_driver.reset(new UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState,
+                                 &trajectoryDoneCb, false, std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
   // Once RTDE communication is started, we have to make sure to read from the interface buffer, as
   // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main
   // loop.

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

@@ -109,7 +109,7 @@ class ReverseInterface
 
   virtual void disconnectionCallback(const int filedescriptor);
 
-  virtual void messageCallback(const int filedescriptor, char* buffer);
+  virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv);
 
   int client_fd_;
   comm::TCPServer server_;

include/ur_client_library/control/trajectory_point_interface.h

@@ -37,6 +37,17 @@ 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.
@@ -72,12 +83,20 @@ class TrajectoryPointInterface : public ReverseInterface
   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) override;
+  virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) override;
+
+private:
+  std::function<void(TrajectoryResult)> handle_trajectory_end_;
 };
 
 }  // namespace control

include/ur_client_library/ur/ur_driver.h

@@ -87,7 +87,8 @@ class UrDriver
    * address of the interface that is used for connecting to the robot's RTDE port will be used.
    */
   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& input_recipe_file, std::function<void(bool)> handle_program_state,
+           std::function<void(control::TrajectoryResult)> trajectory_done_cb, 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 = "");
@@ -116,7 +117,8 @@ class UrDriver
    * address of the interface that is used for connecting to the robot's RTDE port will be used.
    */
   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& input_recipe_file, std::function<void(bool)> handle_program_state,
+           std::function<void(control::TrajectoryResult)> trajectory_done_cb, 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 = "");
@@ -144,13 +146,14 @@ class UrDriver
    * address of the interface that is used for connecting to the robot's RTDE port will be used.
    */
   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& input_recipe_file, std::function<void(bool)> handle_program_state,
+           std::function<void(control::TrajectoryResult)> trajectory_done_cb, 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 = "")
-    : 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)
+    : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, trajectory_done_cb,
+               headless_mode, std::unique_ptr<ToolCommSetup>{}, calibration_checksum, reverse_port, script_sender_port,
+               servoj_gain, servoj_lookahead_time, non_blocking_read, reverse_ip)
   {
   }
 

resources/external_control.urscript

@@ -27,6 +27,10 @@ TRAJECTORY_POINT_JOINT = 0
 TRAJECTORY_POINT_CARTESIAN = 1
 TRAJECTORY_DATA_DIMENSION = 7
 
+TRAJECTORY_RESULT_SUCCESS = 0
+TRAJECTORY_RESULT_CANCELED = 1
+TRAJECTORY_RESULT_FAILURE = 2
+
 #Global variables are also showed in the Teach pendants variable list
 global cmd_servo_state = SERVO_UNINITIALIZED
 global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
@@ -150,6 +154,7 @@ thread jointTrajectoryThread():
     #add some form of error handling here
     end
   end
+  socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket")
   textmsg("Trajectory finished")
 end
 
@@ -212,6 +217,7 @@ while keepalive > 0 and control_mode > MODE_STOPPED:
         textmsg("cancel received")
         kill thread_trajectory
         clear_remaining_trajectory_points()
+        socket_send_int(TRAJECTORY_RESULT_CANCELED, "trajectory_socket")
       end
     end
   else:

src/comm/tcp_server.cpp

@@ -272,7 +272,7 @@ void TCPServer::readData(const int fd)
   {
     if (message_callback_)
     {
-      message_callback_(fd, input_buffer_);
+      message_callback_(fd, input_buffer_, nbytesrecv);
     }
   }
   else

src/control/reverse_interface.cpp

@@ -35,8 +35,8 @@ ReverseInterface::ReverseInterface(uint32_t port, std::function<void(bool)> hand
   : client_fd_(-1), server_(port), handle_program_state_(handle_program_state), keepalive_count_(1)
 {
   handle_program_state_(false);
-  server_.setMessageCallback(
-      std::bind(&ReverseInterface::messageCallback, this, std::placeholders::_1, std::placeholders::_2));
+  server_.setMessageCallback(std::bind(&ReverseInterface::messageCallback, this, std::placeholders::_1,
+                                       std::placeholders::_2, std::placeholders::_3));
   server_.setConnectCallback(std::bind(&ReverseInterface::connectionCallback, this, std::placeholders::_1));
   server_.setDisconnectCallback(std::bind(&ReverseInterface::disconnectionCallback, this, std::placeholders::_1));
   server_.setMaxClientsAllowed(1);
@@ -135,7 +135,7 @@ void ReverseInterface::disconnectionCallback(const int filedescriptor)
   handle_program_state_(false);
 }
 
-void ReverseInterface::messageCallback(const int filedescriptor, char* buffer)
+void ReverseInterface::messageCallback(const int filedescriptor, char* buffer, int nbytesrecv)
 {
   URCL_LOG_WARN("Message on ReverseInterface received. The reverse interface currently does not support any message "
                 "handling. This message will be ignored.");

src/control/trajectory_point_interface.cpp

@@ -105,10 +105,23 @@ void TrajectoryPointInterface::disconnectionCallback(const int filedescriptor)
   client_fd_ = -1;
 }
 
-void TrajectoryPointInterface::messageCallback(const int filedescriptor, char* buffer)
+void TrajectoryPointInterface::messageCallback(const int filedescriptor, char* buffer, int nbytesrecv)
 {
-  // TODO: Send up. This will be things like "finished trajectory"
-  URCL_LOG_INFO("Received message %s on TrajectoryPointInterface", buffer);
+  if (nbytesrecv == 4)
+  {
+    int32_t* status = reinterpret_cast<int*>(buffer);
+    URCL_LOG_DEBUG("Received message %d on TrajectoryPointInterface", be32toh(*status));
+
+    if (handle_trajectory_end_)
+    {
+      handle_trajectory_end_(static_cast<TrajectoryResult>(*status));
+    }
+  }
+  else
+  {
+    URCL_LOG_WARN("Received %d bytes on TrajectoryPointInterface. Expecting 4 bytes, so ignoring this message",
+                  nbytesrecv);
+  }
 }
 }  // namespace control
 }  // namespace urcl

src/ur/ur_driver.cpp

@@ -49,7 +49,8 @@ static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
 
 urcl::UrDriver::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::function<void(bool)> handle_program_state,
+                         std::function<void(control::TrajectoryResult)> trajectory_done_cb, bool headless_mode,
                          std::unique_ptr<ToolCommSetup> tool_comm_setup, const uint32_t reverse_port,
                          const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time,
                          bool non_blocking_read, const std::string& reverse_ip)
@@ -156,19 +157,21 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_
   reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state));
   // TODO swap to configurable or static port
   trajectory_interface_.reset(new control::TrajectoryPointInterface(reverse_port + 10));
+  trajectory_interface_->setTrajectoryEndCallback(trajectory_done_cb);
 
   URCL_LOG_DEBUG("Initialization done");
 }
 
 urcl::UrDriver::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::function<void(bool)> handle_program_state,
+                         std::function<void(control::TrajectoryResult)> trajectory_done_cb, bool headless_mode,
                          std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum,
                          const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain,
                          double servoj_lookahead_time, bool non_blocking_read, const std::string& reverse_ip)
-  : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode,
-             std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time,
-             non_blocking_read, reverse_ip)
+  : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, trajectory_done_cb,
+             headless_mode, std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain,
+             servoj_lookahead_time, non_blocking_read, reverse_ip)
 {
   URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been "
                 "deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This "