Skip to content

Commit 3dfdef2

Browse files
bmagyarv-lopez
bmagyar
and
v-lopez
authored
diff_drive_controller (ros-controls#67)
* diff_drive_controller * Update PAL Robotics's headers license to Apache 2 (ros-controls#69) * Apply suggestions from code review Co-authored-by: Victor Lopez <[email protected]> * Remove boost and add custom rolling mean accumulator (ros-controls#70) * Remove boost and add custom rolling mean accumulator * Use index instead of iterators and remove sources of branching * Update package-level license tag * Remove shorthands * Make linters happy * Update gh actions to run on diff_drive_controller * Make linters even happier * [diff-drive] Fix existing tests (ros-controls#73) * Fix msg type on cleanup test * Make DiffDriveController members protected To make them accessible by tests * Fix failing tests and remove some sleeps refs ros-controls#27 * Don't update after cleanup, node is unconfigured * Remove unused executor that caused test to hang spuriosuly Also use SingleThreaded executors by default * Change how twist messages are waited for * Use enum class return types * ASSERT_EQ instead of if-FAIL * std::swap to reset queue Co-authored-by: Victor Lopez <[email protected]>
1 parent a049b7a commit 3dfdef2

17 files changed

+1998
-1
lines changed

‎.github/workflows/lint.yml

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ jobs:
1616
package-name: |
1717
joint_state_controller
1818
joint_trajectory_controller
19+
diff_drive_controller
1920
ament_xmllint:
2021
name: ament_xmllint
2122
runs-on: ubuntu-18.04
@@ -26,9 +27,10 @@ jobs:
2627
with:
2728
linter: xmllint
2829
package-name: |
30+
ros2_controllers
2931
joint_state_controller
3032
joint_trajectory_controller
31-
ros2_controllers
33+
diff_drive_controller
3234
ament_lint_cpp: # Linters applicable to C++ packages
3335
name: ament_${{ matrix.linter }}
3436
runs-on: ubuntu-18.04
@@ -45,3 +47,4 @@ jobs:
4547
package-name: |
4648
joint_state_controller
4749
joint_trajectory_controller
50+
diff_drive_controller

‎.github/workflows/test.yml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@ jobs:
2020
package-name: |
2121
joint_state_controller
2222
joint_trajectory_controller
23+
diff_drive_controller
2324
vcs-repo-file-url: |
2425
https://raw.githubusercontent.com/${{ github.repository }}/${{ github.sha }}/.github/workspace.repos
2526
https://raw.githubusercontent.com/ros2/ros2/master/ros2.repos

‎diff_drive_controller/CMakeLists.txt

Lines changed: 129 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,129 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
project(diff_drive_controller)
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)
11+
endif()
12+
13+
find_package(ament_cmake REQUIRED)
14+
find_package(controller_interface REQUIRED)
15+
find_package(geometry_msgs REQUIRED)
16+
find_package(hardware_interface REQUIRED)
17+
find_package(nav_msgs REQUIRED)
18+
find_package(pluginlib REQUIRED)
19+
find_package(rclcpp REQUIRED)
20+
find_package(rclcpp_lifecycle REQUIRED)
21+
find_package(realtime_tools REQUIRED)
22+
find_package(sensor_msgs REQUIRED)
23+
find_package(tf2 REQUIRED)
24+
find_package(tf2_msgs REQUIRED)
25+
26+
add_library(diff_drive_controller SHARED
27+
src/diff_drive_controller.cpp
28+
src/odometry.cpp
29+
src/speed_limiter.cpp
30+
)
31+
32+
target_include_directories(diff_drive_controller PRIVATE include)
33+
ament_target_dependencies(diff_drive_controller
34+
builtin_interfaces
35+
controller_interface
36+
geometry_msgs
37+
hardware_interface
38+
nav_msgs
39+
pluginlib
40+
rclcpp
41+
rclcpp_lifecycle
42+
realtime_tools
43+
sensor_msgs
44+
tf2
45+
tf2_msgs
46+
)
47+
# Causes the visibility macros to use dllexport rather than dllimport,
48+
# which is appropriate when building the dll but not consuming it.
49+
target_compile_definitions(diff_drive_controller PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL")
50+
51+
pluginlib_export_plugin_description_file(controller_interface diff_drive_plugin.xml)
52+
53+
install(DIRECTORY include/
54+
DESTINATION include
55+
)
56+
57+
install(TARGETS diff_drive_controller
58+
RUNTIME DESTINATION bin
59+
ARCHIVE DESTINATION lib
60+
LIBRARY DESTINATION lib
61+
)
62+
63+
if(BUILD_TESTING)
64+
find_package(ament_cmake_gtest REQUIRED)
65+
find_package(ament_lint_auto REQUIRED)
66+
find_package(controller_manager REQUIRED)
67+
find_package(test_robot_hardware REQUIRED)
68+
69+
ament_lint_auto_find_test_dependencies()
70+
71+
ament_add_gtest(test_diff_drive_controller
72+
test/test_diff_drive_controller.cpp
73+
ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml)
74+
target_include_directories(test_diff_drive_controller PRIVATE include)
75+
target_link_libraries(test_diff_drive_controller
76+
diff_drive_controller
77+
)
78+
79+
ament_target_dependencies(test_diff_drive_controller
80+
geometry_msgs
81+
hardware_interface
82+
nav_msgs
83+
rclcpp
84+
rclcpp_lifecycle
85+
realtime_tools
86+
sensor_msgs
87+
test_robot_hardware
88+
tf2
89+
tf2_msgs
90+
)
91+
92+
ament_add_gtest(
93+
test_load_diff_drive_controller
94+
test/test_load_diff_drive_controller.cpp
95+
)
96+
97+
target_include_directories(test_load_diff_drive_controller PRIVATE include)
98+
ament_target_dependencies(test_load_diff_drive_controller
99+
controller_manager
100+
test_robot_hardware
101+
)
102+
103+
ament_add_gtest(
104+
test_accumulator
105+
test/test_accumulator.cpp
106+
)
107+
108+
target_include_directories(test_accumulator PRIVATE include)
109+
ament_target_dependencies(test_accumulator)
110+
111+
endif()
112+
113+
ament_export_dependencies(
114+
controller_interface
115+
geometry_msgs
116+
hardware_interface
117+
rclcpp
118+
rclcpp_lifecycle
119+
sensor_msgs
120+
tf2
121+
tf2_msgs
122+
)
123+
ament_export_include_directories(
124+
include
125+
)
126+
ament_export_libraries(
127+
diff_drive_controller
128+
)
129+
ament_package()

‎diff_drive_controller/diff_drive_plugin.xml

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
<library path="diff_drive_controller">
2+
<class name="diff_drive_controller/DiffDriveController" type="diff_drive_controller::DiffDriveController" base_class_type="controller_interface::ControllerInterface">
3+
<description>
4+
The differential drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a differential drive robot.
5+
</description>
6+
</class>
7+
</library>

‎diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

Lines changed: 180 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,180 @@
1+
// Copyright 2020 PAL Robotics S.L.
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+
/*
16+
* Author: Bence Magyar, Enrique Fernández, Manuel Meraz
17+
*/
18+
19+
#ifndef DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
20+
#define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
21+
22+
#include <chrono>
23+
#include <cmath>
24+
#include <memory>
25+
#include <queue>
26+
#include <string>
27+
#include <vector>
28+
29+
#include "controller_interface/controller_interface.hpp"
30+
#include "diff_drive_controller/odometry.hpp"
31+
#include "diff_drive_controller/speed_limiter.hpp"
32+
#include "diff_drive_controller/visibility_control.h"
33+
#include "geometry_msgs/msg/twist.hpp"
34+
#include "geometry_msgs/msg/twist_stamped.hpp"
35+
#include "hardware_interface/joint_command_handle.hpp"
36+
#include "hardware_interface/operation_mode_handle.hpp"
37+
#include "hardware_interface/robot_hardware.hpp"
38+
#include "nav_msgs/msg/odometry.hpp"
39+
#include "odometry.hpp"
40+
#include "rclcpp/rclcpp.hpp"
41+
#include "rclcpp_lifecycle/state.hpp"
42+
#include "realtime_tools/realtime_buffer.h"
43+
#include "realtime_tools/realtime_publisher.h"
44+
#include "sensor_msgs/msg/joint_state.hpp"
45+
#include "tf2_msgs/msg/tf_message.hpp"
46+
47+
namespace diff_drive_controller
48+
{
49+
class DiffDriveController : public controller_interface::ControllerInterface
50+
{
51+
using Twist = geometry_msgs::msg::TwistStamped;
52+
53+
public:
54+
DIFF_DRIVE_CONTROLLER_PUBLIC
55+
DiffDriveController();
56+
57+
DIFF_DRIVE_CONTROLLER_PUBLIC
58+
DiffDriveController(
59+
std::vector<std::string> left_wheel_names,
60+
std::vector<std::string> right_wheel_names,
61+
std::vector<std::string> operation_mode_names);
62+
63+
DIFF_DRIVE_CONTROLLER_PUBLIC
64+
controller_interface::return_type
65+
init(
66+
std::weak_ptr<hardware_interface::RobotHardware> robot_hardware,
67+
const std::string & controller_name) override;
68+
69+
DIFF_DRIVE_CONTROLLER_PUBLIC
70+
controller_interface::return_type update() override;
71+
72+
DIFF_DRIVE_CONTROLLER_PUBLIC
73+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
74+
on_configure(const rclcpp_lifecycle::State & previous_state) override;
75+
76+
DIFF_DRIVE_CONTROLLER_PUBLIC
77+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
78+
on_activate(const rclcpp_lifecycle::State & previous_state) override;
79+
80+
DIFF_DRIVE_CONTROLLER_PUBLIC
81+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
82+
on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
83+
84+
DIFF_DRIVE_CONTROLLER_PUBLIC
85+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
86+
on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
87+
88+
DIFF_DRIVE_CONTROLLER_PUBLIC
89+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
90+
on_error(const rclcpp_lifecycle::State & previous_state) override;
91+
92+
DIFF_DRIVE_CONTROLLER_PUBLIC
93+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
94+
on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
95+
96+
protected:
97+
struct WheelHandle
98+
{
99+
const hardware_interface::JointStateHandle * state = nullptr;
100+
hardware_interface::JointCommandHandle * command = nullptr;
101+
};
102+
103+
CallbackReturn configure_side(
104+
const std::string & side,
105+
const std::vector<std::string> & wheel_names,
106+
std::vector<WheelHandle> & registered_handles,
107+
hardware_interface::RobotHardware & robot_hardware);
108+
109+
std::vector<std::string> left_wheel_names_;
110+
std::vector<std::string> right_wheel_names_;
111+
112+
std::vector<WheelHandle> registered_left_wheel_handles_;
113+
std::vector<WheelHandle> registered_right_wheel_handles_;
114+
115+
struct WheelParams
116+
{
117+
size_t wheels_per_side = 0;
118+
double separation = 0.0; // w.r.t. the midpoint of the wheel width
119+
double radius = 0.0; // Assumed to be the same for both wheels
120+
double separation_multiplier = 1.0;
121+
double left_radius_multiplier = 1.0;
122+
double right_radius_multiplier = 1.0;
123+
} wheel_params_;
124+
125+
struct OdometryParams
126+
{
127+
bool open_loop = false;
128+
bool enable_odom_tf = true;
129+
std::string base_frame_id = "base_link";
130+
std::string odom_frame_id = "odom";
131+
std::array<double, 6> pose_covariance_diagonal;
132+
std::array<double, 6> twist_covariance_diagonal;
133+
} odom_params_;
134+
135+
Odometry odometry_;
136+
137+
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Odometry>> odometry_publisher_
138+
=
139+
nullptr;
140+
std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
141+
realtime_odometry_publisher_ = nullptr;
142+
143+
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<tf2_msgs::msg::TFMessage>>
144+
odometry_transform_publisher_ = nullptr;
145+
std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
146+
realtime_odometry_transform_publisher_ = nullptr;
147+
148+
// Timeout to consider cmd_vel commands old
149+
std::chrono::milliseconds cmd_vel_timeout_{500};
150+
151+
std::vector<std::string> write_op_names_;
152+
std::vector<hardware_interface::OperationModeHandle *> registered_operation_mode_handles_;
153+
154+
bool subscriber_is_active_ = false;
155+
rclcpp::Subscription<Twist>::SharedPtr velocity_command_subscriber_ = nullptr;
156+
157+
std::shared_ptr<Twist> received_velocity_msg_ptr_ = nullptr;
158+
159+
std::queue<Twist> previous_commands_; // last two commands
160+
161+
// speed limiters
162+
SpeedLimiter limiter_linear_;
163+
SpeedLimiter limiter_angular_;
164+
165+
bool publish_limited_velocity_ = false;
166+
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<Twist>> limited_velocity_publisher_ =
167+
nullptr;
168+
std::shared_ptr<realtime_tools::RealtimePublisher<Twist>>
169+
realtime_limited_velocity_publisher_ = nullptr;
170+
171+
rclcpp::Time previous_update_timestamp_{0};
172+
173+
bool is_halted = false;
174+
175+
bool reset();
176+
void set_op_mode(const hardware_interface::OperationMode & mode);
177+
void halt();
178+
};
179+
} // namespace diff_drive_controller
180+
#endif // DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_

0 commit comments

Comments
 (0)