Merge pull request #60 from MGBla/fb_dashboard_client_fix

Dashboard client fixes

Author: destogl

ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.h

@@ -21,6 +21,8 @@
  *
  * \author  Felix Exner [email protected]
  * \date    2019-10-21
+ * \author  Marvin Große Besselmann [email protected]
+ * \date    2021-03-22
  *
  */
 //----------------------------------------------------------------------
@@ -36,6 +38,7 @@
 
 // UR client library
 #include <ur_client_library/ur/dashboard_client.h>
+#include <ur_client_library/exceptions.h>
 #include <ur_dashboard_msgs/msg/program_state.hpp>
 #include <ur_dashboard_msgs/srv/add_to_log.hpp>
 #include <ur_dashboard_msgs/srv/get_loaded_program.hpp>
@@ -74,12 +77,20 @@ class DashboardClientROS
                                                                                       const std::string& expected)
   {
     rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service = node_->create_service<std_srvs::srv::Trigger>(
-        "topic", [&, command, expected](const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
-                                        const std::shared_ptr<std_srvs::srv::Trigger::Response> resp) {
-          resp->message = this->client_.sendAndReceive(command);
-          resp->success = std::regex_match(resp->message, std::regex(expected));
+        topic, [&, command, expected](const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
+                                      const std::shared_ptr<std_srvs::srv::Trigger::Response> resp) {
+          try
+          {
+            resp->message = this->client_.sendAndReceive(command);
+            resp->success = std::regex_match(resp->message, std::regex(expected));
+          }
+          catch (const urcl::UrException& e)
+          {
+            RCLCPP_ERROR(rclcpp::get_logger("Dashboard_Client"), "Service Call failed: '%s'", e.what());
+            resp->message = e.what();
+            resp->success = false;
+          }
         });
-
     return service;
   }
 

ur_robot_driver/include/ur_robot_driver/hardware_interface.h

@@ -21,6 +21,7 @@
  *
  * \author  Lovro Ivanov [email protected]
  * \author  Andy Zelenak [email protected]
+ * \author  Marvin Große Besselmann [email protected]
  * \date    2019-04-11
  *
  */
@@ -154,6 +155,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   double pausing_ramp_up_increment_;
 
   std::unique_ptr<urcl::UrDriver> ur_driver_;
+  std::unique_ptr<ur_robot_driver::DashboardClientROS> dashboard_client_;
 };
 }  // namespace ur_robot_driver