From eaac14424412a7cb559549bf5104ed64a56e7de5 Mon Sep 17 00:00:00 2001 From: Cristian Beltran Date: Wed, 24 Mar 2021 12:07:39 +0900 Subject: [PATCH 1/3] Adding reset revolution counter service (#325) Reset the revolution counter of the robot's wrist_3_link --- ur_robot_driver/doc/ROS_INTERFACE.md | 4 ++++ .../include/ur_robot_driver/hardware_interface.h | 2 ++ ur_robot_driver/src/hardware_interface.cpp | 13 +++++++++++++ 3 files changed, 19 insertions(+) diff --git a/ur_robot_driver/doc/ROS_INTERFACE.md b/ur_robot_driver/doc/ROS_INTERFACE.md index 696cbb2ba..02847cf23 100644 --- a/ur_robot_driver/doc/ROS_INTERFACE.md +++ b/ur_robot_driver/doc/ROS_INTERFACE.md @@ -760,6 +760,10 @@ Setup the mounted payload through a ROS service Calling this service will zero the robot's ftsensor. Note: On e-Series robots this will only work when the robot is in remote-control mode. +##### reset_revolution_counter ([std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)) + +Calling this service will reset the revolution counter for the robot's wrist_3_link. Note: On e-Series robots this will only work when the robot is in remote-control mode. + #### Parameters ##### dashboard/receive_timeout (Required) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h index aff9e09d7..b6b60e4da 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.h @@ -200,6 +200,7 @@ class HardwareInterface : public hardware_interface::RobotHW bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& res); bool resendRobotProgram(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); bool zeroFTSensor(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); + bool resetRevolutionCounter(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); void commandCallback(const std_msgs::StringConstPtr& msg); std::unique_ptr ur_driver_; @@ -215,6 +216,7 @@ class HardwareInterface : public hardware_interface::RobotHW ros::ServiceServer deactivate_srv_; ros::ServiceServer tare_sensor_srv_; + ros::ServiceServer reset_revolution_counter_srv_; ros::ServiceServer set_payload_srv_; hardware_interface::JointStateInterface js_interface_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 7ad32c88e..59de12601 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -384,6 +384,10 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw // work when the robot is in remote-control mode. tare_sensor_srv_ = robot_hw_nh.advertiseService("zero_ftsensor", &HardwareInterface::zeroFTSensor, this); + // Calling this service will reset the revolution counter for the robot's wrist_3_link. Note: On e-Series robots this will only + // work when the robot is in remote-control mode. + reset_revolution_counter_srv_ = robot_hw_nh.advertiseService("reset_revolution_counter", &HardwareInterface::resetRevolutionCounter, this); + ur_driver_->startRTDECommunication(); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface"); @@ -912,6 +916,15 @@ end return true; } +bool HardwareInterface::resetRevolutionCounter(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res) +{ + res.success = this->ur_driver_->sendScript(R"(sec resetRevolutionCounter(): + reset_revolution_counter() +end +)"); + return true; +} + void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg) { std::string str = msg->data; From 217d3d0512cb654b373760b78eca46b1641968a2 Mon Sep 17 00:00:00 2001 From: Cristian Beltran Date: Thu, 25 Mar 2021 14:58:19 +0900 Subject: [PATCH 2/3] Stopping controllers before resetting revolution counter (#325) --- ur_robot_driver/src/hardware_interface.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 59de12601..f0c1572fc 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -918,10 +918,16 @@ end bool HardwareInterface::resetRevolutionCounter(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res) { + std_msgs::Bool msg; + msg.data = false; + program_state_pub_.publish(msg); res.success = this->ur_driver_->sendScript(R"(sec resetRevolutionCounter(): reset_revolution_counter() end )"); + ros::Duration(0.1).sleep(); // wait for the reset to be done + msg.data = true; + program_state_pub_.publish(msg); return true; } From 3df3d3134bdda2fb90d8e10d5ec9a46a3b6f3985 Mon Sep 17 00:00:00 2001 From: Cristian Beltran Date: Fri, 26 Mar 2021 15:26:28 +0900 Subject: [PATCH 3/3] adding mutex and calling handleRobotProgramState (#325) --- ur_robot_driver/src/hardware_interface.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index f0c1572fc..69b25bcb0 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -33,6 +33,8 @@ #include +#include + using industrial_robot_status_interface::RobotMode; using industrial_robot_status_interface::TriState; // using namespace urcl::rtde_interface; @@ -384,9 +386,10 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw // work when the robot is in remote-control mode. tare_sensor_srv_ = robot_hw_nh.advertiseService("zero_ftsensor", &HardwareInterface::zeroFTSensor, this); - // Calling this service will reset the revolution counter for the robot's wrist_3_link. Note: On e-Series robots this will only - // work when the robot is in remote-control mode. - reset_revolution_counter_srv_ = robot_hw_nh.advertiseService("reset_revolution_counter", &HardwareInterface::resetRevolutionCounter, this); + // Calling this service will reset the revolution counter for the robot's wrist_3_link. Note: On e-Series robots this + // will only work when the robot is in remote-control mode. + reset_revolution_counter_srv_ = + robot_hw_nh.advertiseService("reset_revolution_counter", &HardwareInterface::resetRevolutionCounter, this); ur_driver_->startRTDECommunication(); ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_robot_driver hardware_interface"); @@ -918,16 +921,16 @@ end bool HardwareInterface::resetRevolutionCounter(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res) { - std_msgs::Bool msg; - msg.data = false; - program_state_pub_.publish(msg); + std::mutex mtx; + mtx.lock(); + handleRobotProgramState(false); res.success = this->ur_driver_->sendScript(R"(sec resetRevolutionCounter(): reset_revolution_counter() end )"); - ros::Duration(0.1).sleep(); // wait for the reset to be done - msg.data = true; - program_state_pub_.publish(msg); + ros::Duration(0.1).sleep(); // wait for the reset to be done + handleRobotProgramState(true); + mtx.unlock(); return true; }