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..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,6 +386,11 @@ 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 +919,21 @@ end return true; } +bool HardwareInterface::resetRevolutionCounter(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res) +{ + 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 + handleRobotProgramState(true); + mtx.unlock(); + return true; +} + void HardwareInterface::commandCallback(const std_msgs::StringConstPtr& msg) { std::string str = msg->data;