Skip to content

Commit 2d70a9b

Browse files
URJalaURJala
committed
Create new URConfigurationController
And moved the get_robot_software_version service in to it.
1 parent bac64ae commit 2d70a9b

File tree

16 files changed

+257
-315
lines changed

16 files changed

+257
-315
lines changed

‎ur_controllers/CMakeLists.txt‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,10 +54,16 @@ generate_parameter_library(
5454
src/scaled_joint_trajectory_controller_parameters.yaml
5555
)
5656

57+
generate_parameter_library(
58+
ur_configuration_controller_parameters
59+
src/ur_configuration_controller_parameters.yaml
60+
)
61+
5762
add_library(${PROJECT_NAME} SHARED
5863
src/scaled_joint_trajectory_controller.cpp
5964
src/speed_scaling_state_broadcaster.cpp
60-
src/gpio_controller.cpp)
65+
src/gpio_controller.cpp
66+
src/ur_configuration_controller.cpp)
6167

6268
target_include_directories(${PROJECT_NAME} PRIVATE
6369
include
@@ -66,6 +72,7 @@ target_link_libraries(${PROJECT_NAME}
6672
gpio_controller_parameters
6773
speed_scaling_state_broadcaster_parameters
6874
scaled_joint_trajectory_controller_parameters
75+
ur_configuration_controller_parameters
6976
)
7077
ament_target_dependencies(${PROJECT_NAME}
7178
${THIS_PACKAGE_INCLUDE_DEPENDS}

‎ur_controllers/controller_plugins.xml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,4 +14,9 @@
1414
This controller publishes the Tool IO.
1515
</description>
1616
</class>
17+
<class name="ur_controllers/URConfigurationController" type="ur_controllers::URConfigurationController" base_class_type="controller_interface::ControllerInterface">
18+
<description>
19+
Controller used to get and change the configuration of the robot
20+
</description>
21+
</class>
1722
</library>

‎ur_controllers/include/ur_controllers/gpio_controller.hpp‎

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

6261
namespace ur_controllers
6362
{
@@ -80,8 +79,6 @@ enum CommandInterfaces
8079
ZERO_FTSENSOR_ASYNC_SUCCESS = 32,
8180
HAND_BACK_CONTROL_CMD = 33,
8281
HAND_BACK_CONTROL_ASYNC_SUCCESS = 34,
83-
GET_VERSION_CMD = 35,
84-
GET_VERSION_ASYNC_SUCCESS = 36
8582
};
8683

8784
enum StateInterfaces
@@ -103,10 +100,6 @@ enum StateInterfaces
103100
SAFETY_STATUS_BITS = 58,
104101
INITIALIZED_FLAG = 69,
105102
PROGRAM_RUNNING = 70,
106-
GET_VERSION_MAJOR = 71,
107-
GET_VERSION_MINOR = 72,
108-
GET_VERSION_BUGFIX = 73,
109-
GET_VERSION_BUILD = 74
110103
};
111104

112105
class GPIOController : public controller_interface::ControllerInterface
@@ -143,9 +136,6 @@ class GPIOController : public controller_interface::ControllerInterface
143136

144137
bool zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp);
145138

146-
bool getRobotSoftwareVersion(ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr req,
147-
ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp);
148-
149139
void publishIO();
150140

151141
void publishToolData();
@@ -173,7 +163,6 @@ class GPIOController : public controller_interface::ControllerInterface
173163
rclcpp::Service<ur_msgs::srv::SetIO>::SharedPtr set_io_srv_;
174164
rclcpp::Service<ur_msgs::srv::SetPayload>::SharedPtr set_payload_srv_;
175165
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr tare_sensor_srv_;
176-
rclcpp::Service<ur_msgs::srv::GetRobotSoftwareVersion>::SharedPtr get_robot_software_version_srv_;
177166

178167
std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::IOStates>> io_pub_;
179168
std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::ToolDataMsg>> tool_data_pub_;

‎ur_controllers/include/ur_controllers/ur_configuration_controller.hpp‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
// Copyright 2024, Universal Robots A/S
2+
//
3+
// Redistribution and use in source and binary forms, with or without
4+
// modification, are permitted provided that the following conditions are met:
5+
//
6+
// * Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
//
9+
// * Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
12+
//
13+
// * Neither the name of the {copyright_holder} nor the names of its
14+
// contributors may be used to endorse or promote products derived from
15+
// this software without specific prior written permission.
16+
//
17+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
// POSSIBILITY OF SUCH DAMAGE.
28+
29+
//----------------------------------------------------------------------
30+
/*!\file
31+
*
32+
* \author Jacob Larsen [email protected]
33+
* \date 2024-07-11
34+
*
35+
*
36+
*
37+
*
38+
*/
39+
//----------------------------------------------------------------------
40+
41+
#ifndef UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_
42+
#define UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_
43+
44+
#include <memory>
45+
46+
#include <controller_interface/controller_interface.hpp>
47+
48+
#include "ur_msgs/srv/get_robot_software_version.hpp"
49+
#include "ur_configuration_controller_parameters.hpp"
50+
51+
namespace ur_controllers
52+
{
53+
54+
enum StateInterfaces
55+
{
56+
ROBOT_VERSION_MAJOR,
57+
ROBOT_VERSION_MINOR,
58+
ROBOT_VERSION_BUILD,
59+
ROBOT_VERSION_BUGFIX
60+
};
61+
62+
class URConfigurationController : public controller_interface::ControllerInterface
63+
{
64+
public:
65+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
66+
67+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
68+
69+
controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
70+
71+
CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
72+
73+
CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
74+
75+
CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
76+
77+
CallbackReturn on_init() override;
78+
79+
private:
80+
rclcpp::Service<ur_msgs::srv::GetRobotSoftwareVersion>::SharedPtr get_robot_software_version_srv_;
81+
bool getRobotSoftwareVersion(ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr req,
82+
ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp);
83+
84+
std::shared_ptr<ur_configuration_controller::ParamListener> param_listener_;
85+
ur_configuration_controller::Params params_;
86+
};
87+
} // namespace ur_controllers
88+
89+
#endif // UR_CONTROLLERS__UR_CONFIGURATION_CONTROLLER_HPP_

‎ur_controllers/src/gpio_controller.cpp‎

Lines changed: 0 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -97,10 +97,6 @@ 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-
// Robot software version service
101-
config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_cmd");
102-
config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_async_success");
103-
104100
return config;
105101
}
106102

@@ -322,10 +318,6 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre
322318
tare_sensor_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
323319
"~/zero_ftsensor",
324320
std::bind(&GPIOController::zeroFTSensor, this, std::placeholders::_1, std::placeholders::_2));
325-
326-
get_robot_software_version_srv_ = get_node()->create_service<ur_msgs::srv::GetRobotSoftwareVersion>(
327-
"~/get_robot_software_version",
328-
std::bind(&GPIOController::getRobotSoftwareVersion, this, std::placeholders::_1, std::placeholders::_2));
329321
} catch (...) {
330322
return LifecycleNodeInterface::CallbackReturn::ERROR;
331323
}
@@ -534,28 +526,6 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r
534526
return true;
535527
}
536528

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

‎ur_controllers/src/ur_configuration_controller.cpp‎

Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,122 @@
1+
// Copyright 2024, Universal Robots A/S
2+
//
3+
// Redistribution and use in source and binary forms, with or without
4+
// modification, are permitted provided that the following conditions are met:
5+
//
6+
// * Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
//
9+
// * Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
12+
//
13+
// * Neither the name of the {copyright_holder} nor the names of its
14+
// contributors may be used to endorse or promote products derived from
15+
// this software without specific prior written permission.
16+
//
17+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
// POSSIBILITY OF SUCH DAMAGE.
28+
29+
//----------------------------------------------------------------------
30+
/*!\file
31+
*
32+
* \author Jacob Larsen [email protected]
33+
* \date 2024-07-11
34+
*
35+
*
36+
*
37+
*
38+
*/
39+
//----------------------------------------------------------------------
40+
41+
#include <ur_controllers/ur_configuration_controller.hpp>
42+
namespace ur_controllers
43+
{
44+
45+
controller_interface::CallbackReturn URConfigurationController::on_init()
46+
{
47+
param_listener_ = std::make_shared<ur_configuration_controller::ParamListener>(get_node());
48+
params_ = param_listener_->get_params();
49+
return controller_interface::CallbackReturn::SUCCESS;
50+
}
51+
52+
controller_interface::CallbackReturn
53+
URConfigurationController::on_configure(const rclcpp_lifecycle::State& /* previous_state */)
54+
{
55+
return controller_interface::CallbackReturn::SUCCESS;
56+
}
57+
58+
controller_interface::InterfaceConfiguration URConfigurationController::command_interface_configuration() const
59+
{
60+
// No command interfaces currently
61+
controller_interface::InterfaceConfiguration config;
62+
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
63+
64+
const std::string tf_prefix = params_.tf_prefix;
65+
66+
return config;
67+
}
68+
69+
controller_interface::InterfaceConfiguration URConfigurationController::state_interface_configuration() const
70+
{
71+
controller_interface::InterfaceConfiguration config;
72+
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
73+
74+
const std::string tf_prefix = params_.tf_prefix;
75+
76+
config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_major");
77+
config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_minor");
78+
config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_build");
79+
config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_bugfix");
80+
81+
return config;
82+
}
83+
84+
controller_interface::return_type URConfigurationController::update(const rclcpp::Time& /* time */,
85+
const rclcpp::Duration& /* period */)
86+
{
87+
return controller_interface::return_type::OK;
88+
}
89+
90+
controller_interface::CallbackReturn
91+
URConfigurationController::on_activate(const rclcpp_lifecycle::State& /* previous_state */)
92+
{
93+
get_robot_software_version_srv_ = get_node()->create_service<ur_msgs::srv::GetRobotSoftwareVersion>(
94+
"~/get_robot_software_version", std::bind(&URConfigurationController::getRobotSoftwareVersion, this,
95+
std::placeholders::_1, std::placeholders::_2));
96+
97+
return controller_interface::CallbackReturn::SUCCESS;
98+
}
99+
100+
controller_interface::CallbackReturn
101+
URConfigurationController::on_deactivate(const rclcpp_lifecycle::State& /* previous_state */)
102+
{
103+
get_robot_software_version_srv_.reset();
104+
return controller_interface::CallbackReturn::SUCCESS;
105+
}
106+
107+
bool URConfigurationController::getRobotSoftwareVersion(
108+
ur_msgs::srv::GetRobotSoftwareVersion::Request::SharedPtr /*req*/,
109+
ur_msgs::srv::GetRobotSoftwareVersion::Response::SharedPtr resp)
110+
{
111+
resp->major = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_MAJOR].get_value());
112+
resp->minor = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_MINOR].get_value());
113+
resp->bugfix = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_BUGFIX].get_value());
114+
resp->build = static_cast<uint32_t>(state_interfaces_[StateInterfaces::ROBOT_VERSION_BUILD].get_value());
115+
116+
return true;
117+
}
118+
} // namespace ur_controllers
119+
120+
#include "pluginlib/class_list_macros.hpp"
121+
122+
PLUGINLIB_EXPORT_CLASS(ur_controllers::URConfigurationController, controller_interface::ControllerInterface)

‎ur_controllers/src/ur_configuration_controller_parameters.yaml‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
ur_configuration_controller:
2+
tf_prefix: {
3+
type: string,
4+
default_value: "",
5+
description: "Urdf prefix of the corresponding arm"
6+
}

0 commit comments

Comments
 (0)