Skip to content

Commit 1f1fefc

Browse files
livanov93livanov93
committed
Introduce cartesian twist controller.
1 parent 427e86b commit 1f1fefc

File tree

6 files changed

+374
-0
lines changed

6 files changed

+374
-0
lines changed

‎cartesian_controllers/CMakeLists.txt‎

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(cartesian_controllers)
3+
4+
# Default to C++14
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 14)
7+
endif()
8+
9+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10+
add_compile_options(-Wall -Wextra -Wpedantic)
11+
endif()
12+
13+
# find dependencies
14+
find_package(ament_cmake REQUIRED)
15+
find_package(ament_cmake_ros REQUIRED)
16+
find_package(controller_interface REQUIRED)
17+
find_package(hardware_interface REQUIRED)
18+
find_package(rclcpp REQUIRED)
19+
find_package(rclcpp_lifecycle REQUIRED)
20+
find_package(realtime_tools REQUIRED)
21+
find_package(geometry_msgs REQUIRED)
22+
find_package(std_msgs REQUIRED)
23+
find_package(pluginlib REQUIRED)
24+
25+
add_library(twist_controller src/twist_controller.cpp)
26+
target_include_directories(twist_controller PUBLIC
27+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
28+
$<INSTALL_INTERFACE:include>)
29+
ament_target_dependencies(
30+
twist_controller
31+
"controller_interface"
32+
"hardware_interface"
33+
"rclcpp"
34+
"rclcpp_lifecycle"
35+
"realtime_tools"
36+
"geometry_msgs"
37+
"std_msgs"
38+
"pluginlib"
39+
)
40+
41+
# Causes the visibility macros to use dllexport rather than dllimport,
42+
# which is appropriate when building the dll but not consuming it.
43+
target_compile_definitions(twist_controller PRIVATE "CARTESIAN_CONTROLLERS_BUILDING_LIBRARY")
44+
pluginlib_export_plugin_description_file(controller_interface twist_controller_plugin.xml)
45+
46+
install(
47+
DIRECTORY include/
48+
DESTINATION include
49+
)
50+
install(
51+
TARGETS twist_controller
52+
EXPORT export_${PROJECT_NAME}
53+
ARCHIVE DESTINATION lib
54+
LIBRARY DESTINATION lib
55+
RUNTIME DESTINATION bin
56+
)
57+
58+
if(BUILD_TESTING)
59+
find_package(ament_cmake_gmock REQUIRED)
60+
find_package(controller_manager REQUIRED)
61+
find_package(hardware_interface REQUIRED)
62+
find_package(ros2_control_test_assets REQUIRED)
63+
endif()
64+
65+
ament_export_dependencies(
66+
controller_interface
67+
hardware_interface
68+
rclcpp
69+
rclcpp_lifecycle
70+
realtime_tools
71+
std_msgs
72+
geometry_msgs
73+
)
74+
75+
ament_export_include_directories(
76+
include
77+
)
78+
ament_export_libraries(
79+
twist_controller
80+
)
81+
ament_export_targets(
82+
export_${PROJECT_NAME}
83+
)
84+
85+
ament_package()

‎cartesian_controllers/include/cartesian_controllers/twist_controller.hpp‎

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
// Copyright 2022 VoodooIT, sole proprietorship
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef CARTESIAN_CONTROLLERS__TWIST_CONTROLLER_HPP_
16+
#define CARTESIAN_CONTROLLERS__TWIST_CONTROLLER_HPP_
17+
18+
#include <memory>
19+
#include <string>
20+
#include <vector>
21+
22+
#include "controller_interface/controller_interface.hpp"
23+
#include "geometry_msgs/msg/twist_stamped.hpp"
24+
#include "realtime_tools/realtime_buffer.h"
25+
26+
#include "cartesian_controllers/visibility_control.h"
27+
28+
namespace cartesian_controllers
29+
{
30+
using CmdType = geometry_msgs::msg::TwistStamped;
31+
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
32+
class TwistController : public controller_interface::ControllerInterface
33+
{
34+
public:
35+
CARTESIAN_CONTROLLERS_PUBLIC
36+
TwistController();
37+
38+
CARTESIAN_CONTROLLERS_PUBLIC
39+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
40+
41+
CARTESIAN_CONTROLLERS_PUBLIC
42+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
43+
44+
CARTESIAN_CONTROLLERS_PUBLIC
45+
CallbackReturn on_init() override;
46+
47+
CARTESIAN_CONTROLLERS_PUBLIC
48+
controller_interface::return_type update(
49+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
50+
51+
CARTESIAN_CONTROLLERS_PUBLIC
52+
CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
53+
54+
CARTESIAN_CONTROLLERS_PUBLIC
55+
CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
56+
57+
CARTESIAN_CONTROLLERS_PUBLIC
58+
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
59+
60+
protected:
61+
std::string joint_name_;
62+
std::vector<std::string> interface_names_;
63+
64+
realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>> rt_command_ptr_;
65+
rclcpp::Subscription<CmdType>::SharedPtr twist_command_subscriber_;
66+
67+
std::string logger_name_;
68+
};
69+
70+
} // namespace cartesian_controllers
71+
72+
#endif // CARTESIAN_CONTROLLERS__TWIST_CONTROLLER_HPP_

‎cartesian_controllers/include/cartesian_controllers/visibility_control.h‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
// Copyright 2022 VoodooIT, sole proprietorship
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef CARTESIAN_CONTROLLERS__VISIBILITY_CONTROL_H_
16+
#define CARTESIAN_CONTROLLERS__VISIBILITY_CONTROL_H_
17+
18+
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
19+
// https://gcc.gnu.org/wiki/Visibility
20+
21+
#if defined _WIN32 || defined __CYGWIN__
22+
#ifdef __GNUC__
23+
#define CARTESIAN_CONTROLLERS_EXPORT __attribute__((dllexport))
24+
#define CARTESIAN_CONTROLLERS_IMPORT __attribute__((dllimport))
25+
#else
26+
#define CARTESIAN_CONTROLLERS_EXPORT __declspec(dllexport)
27+
#define CARTESIAN_CONTROLLERS_IMPORT __declspec(dllimport)
28+
#endif
29+
#ifdef CARTESIAN_CONTROLLERS_BUILDING_LIBRARY
30+
#define CARTESIAN_CONTROLLERS_PUBLIC CARTESIAN_CONTROLLERS_EXPORT
31+
#else
32+
#define CARTESIAN_CONTROLLERS_PUBLIC CARTESIAN_CONTROLLERS_IMPORT
33+
#endif
34+
#define CARTESIAN_CONTROLLERS_PUBLIC_TYPE CARTESIAN_CONTROLLERS_PUBLIC
35+
#define CARTESIAN_CONTROLLERS_LOCAL
36+
#else
37+
#define CARTESIAN_CONTROLLERS_EXPORT __attribute__((visibility("default")))
38+
#define CARTESIAN_CONTROLLERS_IMPORT
39+
#if __GNUC__ >= 4
40+
#define CARTESIAN_CONTROLLERS_PUBLIC __attribute__((visibility("default")))
41+
#define CARTESIAN_CONTROLLERS_LOCAL __attribute__((visibility("hidden")))
42+
#else
43+
#define CARTESIAN_CONTROLLERS_PUBLIC
44+
#define CARTESIAN_CONTROLLERS_LOCAL
45+
#endif
46+
#define CARTESIAN_CONTROLLERS_PUBLIC_TYPE
47+
#endif
48+
49+
#endif // CARTESIAN_CONTROLLERS__VISIBILITY_CONTROL_H_

‎cartesian_controllers/package.xml‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>cartesian_controllers</name>
5+
<version>0.0.0</version>
6+
<description>Cartesain controllers for ROS2.</description>
7+
<maintainer email="[email protected]">Lovro Ivanov</maintainer>
8+
<license>Apache-2.0</license>
9+
10+
<buildtool_depend>ament_cmake_ros</buildtool_depend>
11+
12+
<depend>controller_interface</depend>
13+
<depend>hardware_interface</depend>
14+
<depend>rclcpp</depend>
15+
<depend>rclcpp_lifecycle</depend>
16+
<depend>realtime_tools</depend>
17+
<depend>geometry_msgs</depend>
18+
<depend>std_msgs</depend>
19+
<depend>pluginlib</depend>
20+
21+
<test_depend>ament_lint_auto</test_depend>
22+
<test_depend>ament_lint_common</test_depend>
23+
24+
<export>
25+
<build_type>ament_cmake</build_type>
26+
</export>
27+
</package>

‎cartesian_controllers/src/twist_controller.cpp‎

Lines changed: 134 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,134 @@
1+
// Copyright 2022 VoodooIT, sole proprietorship
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "cartesian_controllers/twist_controller.hpp"
16+
17+
namespace cartesian_controllers
18+
{
19+
TwistController::TwistController()
20+
: controller_interface::ControllerInterface(),
21+
rt_command_ptr_(nullptr),
22+
twist_command_subscriber_(nullptr)
23+
{
24+
}
25+
26+
controller_interface::InterfaceConfiguration TwistController::command_interface_configuration()
27+
const
28+
{
29+
controller_interface::InterfaceConfiguration command_interfaces_config;
30+
command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
31+
32+
for (const auto & interface : interface_names_)
33+
{
34+
command_interfaces_config.names.push_back(joint_name_ + "/" + interface);
35+
}
36+
}
37+
38+
controller_interface::InterfaceConfiguration TwistController::state_interface_configuration() const
39+
{
40+
return controller_interface::InterfaceConfiguration{
41+
controller_interface::interface_configuration_type::NONE};
42+
}
43+
44+
CallbackReturn TwistController::on_init()
45+
{
46+
try
47+
{
48+
auto_declare<std::vector<std::string>>("interface_names", std::vector<std::string>());
49+
50+
auto_declare<std::string>("joint", "");
51+
}
52+
catch (const std::exception & e)
53+
{
54+
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
55+
return CallbackReturn::ERROR;
56+
}
57+
58+
return CallbackReturn::SUCCESS;
59+
}
60+
61+
controller_interface::return_type TwistController::update(
62+
const rclcpp::Time & time, const rclcpp::Duration & period)
63+
{
64+
auto twist_commands = rt_command_ptr_.readFromRT();
65+
66+
// no command received yet
67+
if (!twist_commands || !(*twist_commands))
68+
{
69+
return controller_interface::return_type::OK;
70+
}
71+
72+
if (command_interfaces_.size() != 6)
73+
{
74+
RCLCPP_ERROR_THROTTLE(
75+
get_node()->get_logger(), *node_->get_clock(), 1000,
76+
"Twist controller needs does not match number of interfaces needed 6, given (%zu) interfaces",
77+
command_interfaces_.size());
78+
return controller_interface::return_type::ERROR;
79+
}
80+
command_interfaces_[0].set_value((*twist_commands)->twist.linear.x);
81+
command_interfaces_[1].set_value((*twist_commands)->twist.linear.y);
82+
command_interfaces_[2].set_value((*twist_commands)->twist.linear.z);
83+
command_interfaces_[3].set_value((*twist_commands)->twist.angular.x);
84+
command_interfaces_[4].set_value((*twist_commands)->twist.angular.y);
85+
command_interfaces_[5].set_value((*twist_commands)->twist.angular.z);
86+
87+
return controller_interface::return_type::OK;
88+
}
89+
90+
CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State & previous_state)
91+
{
92+
joint_name_ = node_->get_parameter("joint").as_string();
93+
94+
if (joint_name_.empty())
95+
{
96+
RCLCPP_ERROR(get_node()->get_logger(), "'joint' parameter was empty");
97+
return CallbackReturn::ERROR;
98+
}
99+
100+
// Specialized, child controllers set interfaces before calling configure function.
101+
if (interface_names_.empty())
102+
{
103+
interface_names_ = node_->get_parameter("interface_names").as_string_array();
104+
}
105+
106+
if (interface_names_.empty())
107+
{
108+
RCLCPP_ERROR(get_node()->get_logger(), "'interface_names' parameter was empty");
109+
return CallbackReturn::ERROR;
110+
}
111+
112+
twist_command_subscriber_ = get_node()->create_subscription<CmdType>(
113+
"~/commands", rclcpp::SystemDefaultsQoS(),
114+
[this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); });
115+
116+
RCLCPP_INFO(get_node()->get_logger(), "configure successful");
117+
return CallbackReturn::SUCCESS;
118+
}
119+
120+
CallbackReturn TwistController::on_activate(const rclcpp_lifecycle::State & previous_state)
121+
{
122+
// reset command buffer if a command came through callback when controller was inactive
123+
rt_command_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>>(nullptr);
124+
return CallbackReturn::SUCCESS;
125+
}
126+
127+
CallbackReturn TwistController::on_deactivate(const rclcpp_lifecycle::State & previous_state)
128+
{
129+
// reset command buffer
130+
rt_command_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>>(nullptr);
131+
return CallbackReturn::SUCCESS;
132+
}
133+
134+
} // namespace cartesian_controllers

‎cartesian_controllers/twist_controller_plugin.xml‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
<library path="twist_controller">
2+
<class name="cartesian_controllers/TwistController" type="cartesian_controllers::TwistController" base_class_type="controller_interface::ControllerInterface">
3+
<description>
4+
6dof pure forwarding twist controller.
5+
</description>
6+
</class>
7+
</library>

0 commit comments

Comments
 (0)