Skip to content

Commit dffccc6

Browse files
URJalaFelix Exner (fexner)
URJala
authored and
Felix Exner (fexner)
committed
Implemented get_version service
1 parent 4c2971d commit dffccc6

File tree

5 files changed

+87
-0
lines changed

5 files changed

+87
-0
lines changed

‎ur_controllers/include/ur_controllers/gpio_controller.hpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -57,6 +57,7 @@
5757
#include "rclcpp/duration.hpp"
5858
#include "std_msgs/msg/bool.hpp"
5959
#include "gpio_controller_parameters.hpp"
60+
#include "ur_msgs/srv/get_version.hpp"
6061

6162
namespace ur_controllers
6263
{
@@ -79,6 +80,12 @@ enum CommandInterfaces
7980
ZERO_FTSENSOR_ASYNC_SUCCESS = 32,
8081
HAND_BACK_CONTROL_CMD = 33,
8182
HAND_BACK_CONTROL_ASYNC_SUCCESS = 34,
83+
GET_VERSION_CMD = 39,
84+
GET_VERSION_ASYNC_SUCCESS = 40,
85+
GET_VERSION_MAJOR = 41,
86+
GET_VERSION_MINOR = 42,
87+
GET_VERSION_BUGFIX = 43,
88+
GET_VERSION_BUILD = 44
8289
};
8390

8491
enum StateInterfaces
@@ -136,6 +143,8 @@ class GPIOController : public controller_interface::ControllerInterface
136143

137144
bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp);
138145

146+
bool getVersion(ur_msgs::srv::GetVersion::Request::SharedPtr req, ur_msgs::srv::GetVersion::Response::SharedPtr resp);
147+
139148
void publishIO();
140149

141150
void publishToolData();
@@ -163,6 +172,7 @@ class GPIOController : public controller_interface::ControllerInterface
163172
rclcpp::Service<ur_msgs::srv::SetIO>::SharedPtr set_io_srv_;
164173
rclcpp::Service<ur_msgs::srv::SetPayload>::SharedPtr set_payload_srv_;
165174
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr tare_sensor_srv_;
175+
rclcpp::Service<ur_msgs::srv::GetVersion>::SharedPtr get_version_srv_;
166176

167177
std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::IOStates>> io_pub_;
168178
std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::ToolDataMsg>> tool_data_pub_;

‎ur_controllers/src/gpio_controller.cpp‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,14 @@ controller_interface::InterfaceConfiguration GPIOController::command_interface_c
9797
config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_cmd");
9898
config.names.emplace_back(tf_prefix + "hand_back_control/hand_back_control_async_success");
9999

100+
// Get version service
101+
config.names.emplace_back(tf_prefix + "get_version/get_version_cmd");
102+
config.names.emplace_back(tf_prefix + "get_version/get_version_async_success");
103+
config.names.emplace_back(tf_prefix + "get_version/get_version_major");
104+
config.names.emplace_back(tf_prefix + "get_version/get_version_minor");
105+
config.names.emplace_back(tf_prefix + "get_version/get_version_bugfix");
106+
config.names.emplace_back(tf_prefix + "get_version/get_version_build");
107+
100108
return config;
101109
}
102110

@@ -312,6 +320,9 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre
312320
tare_sensor_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
313321
"~/zero_ftsensor",
314322
std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2));
323+
324+
get_version_srv_ = get_node()->create_service<ur_msgs::srv::GetVersion>(
325+
"~/get_version", std::bind(&GPIOController::getVersion, this, std::placeholders::_1, std::placeholders::_2));
315326
} catch (...) {
316327
return LifecycleNodeInterface::CallbackReturn::ERROR;
317328
}
@@ -520,6 +531,28 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r
520531
return true;
521532
}
522533

534+
bool GPIOController::getVersion(ur_msgs::srv::GetVersion::Request::SharedPtr /*req*/,
535+
ur_msgs::srv::GetVersion::Response::SharedPtr resp)
536+
{
537+
// reset success flag
538+
command_interfaces_[CommandInterfaces::GET_VERSION_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
539+
540+
// call the service in the hardware
541+
command_interfaces_[CommandInterfaces::GET_VERSION_CMD].set_value(1.0);
542+
543+
if (!waitForAsyncCommand(
544+
[&]() { return command_interfaces_[CommandInterfaces::GET_VERSION_ASYNC_SUCCESS].get_value(); })) {
545+
RCLCPP_WARN(get_node()->get_logger(), "Could not verify software version of robot.");
546+
}
547+
548+
resp->major = static_cast<uint32_t>(command_interfaces_[CommandInterfaces::GET_VERSION_MAJOR].get_value());
549+
resp->minor = static_cast<uint32_t>(command_interfaces_[CommandInterfaces::GET_VERSION_MINOR].get_value());
550+
resp->bugfix = static_cast<uint32_t>(command_interfaces_[CommandInterfaces::GET_VERSION_BUGFIX].get_value());
551+
resp->build = static_cast<uint32_t>(command_interfaces_[CommandInterfaces::GET_VERSION_BUILD].get_value());
552+
553+
return true;
554+
}
555+
523556
void GPIOController::initMsgs()
524557
{
525558
io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size());

‎ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -191,6 +191,12 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
191191
bool initialized_;
192192
double system_interface_initialized_;
193193
bool async_thread_shutdown_;
194+
double get_version_async_success_;
195+
double get_version_cmd_;
196+
double get_version_major_;
197+
double get_version_minor_;
198+
double get_version_bugfix_;
199+
double get_version_build_;
194200

195201
// payload stuff
196202
urcl::vector3d_t payload_center_of_gravity_;

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -298,6 +298,24 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
298298
command_interfaces.emplace_back(hardware_interface::CommandInterface(
299299
tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_));
300300

301+
command_interfaces.emplace_back(
302+
hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_cmd", &get_version_cmd_));
303+
304+
command_interfaces.emplace_back(hardware_interface::CommandInterface(
305+
tf_prefix + "get_version", "get_version_async_success", &get_version_async_success_));
306+
307+
command_interfaces.emplace_back(
308+
hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_major", &get_version_major_));
309+
310+
command_interfaces.emplace_back(
311+
hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_minor", &get_version_minor_));
312+
313+
command_interfaces.emplace_back(
314+
hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_bugfix", &get_version_bugfix_));
315+
316+
command_interfaces.emplace_back(
317+
hardware_interface::CommandInterface(tf_prefix + "get_version", "get_version_build", &get_version_build_));
318+
301319
return command_interfaces;
302320
}
303321

@@ -603,6 +621,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
603621
resend_robot_program_cmd_ = NO_NEW_CMD_;
604622
zero_ftsensor_cmd_ = NO_NEW_CMD_;
605623
hand_back_control_cmd_ = NO_NEW_CMD_;
624+
get_version_cmd_ = NO_NEW_CMD_;
606625
initialized_ = true;
607626
}
608627

@@ -727,6 +746,16 @@ void URPositionHardwareInterface::checkAsyncIO()
727746
zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor();
728747
zero_ftsensor_cmd_ = NO_NEW_CMD_;
729748
}
749+
750+
if (!std::isnan(get_version_cmd_) && ur_driver_ != nullptr) {
751+
urcl::VersionInformation version_info = ur_driver_->getVersion();
752+
get_version_cmd_ = NO_NEW_CMD_;
753+
get_version_major_ = version_info.major;
754+
get_version_minor_ = version_info.minor;
755+
get_version_bugfix_ = version_info.bugfix;
756+
get_version_build_ = version_info.build;
757+
get_version_async_success_ = 1.0;
758+
}
730759
}
731760

732761
void URPositionHardwareInterface::updateNonDoubleValues()

‎ur_robot_driver/urdf/ur.ros2_control.xacro‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -223,6 +223,15 @@
223223
<command_interface name="zero_ftsensor_async_success"/>
224224
</gpio>
225225

226+
<gpio name="${tf_prefix}get_version">
227+
<command_interface name="get_version_cmd"/>
228+
<command_interface name="get_version_async_success"/>
229+
<command_interface name="get_version_major"/>
230+
<command_interface name="get_version_minor"/>
231+
<command_interface name="get_version_bugfix"/>
232+
<command_interface name="get_version_build"/>
233+
</gpio>
234+
226235
<gpio name="${tf_prefix}system_interface">
227236
<state_interface name="initialized"/>
228237
</gpio>

0 commit comments

Comments
 (0)