Skip to content

Commit 1e14150

Browse files
BorgesJVTdestogl
BorgesJVT
and
destogl
authored
demo_diff_drive_controller (#70)
* demo_diff_drive_controller * fix ament_flake errors * applying review suggestions * velocity and position models to simulate read hardware * change folder to find packages * Apply suggestions from code review * adjust everywhere return_type * wheels first-order dynamics * remove slowdown parameter and uncrustify * applying review suggestions * Apply formatting Co-authored-by: Denis Štogl <[email protected]> Co-authored-by: Denis Štogl <[email protected]>
1 parent f5d821c commit 1e14150

File tree

11 files changed

+790
-0
lines changed

11 files changed

+790
-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: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
// Copyright 2021 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+
#ifndef ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_
16+
#define ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_
17+
18+
#include <memory>
19+
#include <string>
20+
#include <vector>
21+
22+
#include "hardware_interface/base_interface.hpp"
23+
#include "hardware_interface/handle.hpp"
24+
#include "hardware_interface/hardware_info.hpp"
25+
#include "hardware_interface/system_interface.hpp"
26+
#include "hardware_interface/types/hardware_interface_return_values.hpp"
27+
#include "hardware_interface/types/hardware_interface_status_values.hpp"
28+
#include "rclcpp/macros.hpp"
29+
#include "ros2_control_demo_hardware/visibility_control.h"
30+
31+
namespace ros2_control_demo_hardware
32+
{
33+
class DiffBotSystemHardware
34+
: public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
35+
{
36+
public:
37+
RCLCPP_SHARED_PTR_DEFINITIONS(DiffBotSystemHardware);
38+
39+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
40+
hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override;
41+
42+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
43+
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
44+
45+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
46+
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
47+
48+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
49+
hardware_interface::return_type start() override;
50+
51+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
52+
hardware_interface::return_type stop() override;
53+
54+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
55+
hardware_interface::return_type read() override;
56+
57+
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
58+
hardware_interface::return_type write() override;
59+
60+
private:
61+
// Parameters for the DiffBot simulation
62+
double hw_start_sec_;
63+
double hw_stop_sec_;
64+
65+
// Store the command for the simulated robot
66+
std::vector<double> hw_commands_;
67+
std::vector<double> hw_states_;
68+
};
69+
70+
} // namespace ros2_control_demo_hardware
71+
72+
#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 DiffBot example using a system hardware interface-type. It uses velocity command and position state interface. The example is the starting point to implement a hardware interface for differential-drive mobile robots.
21+
</description>
22+
</class>
1623
</library>

‎ros2_control_demo_hardware/src/diffbot_system.cpp‎

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

‎ros2_control_demo_robot/controllers/diffbot_diff_drive_controller.yaml‎

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

0 commit comments

Comments
 (0)