Skip to content

Commit 7162aa7

Browse files
livanov93livanov93
authored
Merge pull request #38 from MGBla/fb_speed_scaling_state_controller
SpeedScalingStateController
2 parents acebc7f + d29784e commit 7162aa7

File tree

7 files changed

+164
-12
lines changed

7 files changed

+164
-12
lines changed

‎ur_controllers/CMakeLists.txt‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,18 +11,26 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
1111
endif()
1212

1313
find_package(ament_cmake REQUIRED)
14+
find_package(control_msgs REQUIRED)
1415
find_package(controller_interface REQUIRED)
1516
find_package(joint_trajectory_controller REQUIRED)
1617
find_package(pluginlib REQUIRED)
18+
find_package(rclcpp_lifecycle REQUIRED)
19+
find_package(realtime_tools REQUIRED)
20+
find_package(sensor_msgs REQUIRED)
1721
find_package(std_msgs REQUIRED)
1822
find_package(geometry_msgs REQUIRED)
1923
find_package(rclcpp_lifecycle REQUIRED)
2024
find_package(rcutils REQUIRED)
2125

2226
set(THIS_PACKAGE_INCLUDE_DEPENDS
27+
control_msgs
2328
controller_interface
2429
joint_trajectory_controller
2530
pluginlib
31+
rclcpp_lifecycle
32+
realtime_tools
33+
sensor_msgs
2634
std_msgs
2735
geometry_msgs
2836
rclcpp_lifecycle
@@ -64,6 +72,8 @@ install(FILES controller_plugins.xml
6472
)
6573

6674
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
75+
ament_export_include_directories(include)
76+
ament_export_libraries(${PROJECT_NAME})
6777

6878
ament_export_include_directories(
6979
include

‎ur_controllers/controller_plugins.xml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,4 +4,9 @@
44
The force torque state controller publishes the current force and torques
55
</description>
66
</class>
7+
<class name="ur_controllers/SpeedScalingStateController" type="ur_controllers::SpeedScalingStateController" base_class_type="controller_interface::ControllerInterface">
8+
<description>
9+
This controller publishes the readings of all available speed scaling factors.
10+
</description>
11+
</class>
712
</library>

‎ur_controllers/include/ur_controllers/speed_scaling_state_controller.h‎

Lines changed: 34 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -19,19 +19,50 @@
1919
//----------------------------------------------------------------------
2020
/*!\file
2121
*
22-
* \author Felix Exner exner@fzi.de
23-
* \date 2019-04-17
22+
* \author Marvin Große Besselmann grosse@fzi.de
23+
* \date 2021-02-10
2424
*
2525
*/
2626
//----------------------------------------------------------------------
2727

2828
#ifndef UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED
2929
#define UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED
3030

31+
#include "controller_interface/controller_interface.hpp"
32+
33+
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
34+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
35+
#include "std_msgs/msg/float64.hpp"
36+
3137
namespace ur_controllers
3238
{
33-
class SpeedScalingStateController
39+
class SpeedScalingStateController : public controller_interface::ControllerInterface
3440
{
41+
public:
42+
SpeedScalingStateController();
43+
44+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
45+
46+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
47+
48+
controller_interface::return_type update() override;
49+
50+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
51+
on_configure(const rclcpp_lifecycle::State& previous_state) override;
52+
53+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
54+
on_activate(const rclcpp_lifecycle::State& previous_state) override;
55+
56+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
57+
on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
58+
59+
protected:
60+
std::vector<std::string> sensor_names_;
61+
rclcpp::Time last_publish_time_;
62+
double publish_rate_;
63+
64+
std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64>> speed_scaling_state_publisher_;
65+
std_msgs::msg::Float64 speed_scaling_state_msg_;
3566
};
3667
} // namespace ur_controllers
3768
#endif // ifndef UR_CONTROLLERS_SPEED_SCALING_STATE_CONTROLLER_H_INCLUDED

‎ur_controllers/package.xml‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -5,22 +5,22 @@
55
<version>0.0.0</version>
66
<description>Provides controllers that use the speed scaling interface of Universal Robots.</description>
77

8-
<maintainer email="exner@fzi.de">Felix Exner</maintainer>
8+
<maintainer email="grosse@fzi.de">Marvin Große Besselmann</maintainer>
99

1010
<license>Apache-2.0</license>
1111

1212
<buildtool_depend>ament_cmake</buildtool_depend>
1313

14-
<build_depend>pluginlib</build_depend>
15-
<build_depend>rcutils</build_depend>
16-
17-
<exec_depend>pluginlib</exec_depend>
18-
<exec_depend>rcutils</exec_depend>
1914

15+
<depend>control_msgs</depend>
2016
<depend>controller_interface</depend>
21-
<depend>rclcpp_lifecycle</depend>
2217
<depend>geometry_msgs</depend>
2318
<depend>joint_trajectory_controller</depend>
19+
<depend>pluginlib</depend>
20+
<depend>rclcpp_lifecycle</depend>
21+
<depend>rcutils</depend>
22+
<depend>realtime_tools</depend>
23+
<depend>sensor_msgs</depend>
2424
<depend>std_msgs</depend>
2525
<depend>std_srvs</depend>
2626
<depend>ur_client_library</depend>

‎ur_controllers/src/speed_scaling_state_controller.cpp‎

Lines changed: 98 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,10 +19,106 @@
1919
//----------------------------------------------------------------------
2020
/*!\file
2121
*
22-
* \author Felix Exner exner@fzi.de
23-
* \date 2019-04-17
22+
* \author Marvin Große Besselmann grosse@fzi.de
23+
* \date 2021-02-10
2424
*
2525
*/
2626
//----------------------------------------------------------------------
2727

2828
#include "ur_controllers/speed_scaling_state_controller.h"
29+
30+
#include "hardware_interface/types/hardware_interface_return_values.hpp"
31+
#include "hardware_interface/types/hardware_interface_type_values.hpp"
32+
#include "rclcpp/clock.hpp"
33+
#include "rclcpp/qos.hpp"
34+
#include "rclcpp/qos_event.hpp"
35+
#include "rclcpp/time.hpp"
36+
#include "rclcpp_lifecycle/lifecycle_node.hpp"
37+
#include "rcpputils/split.hpp"
38+
#include "rcutils/logging_macros.h"
39+
40+
namespace rclcpp_lifecycle
41+
{
42+
class State;
43+
} // namespace rclcpp_lifecycle
44+
45+
namespace ur_controllers
46+
{
47+
SpeedScalingStateController::SpeedScalingStateController()
48+
{
49+
}
50+
51+
controller_interface::InterfaceConfiguration SpeedScalingStateController::command_interface_configuration() const
52+
{
53+
return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::NONE };
54+
}
55+
56+
controller_interface::InterfaceConfiguration SpeedScalingStateController::state_interface_configuration() const
57+
{
58+
controller_interface::InterfaceConfiguration config;
59+
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
60+
config.names.push_back("speed_scaling/speed_scaling_factor");
61+
return config;
62+
}
63+
64+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
65+
SpeedScalingStateController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
66+
{
67+
node_->declare_parameter("state_publish_rate");
68+
69+
if (!node_->get_parameter("state_publish_rate", publish_rate_))
70+
{
71+
RCLCPP_INFO(get_node()->get_logger(), "Parameter 'state_publish_rate' not set");
72+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
73+
}
74+
else
75+
{
76+
RCLCPP_INFO(get_node()->get_logger(), "Publisher rate set to : %.1f Hz", publish_rate_);
77+
}
78+
79+
try
80+
{
81+
speed_scaling_state_publisher_ =
82+
get_node()->create_publisher<std_msgs::msg::Float64>("speed_scaling_factor", rclcpp::SystemDefaultsQoS());
83+
}
84+
catch (const std::exception& e)
85+
{
86+
// get_node() may throw, logging raw here
87+
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
88+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
89+
}
90+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
91+
}
92+
93+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
94+
SpeedScalingStateController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
95+
{
96+
last_publish_time_ = node_->now();
97+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
98+
}
99+
100+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
101+
SpeedScalingStateController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
102+
{
103+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
104+
}
105+
106+
controller_interface::return_type SpeedScalingStateController::update()
107+
{
108+
if (publish_rate_ > 0.0 && (node_->now() - last_publish_time_) > rclcpp::Duration(1.0 / publish_rate_, 0.0))
109+
{
110+
// Speed scaling is the only interface of the controller
111+
speed_scaling_state_msg_.data = state_interfaces_[0].get_value();
112+
113+
// publish
114+
speed_scaling_state_publisher_->publish(speed_scaling_state_msg_);
115+
last_publish_time_ = node_->now();
116+
}
117+
return controller_interface::return_type::SUCCESS;
118+
}
119+
120+
} // namespace ur_controllers
121+
122+
#include "pluginlib/class_list_macros.hpp"
123+
124+
PLUGINLIB_EXPORT_CLASS(ur_controllers::SpeedScalingStateController, controller_interface::ControllerInterface)

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -128,6 +128,9 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
128128
info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &urcl_joint_efforts_[i]));
129129
}
130130

131+
state_interfaces.emplace_back(
132+
hardware_interface::StateInterface("speed_scaling", "speed_scaling_factor", &speed_scaling_combined_));
133+
131134
for (auto& sensor : info_.sensors)
132135
{
133136
for (uint j = 0; j < sensor.state_interfaces.size(); ++j)

‎ur_ros2_control_demos/config/ur_ros2_control.yaml‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,9 @@ controller_manager:
88
joint_state_controller:
99
type: joint_state_controller/JointStateController
1010

11+
speed_scaling_state_controller:
12+
type: ur_controllers/SpeedScalingStateController
13+
1114
ur_joint_trajectory_controller:
1215
type: joint_trajectory_controller/JointTrajectoryController
1316

@@ -30,6 +33,10 @@ ur_joint_trajectory_controller:
3033
stopped_velocity_tolerance: 0.0
3134
goal_time: 0.0
3235

36+
speed_scaling_state_controller:
37+
ros__parameters:
38+
state_publish_rate: 100.0
39+
3340
forward_command_controller_position:
3441
ros__parameters:
3542
joints:

0 commit comments

Comments
 (0)