Skip to content

Commit 288325a

Browse files
BorgesJVTdestogl
BorgesJVT
authored and
destogl
committed
demo_diff_drive_controller
1 parent f5d821c commit 288325a

File tree

11 files changed

+823
-0
lines changed

11 files changed

+823
-0
lines changed

‎ros2_control_demo_hardware/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@ find_package(rclcpp REQUIRED)
2020
add_library(
2121
${PROJECT_NAME}
2222
SHARED
23+
src/diffbot_system.cpp
2324
src/rrbot_system_position_only.cpp
2425
src/rrbot_system_multi_interface.cpp
2526
)

‎ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
// Copyright 2020 ros2_control Development Team
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+
#ifndef ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_
17+
#define ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_
18+
19+
#include <memory>
20+
#include <string>
21+
#include <vector>
22+
23+
#include "rclcpp/macros.hpp"
24+
25+
#include "hardware_interface/base_interface.hpp"
26+
#include "hardware_interface/system_interface.hpp"
27+
#include "hardware_interface/handle.hpp"
28+
#include "hardware_interface/hardware_info.hpp"
29+
#include "hardware_interface/types/hardware_interface_return_values.hpp"
30+
#include "hardware_interface/types/hardware_interface_status_values.hpp"
31+
#include "ros2_control_demo_hardware/visibility_control.h"
32+
33+
using hardware_interface::return_type;
34+
35+
namespace ros2_control_demo_hardware
36+
{
37+
class DiffBotSystemHardware : public
38+
hardware_interface::BaseInterface<hardware_interface::SystemInterface>
39+
{
40+
public:
41+
RCLCPP_SHARED_PTR_DEFINITIONS(DiffBotSystemHardware);
42+
43+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
44+
return_type configure(const hardware_interface::HardwareInfo & info) override;
45+
46+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
47+
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
48+
49+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
50+
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
51+
52+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
53+
return_type start() override;
54+
55+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
56+
return_type stop() override;
57+
58+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
59+
return_type read() override;
60+
61+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
62+
return_type write() override;
63+
64+
private:
65+
// Parameters for the DiffBot simulation
66+
double hw_start_sec_;
67+
double hw_stop_sec_;
68+
double hw_slowdown_;
69+
70+
// Store the command for the simulated robot
71+
std::vector<double> hw_commands_;
72+
std::vector<double> hw_states_;
73+
};
74+
75+
} // namespace ros2_control_demo_hardware
76+
77+
#endif // ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_

‎ros2_control_demo_hardware/ros2_control_demo_hardware.xml‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,4 +13,11 @@
1313
The ROS2 Control minimal robot protocol for a system with multiple command and state interfaces.
1414
</description>
1515
</class>
16+
<class name="ros2_control_demo_hardware/DiffBotSystemHardware"
17+
type="ros2_control_demo_hardware::DiffBotSystemHardware"
18+
base_class_type="hardware_interface::SystemInterface">
19+
<description>
20+
The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1.
21+
</description>
22+
</class>
1623
</library>

‎ros2_control_demo_hardware/src/diffbot_system.cpp‎

Lines changed: 220 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,220 @@
1+
// Copyright 2020 ros2_control Development Team
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 "ros2_control_demo_hardware/diffbot_system.hpp"
16+
17+
#include <chrono>
18+
#include <cmath>
19+
#include <limits>
20+
#include <memory>
21+
#include <vector>
22+
23+
#include "hardware_interface/types/hardware_interface_type_values.hpp"
24+
#include "rclcpp/rclcpp.hpp"
25+
26+
namespace ros2_control_demo_hardware
27+
{
28+
29+
return_type DiffBotSystemHardware::configure(
30+
const hardware_interface::HardwareInfo & info)
31+
{
32+
if (configure_default(info) != return_type::OK) {
33+
return return_type::ERROR;
34+
}
35+
36+
hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
37+
hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
38+
hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]);
39+
hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
40+
hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
41+
42+
for (const hardware_interface::ComponentInfo & joint : info_.joints) {
43+
// DiffBotSystem has exactly two states and one command interface on each joint
44+
if (joint.command_interfaces.size() != 1) {
45+
RCLCPP_FATAL(
46+
rclcpp::get_logger("DiffBotSystemHardware"),
47+
"Joint '%s' has %d command interfaces found. 1 expected.",
48+
joint.name.c_str(), joint.command_interfaces.size());
49+
return return_type::ERROR;
50+
}
51+
52+
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) {
53+
RCLCPP_FATAL(
54+
rclcpp::get_logger("DiffBotSystemHardware"),
55+
"Joint '%s' have %s command interfaces found. '%s' expected.",
56+
joint.name.c_str(), joint.command_interfaces[0].name.c_str(),
57+
hardware_interface::HW_IF_VELOCITY);
58+
return return_type::ERROR;
59+
}
60+
61+
if (joint.state_interfaces.size() != 2) {
62+
RCLCPP_FATAL(
63+
rclcpp::get_logger("DiffBotSystemHardware"),
64+
"Joint '%s' has %d state interface. 2 expected.",
65+
joint.name.c_str(), joint.state_interfaces.size());
66+
return return_type::ERROR;
67+
}
68+
69+
if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
70+
RCLCPP_FATAL(
71+
rclcpp::get_logger("DiffBotSystemHardware"),
72+
"Joint '%s' have '%s' as first state interface. '%s' and '%s' expected.",
73+
joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
74+
hardware_interface::HW_IF_POSITION);
75+
return return_type::ERROR;
76+
}
77+
78+
if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
79+
RCLCPP_FATAL(
80+
rclcpp::get_logger("DiffBotSystemHardware"),
81+
"Joint '%s' have '%s' as second state interface. '%s' expected.",
82+
joint.name.c_str(), joint.state_interfaces[1].name.c_str(),
83+
hardware_interface::HW_IF_VELOCITY);
84+
return return_type::ERROR;
85+
}
86+
}
87+
88+
status_ = hardware_interface::status::CONFIGURED;
89+
return return_type::OK;
90+
}
91+
92+
std::vector<hardware_interface::StateInterface>
93+
DiffBotSystemHardware::export_state_interfaces()
94+
{
95+
std::vector<hardware_interface::StateInterface> state_interfaces;
96+
for (uint i = 0; i < info_.joints.size(); i++) {
97+
state_interfaces.emplace_back(
98+
hardware_interface::StateInterface(
99+
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
100+
state_interfaces.emplace_back(
101+
hardware_interface::StateInterface(
102+
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_[i]));
103+
}
104+
105+
return state_interfaces;
106+
}
107+
108+
std::vector<hardware_interface::CommandInterface>
109+
DiffBotSystemHardware::export_command_interfaces()
110+
{
111+
std::vector<hardware_interface::CommandInterface> command_interfaces;
112+
for (uint i = 0; i < info_.joints.size(); i++) {
113+
command_interfaces.emplace_back(
114+
hardware_interface::CommandInterface(
115+
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i]));
116+
}
117+
118+
return command_interfaces;
119+
}
120+
121+
122+
return_type DiffBotSystemHardware::start()
123+
{
124+
RCLCPP_INFO(
125+
rclcpp::get_logger("DiffBotSystemHardware"),
126+
"Starting ...please wait...");
127+
128+
for (int i = 0; i <= hw_start_sec_; i++) {
129+
rclcpp::sleep_for(std::chrono::seconds(1));
130+
RCLCPP_INFO(
131+
rclcpp::get_logger("DiffBotSystemHardware"),
132+
"%.1f seconds left...", hw_start_sec_ - i);
133+
}
134+
135+
// set some default values
136+
for (uint i = 0; i < hw_states_.size(); i++) {
137+
if (std::isnan(hw_states_[i])) {
138+
hw_states_[i] = 0;
139+
hw_commands_[i] = 0;
140+
}
141+
}
142+
143+
status_ = hardware_interface::status::STARTED;
144+
145+
RCLCPP_INFO(
146+
rclcpp::get_logger("DiffBotSystemHardware"),
147+
"System Sucessfully started!");
148+
149+
return return_type::OK;
150+
}
151+
152+
return_type DiffBotSystemHardware::stop()
153+
{
154+
RCLCPP_INFO(
155+
rclcpp::get_logger("DiffBotSystemHardware"),
156+
"Stopping ...please wait...");
157+
158+
for (int i = 0; i <= hw_stop_sec_; i++) {
159+
rclcpp::sleep_for(std::chrono::seconds(1));
160+
RCLCPP_INFO(
161+
rclcpp::get_logger("DiffBotSystemHardware"),
162+
"%.1f seconds left...", hw_stop_sec_ - i);
163+
}
164+
165+
status_ = hardware_interface::status::STOPPED;
166+
167+
RCLCPP_INFO(
168+
rclcpp::get_logger("DiffBotSystemHardware"),
169+
"System sucessfully stopped!");
170+
171+
return return_type::OK;
172+
}
173+
174+
hardware_interface::return_type DiffBotSystemHardware::read()
175+
{
176+
RCLCPP_INFO(
177+
rclcpp::get_logger("DiffBotSystemHardware"),
178+
"Reading...");
179+
180+
for (uint i = 0; i < hw_states_.size(); i++) {
181+
// Simulate DiffBot's movement
182+
hw_states_[i] = hw_commands_[i] + (hw_states_[i] - hw_commands_[i]) / hw_slowdown_;
183+
RCLCPP_INFO(
184+
rclcpp::get_logger("DiffBotSystemHardware"),
185+
"Got state %.5f for '%s'!", hw_states_[i], info_.joints[i].name.c_str());
186+
}
187+
RCLCPP_INFO(
188+
rclcpp::get_logger("DiffBotSystemHardware"),
189+
"Joints sucessfully read!");
190+
191+
return return_type::OK;
192+
}
193+
194+
hardware_interface::return_type ros2_control_demo_hardware::DiffBotSystemHardware::write()
195+
{
196+
RCLCPP_INFO(
197+
rclcpp::get_logger("DiffBotSystemHardware"),
198+
"Writing...");
199+
200+
for (uint i = 0; i < hw_commands_.size(); i++) {
201+
// Simulate sending commands to the hardware
202+
RCLCPP_INFO(
203+
rclcpp::get_logger("DiffBotSystemHardware"),
204+
"Got command %.5f for '%s'!", hw_commands_[i], info_.joints[i].name.c_str());
205+
}
206+
RCLCPP_INFO(
207+
rclcpp::get_logger("DiffBotSystemHardware"),
208+
"Joints sucessfully written!");
209+
210+
return return_type::OK;
211+
}
212+
213+
} // namespace ros2_control_demo_hardware
214+
215+
#include "pluginlib/class_list_macros.hpp"
216+
217+
PLUGINLIB_EXPORT_CLASS(
218+
ros2_control_demo_hardware::DiffBotSystemHardware,
219+
hardware_interface::SystemInterface
220+
)

‎ros2_control_demo_robot/controllers/diffbot_diff_drive_controller.yaml‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
controller_manager:
2+
ros__parameters:
3+
update_rate: 2 # Hz
4+
5+
diffbot_base_controller:
6+
type: diff_drive_controller/DiffDriveController
7+
8+
diffbot_base_controller:
9+
ros__parameters:
10+
left_wheel_names: ["left_wheel_joint"]
11+
right_wheel_names: ["right_wheel_joint"]
12+
#write_op_modes: ["motor_controller"]
13+
14+
wheel_separation: 0.10
15+
#wheels_per_side: 1 # actually 2, but both are controlled by 1 signal
16+
wheel_radius: 0.015
17+
18+
wheel_separation_multiplier: 1.0
19+
left_wheel_radius_multiplier: 1.0
20+
right_wheel_radius_multiplier: 1.0
21+
22+
publish_rate: 50.0
23+
odom_frame_id: odom
24+
base_frame_id: base_link
25+
pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
26+
twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
27+
28+
open_loop: true
29+
enable_odom_tf: true
30+
31+
cmd_vel_timeout: 0.5
32+
#publish_limited_velocity: true
33+
use_stamped_vel: false
34+
#velocity_rolling_window_size: 10
35+
36+
# Preserve turning radius when limiting speed/acceleration/jerk
37+
preserve_turning_radius: true
38+
39+
# Publish limited velocity
40+
publish_cmd: true
41+
42+
# Publish wheel data
43+
publish_wheel_data: true
44+
45+
# Velocity and acceleration limits
46+
# Whenever a min_* is unspecified, default to -max_*
47+
linear.x.has_velocity_limits: true
48+
linear.x.has_acceleration_limits: true
49+
linear.x.has_jerk_limits: false
50+
linear.x.max_velocity: 1.0
51+
linear.x.min_velocity: -1.0
52+
linear.x.max_acceleration: 1.0
53+
linear.x.max_jerk: 0.0
54+
linear.x.min_jerk: 0.0
55+
56+
angular.z.has_velocity_limits: true
57+
angular.z.has_acceleration_limits: true
58+
angular.z.has_jerk_limits: false
59+
angular.z.max_velocity: 1.0
60+
angular.z.min_velocity: -1.0
61+
angular.z.max_acceleration: 1.0
62+
angular.z.min_acceleration: -1.0
63+
angular.z.max_jerk: 0.0
64+
angular.z.min_jerk: 0.0

0 commit comments

Comments
 (0)