Skip to content

Commit 6f23ca0

Browse files
ajtudelaajtudela
committed
Rename rotate_to_dock
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
1 parent 72913a3 commit 6f23ca0

File tree

10 files changed

+49
-48
lines changed

10 files changed

+49
-48
lines changed

‎nav2_docking/README.md

+4-4
Original file line numberDiff line numberDiff line change
@@ -209,7 +209,7 @@ For debugging purposes, there are several publishers which can be used with RVIZ
209209
| base_frame | Robot's base frame for control law | string | "base_link" |
210210
| fixed_frame | Fixed frame to use, recommended to be a smooth odometry frame **not** map | string | "odom" |
211211
| odom_topic | The topic to use for the odometry data | string | "odom" |
212-
| rotation_angular_tolerance | Angular tolerance (rad) to exit the rotation loop when backward_blind is enable | double | 0.05 |
212+
| rotation_angular_tolerance | Angular tolerance (rad) to exit the rotation loop when rotate_to_dock is enabled | double | 0.05 |
213213
| dock_prestaging_tolerance | L2 distance in X,Y,Theta from the staging pose to bypass navigation | double | 0.5 |
214214
| dock_plugins | A set of dock plugins to load | vector<string> | N/A |
215215
| dock_database | The filepath to the dock database to use for this environment | string | N/A |
@@ -223,8 +223,8 @@ For debugging purposes, there are several publishers which can be used with RVIZ
223223
| controller.v_linear_max | Maximum linear velocity (m/s) | double | 0.25 |
224224
| controller.v_angular_max | Maximum angular velocity (rad/s) produced by the control law | double | 0.75 |
225225
| controller.slowdown_radius | Radius (m) around the goal pose in which the robot will start to slow down | double | 0.25 |
226-
| controller.rotate_to_heading_angular_vel | Angular velocity (rad/s) to rotate to the goal heading when backward_blind is enable | double | 1.0 |
227-
| controller.rotate_to_heading_max_angular_accel | Maximum angular acceleration (rad/s^2) to rotate to the goal heading when backward_blind is enable | double | 3.2 |
226+
| controller.rotate_to_heading_angular_vel | Angular velocity (rad/s) to rotate to the goal heading when rotate_to_dock is enabled | double | 1.0 |
227+
| controller.rotate_to_heading_max_angular_accel | Maximum angular acceleration (rad/s^2) to rotate to the goal heading when rotate_to_dock is enabled | double | 3.2 |
228228
| controller.use_collision_detection | Whether to use collision detection to avoid obstacles | bool | true |
229229
| controller.costmap_topic | The topic to use for the costmap | string | "local_costmap/costmap_raw" |
230230
| controller.footprint_topic | The topic to use for the robot's footprint | string | "local_costmap/published_footprint" |
@@ -256,7 +256,7 @@ Note: `dock_plugins` and either `docks` or `dock_database` are required.
256256
| staging_x_offset | Staging pose offset forward (negative) of dock pose (m) | double | -0.7 |
257257
| staging_yaw_offset | Staging pose angle relative to dock pose (rad) | double | 0.0 |
258258
| dock_direction | Whether the robot is docking with the dock forward or backward in motion | string | "forward" or "backward" |
259-
| backward_blind | Initial forward detection, then dock in reverse | bool | false |
259+
| rotate_to_dock | Rotate the robot 180º before docking | bool | false |
260260

261261
Note: The external detection rotation angles are setup to work out of the box with Apriltags detectors in `image_proc` and `isaac_ros`.
262262

‎nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -257,7 +257,7 @@ class DockingServer : public nav2_util::LifecycleNode
257257
std::optional<bool> dock_backwards_;
258258
// The tolerance to the dock's staging pose not requiring navigation
259259
double dock_prestaging_tolerance_;
260-
// Angular tolerance to exit the rotation loop when backward_blind is enable
260+
// Angular tolerance to exit the rotation loop when rotate_to_dock is enabled
261261
double rotation_angular_tolerance_;
262262

263263
// This is a class member so it can be accessed in publish feedback

‎nav2_docking/opennav_docking/src/docking_server.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -306,8 +306,8 @@ void DockingServer::dockRobot()
306306
rclcpp::Time dock_contact_time;
307307
while (rclcpp::ok()) {
308308
try {
309-
// Perform pure rotation to dock orientation
310-
if (dock->plugin->isBackwardBlind()) {
309+
// Perform a 180º to face away from the dock if needed
310+
if (dock->plugin->shouldRotateToDock()) {
311311
rotateToDock(dock_pose);
312312
}
313313
// Approach the dock using control law
@@ -501,7 +501,7 @@ bool DockingServer::approachDock(
501501
}
502502

503503
// Update perception
504-
if (!dock->plugin->isBackwardBlind() && !dock->plugin->getRefinedPose(dock_pose, dock->id)) {
504+
if (!dock->plugin->shouldRotateToDock() && !dock->plugin->getRefinedPose(dock_pose, dock->id)) {
505505
throw opennav_docking_core::FailedToDetectDock("Failed dock detection");
506506
}
507507

‎nav2_docking/opennav_docking/src/simple_charging_dock.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -83,7 +83,7 @@ void SimpleChargingDock::configure(
8383
nav2_util::declare_parameter_if_not_declared(
8484
node_, name + ".dock_direction", rclcpp::ParameterValue(std::string("forward")));
8585
nav2_util::declare_parameter_if_not_declared(
86-
node_, name + ".backward_blind", rclcpp::ParameterValue(false));
86+
node_, name + ".rotate_to_dock", rclcpp::ParameterValue(false));
8787

8888
node_->get_parameter(name + ".use_battery_status", use_battery_status_);
8989
node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_);
@@ -112,9 +112,9 @@ void SimpleChargingDock::configure(
112112
throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"};
113113
}
114114

115-
node_->get_parameter(name + ".backward_blind", backward_blind_);
116-
if (backward_blind_ && dock_direction_ != opennav_docking_core::DockDirection::BACKWARD) {
117-
throw std::runtime_error{"Parameter backward_blind is enabled but dock direction is not "
115+
node_->get_parameter(name + ".rotate_to_dock", rotate_to_dock_);
116+
if (rotate_to_dock_ && dock_direction_ != opennav_docking_core::DockDirection::BACKWARD) {
117+
throw std::runtime_error{"Parameter rotate_to_dock is enabled but dock direction is not "
118118
"backward. Please set dock direction to backward."};
119119
}
120120

‎nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,7 @@ void SimpleNonChargingDock::configure(
7474
nav2_util::declare_parameter_if_not_declared(
7575
node_, name + ".dock_direction", rclcpp::ParameterValue(std::string("forward")));
7676
nav2_util::declare_parameter_if_not_declared(
77-
node_, name + ".backward_blind", rclcpp::ParameterValue(false));
77+
node_, name + ".rotate_to_dock", rclcpp::ParameterValue(false));
7878

7979
node_->get_parameter(name + ".use_external_detection_pose", use_external_detection_pose_);
8080
node_->get_parameter(name + ".external_detection_timeout", external_detection_timeout_);
@@ -101,9 +101,9 @@ void SimpleNonChargingDock::configure(
101101
throw std::runtime_error{"Dock direction is not valid. Valid options are: forward or backward"};
102102
}
103103

104-
node_->get_parameter(name + ".backward_blind", backward_blind_);
105-
if (backward_blind_ && dock_direction_ != opennav_docking_core::DockDirection::BACKWARD) {
106-
throw std::runtime_error{"Parameter backward_blind is enabled but dock direction is not "
104+
node_->get_parameter(name + ".rotate_to_dock", rotate_to_dock_);
105+
if (rotate_to_dock_ && dock_direction_ != opennav_docking_core::DockDirection::BACKWARD) {
106+
throw std::runtime_error{"Parameter rotate_to_dock is enabled but dock direction is not "
107107
"backward. Please set dock direction to backward."};
108108
}
109109

‎nav2_docking/opennav_docking/test/docking_params.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ docking_server:
99
plugin: opennav_docking::SimpleChargingDock
1010
use_battery_status: True
1111
dock_direction: forward
12-
backward_blind: False
12+
rotate_to_dock: False
1313
staging_yaw_offset: 0.0
1414
docks: [test_dock]
1515
test_dock:

‎nav2_docking/opennav_docking/test/test_docking_server.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@ def generate_test_description() -> LaunchDescription:
6969

7070
if os.getenv('BACKWARD_BLIND') == 'True':
7171
param_substitutions.update({'dock_direction': 'backward'})
72-
param_substitutions.update({'backward_blind': 'True'})
72+
param_substitutions.update({'rotate_to_dock': 'True'})
7373

7474
configured_params = RewrittenYaml(
7575
source_file=params_file,

‎nav2_docking/opennav_docking/test/test_simple_charging_dock.cpp

+13-13
Original file line numberDiff line numberDiff line change
@@ -363,39 +363,39 @@ TEST(SimpleChargingDockTests, GetDockDirection)
363363
dock.reset();
364364
}
365365

366-
TEST(SimpleChargingDockTests, BackwardBlind)
366+
TEST(SimpleChargingDockTests, ShouldRotateToDock)
367367
{
368368
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
369369

370-
// Case 1: Direction to BACKWARD and backward_blind to true
370+
// Case 1: Direction to BACKWARD and rotate_to_dock to true
371371
node->declare_parameter("my_dock.dock_direction", rclcpp::ParameterValue("backward"));
372-
node->declare_parameter("my_dock.backward_blind", rclcpp::ParameterValue(true));
372+
node->declare_parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true));
373373

374374
auto dock = std::make_unique<opennav_docking::SimpleChargingDock>();
375375
EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr));
376-
EXPECT_EQ(dock->isBackwardBlind(), true);
376+
EXPECT_EQ(dock->shouldRotateToDock(), true);
377377
dock->cleanup();
378378

379-
// Case 2: Direction to BACKWARD and backward_blind to false
379+
// Case 2: Direction to BACKWARD and rotate_to_dock to false
380380
node->set_parameter(
381-
rclcpp::Parameter("my_dock.backward_blind", rclcpp::ParameterValue(false)));
381+
rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false)));
382382
EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr));
383-
EXPECT_EQ(dock->isBackwardBlind(), false);
383+
EXPECT_EQ(dock->shouldRotateToDock(), false);
384384

385-
// Case 3: Direction to FORWARD and backward_blind to true
385+
// Case 3: Direction to FORWARD and rotate_to_dock to true
386386
node->set_parameter(
387387
rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("forward")));
388388
node->set_parameter(
389-
rclcpp::Parameter("my_dock.backward_blind", rclcpp::ParameterValue(true)));
389+
rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true)));
390390
EXPECT_THROW(dock->configure(node, "my_dock", nullptr), std::runtime_error);
391-
EXPECT_EQ(dock->isBackwardBlind(), true);
391+
EXPECT_EQ(dock->shouldRotateToDock(), true);
392392
dock->cleanup();
393393

394-
// Case 4: Direction to FORWARD and backward_blind to false
394+
// Case 4: Direction to FORWARD and rotate_to_dock to false
395395
node->set_parameter(
396-
rclcpp::Parameter("my_dock.backward_blind", rclcpp::ParameterValue(false)));
396+
rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false)));
397397
EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr));
398-
EXPECT_EQ(dock->isBackwardBlind(), false);
398+
EXPECT_EQ(dock->shouldRotateToDock(), false);
399399

400400
dock->cleanup();
401401
dock.reset();

‎nav2_docking/opennav_docking/test/test_simple_non_charging_dock.cpp

+13-13
Original file line numberDiff line numberDiff line change
@@ -324,39 +324,39 @@ TEST(SimpleNonChargingDockTests, GetDockDirection)
324324
dock.reset();
325325
}
326326

327-
TEST(SimpleChargingDockTests, BackwardBlind)
327+
TEST(SimpleChargingDockTests, ShouldRotateToDock)
328328
{
329329
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
330330

331-
// Case 1: Direction to BACKWARD and backward_blind to true
331+
// Case 1: Direction to BACKWARD and rotate_to_dock to true
332332
node->declare_parameter("my_dock.dock_direction", rclcpp::ParameterValue("backward"));
333-
node->declare_parameter("my_dock.backward_blind", rclcpp::ParameterValue(true));
333+
node->declare_parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true));
334334

335335
auto dock = std::make_unique<opennav_docking::SimpleNonChargingDock>();
336336
EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr));
337-
EXPECT_EQ(dock->isBackwardBlind(), true);
337+
EXPECT_EQ(dock->shouldRotateToDock(), true);
338338
dock->cleanup();
339339

340-
// Case 2: Direction to BACKWARD and backward_blind to false
340+
// Case 2: Direction to BACKWARD and rotate_to_dock to false
341341
node->set_parameter(
342-
rclcpp::Parameter("my_dock.backward_blind", rclcpp::ParameterValue(false)));
342+
rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false)));
343343
EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr));
344-
EXPECT_EQ(dock->isBackwardBlind(), false);
344+
EXPECT_EQ(dock->shouldRotateToDock(), false);
345345

346-
// Case 3: Direction to FORWARD and backward_blind to true
346+
// Case 3: Direction to FORWARD and rotate_to_dock to true
347347
node->set_parameter(
348348
rclcpp::Parameter("my_dock.dock_direction", rclcpp::ParameterValue("forward")));
349349
node->set_parameter(
350-
rclcpp::Parameter("my_dock.backward_blind", rclcpp::ParameterValue(true)));
350+
rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(true)));
351351
EXPECT_THROW(dock->configure(node, "my_dock", nullptr), std::runtime_error);
352-
EXPECT_EQ(dock->isBackwardBlind(), true);
352+
EXPECT_EQ(dock->shouldRotateToDock(), true);
353353
dock->cleanup();
354354

355-
// Case 4: Direction to FORWARD and backward_blind to false
355+
// Case 4: Direction to FORWARD and rotate_to_dock to false
356356
node->set_parameter(
357-
rclcpp::Parameter("my_dock.backward_blind", rclcpp::ParameterValue(false)));
357+
rclcpp::Parameter("my_dock.rotate_to_dock", rclcpp::ParameterValue(false)));
358358
EXPECT_NO_THROW(dock->configure(node, "my_dock", nullptr));
359-
EXPECT_EQ(dock->isBackwardBlind(), false);
359+
EXPECT_EQ(dock->shouldRotateToDock(), false);
360360

361361
dock->cleanup();
362362
dock.reset();

‎nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp

+5-4
Original file line numberDiff line numberDiff line change
@@ -137,17 +137,18 @@ class ChargingDock
137137
DockDirection getDockDirection() {return dock_direction_;}
138138

139139
/**
140-
* @brief Indicates if the dock is backward blind
141-
* @return bool True if the dock is backward blind
140+
* @brief Determines whether the robot should rotate 180º to face away from the dock.
141+
* For example, to perform a backward docking without detections.
142+
* @return bool If the robot should rotate to face away from the dock.
142143
*/
143-
bool isBackwardBlind() {return backward_blind_;}
144+
bool shouldRotateToDock() {return rotate_to_dock_;}
144145

145146
std::string getName() {return name_;}
146147

147148
protected:
148149
std::string name_;
149150
DockDirection dock_direction_{DockDirection::UNKNOWN};
150-
bool backward_blind_{false};
151+
bool rotate_to_dock_{false};
151152
};
152153

153154
} // namespace opennav_docking_core

0 commit comments

Comments
 (0)