Skip to content

Commit d88d2df

Browse files
bkoensgenbkoensgen
committed
fix: Address review feedback and fix unit tests
Signed-off-by: Koensgen Benjamin <[email protected]>
1 parent 48771be commit d88d2df

File tree

13 files changed

+186
-59
lines changed

13 files changed

+186
-59
lines changed

‎nav2_docking/README.md

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -41,9 +41,9 @@ The docking procedure is as follows:
4141
5. Enter a vision-control loop where the robot attempts to reach the docking pose while it's actively being refined by the vision system.
4242
6. Exit the vision-control loop once contact has been detected or charging has started (if applicable).
4343
7. Wait until charging starts (if applicable) and return success.
44-
8. Call the dock's plugin `stopDetectionProcess()` method to deactivate any external detection mechanisms (this is also called on failure/cancellation).
44+
8. Call the dock's plugin `stopDetectionProcess()` method to deactivate any external detection mechanisms.
4545

46-
If anywhere this procedure is unsuccessful (before step 8), `N` retries may be made. This involves calling `stopDetectionProcess()`, driving back to the dock's staging pose, and then restarting the process from step 3. If still unsuccessful after retries, it will return a failure code to indicate what kind of failure occurred to the client.
46+
If anywhere this procedure is unsuccessful (before step 8), `N` retries may be made, driving back to the dock's staging pose, and then restarting the process from step 3. If still unsuccessful after retries, it will return a failure code to indicate what kind of failure occurred to the client.
4747

4848
Undocking works more simply:
4949
1. If previously docked, use the known dock information to get the dock type. If not, use the undock action request's indicated dock type
@@ -153,10 +153,6 @@ There are two functions used during dock approach:
153153
for charging to start by calling `isCharging`.
154154
* This may be implemented using the `sensor_msgs/BatteryState` message to check the power status or for charging current.
155155
* Used for charging-typed plugins.
156-
+ * `startDetectionProcess`: Start any detection pipelines required for pose refinement.
157-
This is expected to be called by the server prior to perception.
158-
+ * `stopDetectionProcess`: Stop any detection pipelines running for pose refinement.
159-
Called by the server when docking actions finish or on shutdown.
160156

161157
Similarly, there are two functions used during undocking:
162158

‎nav2_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -108,12 +108,12 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock
108108
/**
109109
* @brief Start external detection process (service call + subscribe).
110110
*/
111-
void startDetectionProcess() override {startDetection();}
111+
bool startDetectionProcess() override {return startDetection();}
112112

113113
/**
114114
* @brief Stop external detection process.
115115
*/
116-
void stopDetectionProcess() override {stopDetection();}
116+
bool stopDetectionProcess() override {return stopDetection();}
117117

118118
protected:
119119
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state);
@@ -180,10 +180,10 @@ class SimpleChargingDock : public opennav_docking_core::ChargingDock
180180
DetectorState detector_state_{DetectorState::OFF};
181181

182182
// Internally enable detector (service + subscribe)
183-
void startDetection();
183+
bool startDetection();
184184

185185
// Internally disable detector (service + unsubscribe)
186-
void stopDetection();
186+
bool stopDetection();
187187
};
188188

189189
} // namespace opennav_docking

‎nav2_docking/opennav_docking/include/opennav_docking/simple_non_charging_dock.hpp

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -93,12 +93,12 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock
9393
/**
9494
* @brief Start external detection process (service call + subscribe).
9595
*/
96-
void startDetectionProcess() override {startDetection();}
96+
bool startDetectionProcess() override {return startDetection();}
9797

9898
/**
9999
* @brief Stop external detection process.
100100
*/
101-
void stopDetectionProcess() override {stopDetection();}
101+
bool stopDetectionProcess() override {return stopDetection();}
102102

103103
protected:
104104
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr state);
@@ -158,10 +158,10 @@ class SimpleNonChargingDock : public opennav_docking_core::NonChargingDock
158158
DetectorState detector_state_{DetectorState::OFF};
159159

160160
// Internally enable detector (service + subscribe)
161-
void startDetection();
161+
bool startDetection();
162162

163163
// Internally disable detector (service + unsubscribe)
164-
void stopDetection();
164+
bool stopDetection();
165165
};
166166

167167
} // namespace opennav_docking

‎nav2_docking/opennav_docking/src/docking_server.cpp

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -438,13 +438,18 @@ Dock * DockingServer::generateGoalDock(std::shared_ptr<const DockRobot::Goal> go
438438
void DockingServer::doInitialPerception(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose)
439439
{
440440
publishDockingFeedback(DockRobot::Feedback::INITIAL_PERCEPTION);
441-
dock->plugin->startDetectionProcess();
441+
442+
if (!dock->plugin->startDetectionProcess()) {
443+
throw opennav_docking_core::FailedToDetectDock("Failed to start the detection process.");
444+
}
445+
442446
rclcpp::Rate loop_rate(controller_frequency_);
443447
auto start = this->now();
444448
auto timeout = rclcpp::Duration::from_seconds(initial_perception_timeout_);
445449
while (!dock->plugin->getRefinedPose(dock_pose, dock->id)) {
446450
if (this->now() - start > timeout) {
447-
throw opennav_docking_core::FailedToDetectDock("Failed initial dock detection");
451+
throw opennav_docking_core::FailedToDetectDock(
452+
"Failed initial dock detection: Timeout exceeded");
448453
}
449454

450455
if (checkAndWarnIfCancelled(docking_action_server_, "dock_robot") ||

‎nav2_docking/opennav_docking/src/simple_charging_dock.cpp

Lines changed: 40 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -179,7 +179,7 @@ void SimpleChargingDock::configure(
179179
// subscription alive by creating it here during configuration
180180
if (!subscribe_toggle_ && use_external_detection_pose_ && !detected_pose_sub_) {
181181
detected_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
182-
"detected_dock_pose", rclcpp::SensorDataQoS(),
182+
"detected_dock_pose", 1,
183183
[this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) {
184184
detected_dock_pose_ = *pose;
185185
detector_state_ = DetectorState::ON;
@@ -224,6 +224,11 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose,
224224
return true;
225225
}
226226

227+
// Guard against using pose data before the first detection has arrived.
228+
if (detector_state_ != DetectorState::ON) {
229+
return false;
230+
}
231+
227232
// If using detections, get current detections, transform to frame, and apply offsets
228233
geometry_msgs::msg::PoseStamped detected = detected_dock_pose_;
229234

@@ -350,58 +355,78 @@ void SimpleChargingDock::jointStateCallback(const sensor_msgs::msg::JointState::
350355
is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_);
351356
}
352357

353-
void SimpleChargingDock::startDetection()
358+
bool SimpleChargingDock::startDetection()
354359
{
355360
// Skip if already starting or ON
356-
if (detector_state_ != DetectorState::OFF) {return;}
361+
if (detector_state_ != DetectorState::OFF) {
362+
return true;
363+
}
357364

358365
// 1. Service START request
359366
if (detector_client_) {
360367
auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
361368
try {
362-
detector_client_->invoke(
369+
auto future = detector_client_->invoke(
363370
req,
364371
std::chrono::duration_cast<std::chrono::nanoseconds>(
365372
std::chrono::duration<double>(detector_service_timeout_)));
373+
374+
if (!future || !future->success) {
375+
RCLCPP_ERROR(
376+
node_->get_logger(), "Detector service '%s' failed to start.",
377+
detector_service_name_.c_str());
378+
return false;
379+
}
366380
} catch (const std::exception & e) {
367-
RCLCPP_WARN(node_->get_logger(),
368-
"Detector service '%s' failed: %s",
381+
RCLCPP_ERROR(
382+
node_->get_logger(), "Calling detector service '%s' failed: %s",
369383
detector_service_name_.c_str(), e.what());
384+
return false;
370385
}
371386
}
372387

373388
// 2. Subscription toggle
374389
// Only subscribe once; will set state to ON on first message
375390
if (subscribe_toggle_ && !detected_pose_sub_) {
376391
detected_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
377-
"detected_dock_pose", rclcpp::SensorDataQoS(),
392+
"detected_dock_pose", 1,
378393
[this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) {
379394
detected_dock_pose_ = *pose;
380395
detector_state_ = DetectorState::ON;
381396
});
382397
}
383398

384-
detector_state_ = DetectorState::ON;
385-
RCLCPP_INFO(node_->get_logger(), "Detector START requested, state set to ON");
399+
RCLCPP_INFO(node_->get_logger(), "Detector START requested.");
400+
return true;
386401
}
387402

388-
void SimpleChargingDock::stopDetection()
403+
bool SimpleChargingDock::stopDetection()
389404
{
390405
// Skip if already OFF
391-
if (detector_state_ == DetectorState::OFF) {return;}
406+
if (detector_state_ == DetectorState::OFF) {
407+
return true;
408+
}
392409

393410
// 1. Service STOP request
394411
if (detector_client_) {
395412
auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
396413
try {
397-
detector_client_->invoke(
414+
auto future = detector_client_->invoke(
398415
req,
399416
std::chrono::duration_cast<std::chrono::nanoseconds>(
400417
std::chrono::duration<double>(detector_service_timeout_)));
418+
419+
if (!future || !future->success) {
420+
RCLCPP_ERROR(
421+
node_->get_logger(), "Detector service '%s' failed to stop.",
422+
detector_service_name_.c_str());
423+
return false;
424+
}
401425
} catch (const std::exception & e) {
402-
RCLCPP_WARN(node_->get_logger(),
403-
"Detector service '%s' failed: %s",
426+
RCLCPP_ERROR(
427+
node_->get_logger(), "Calling detector service '%s' failed: %s",
404428
detector_service_name_.c_str(), e.what());
429+
return false;
405430
}
406431
}
407432

@@ -413,6 +438,7 @@ void SimpleChargingDock::stopDetection()
413438

414439
detector_state_ = DetectorState::OFF;
415440
RCLCPP_INFO(node_->get_logger(), "Detector STOP requested");
441+
return true;
416442
}
417443

418444
void SimpleChargingDock::activate()

‎nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp

Lines changed: 40 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -160,7 +160,7 @@ void SimpleNonChargingDock::configure(
160160
// always processed when using an external detector
161161
if (!subscribe_toggle_ && use_external_detection_pose_ && !detected_pose_sub_) {
162162
detected_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
163-
"detected_dock_pose", rclcpp::SensorDataQoS(),
163+
"detected_dock_pose", 1,
164164
[this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) {
165165
detected_dock_pose_ = *pose;
166166
detector_state_ = DetectorState::ON;
@@ -205,6 +205,10 @@ bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pos
205205
return true;
206206
}
207207

208+
// Guard against using pose data before the first detection has arrived.
209+
if (detector_state_ != DetectorState::ON) {
210+
return false;
211+
}
208212

209213
// If using detections, get current detections, transform to frame, and apply offsets
210214
geometry_msgs::msg::PoseStamped detected = detected_dock_pose_;
@@ -317,57 +321,78 @@ void SimpleNonChargingDock::jointStateCallback(const sensor_msgs::msg::JointStat
317321
is_stalled_ = (velocity < stall_velocity_threshold_) && (effort > stall_effort_threshold_);
318322
}
319323

320-
void SimpleNonChargingDock::startDetection()
324+
bool SimpleNonChargingDock::startDetection()
321325
{
322-
if (detector_state_ != DetectorState::OFF) {return;}
326+
// Skip if already starting or ON
327+
if (detector_state_ != DetectorState::OFF) {
328+
return true;
329+
}
323330

324331
// 1. Service START request
325332
if (detector_client_) {
326333
auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
327334
try {
328-
detector_client_->invoke(
335+
auto future = detector_client_->invoke(
329336
req,
330337
std::chrono::duration_cast<std::chrono::nanoseconds>(
331338
std::chrono::duration<double>(detector_service_timeout_)));
339+
340+
if (!future || !future->success) {
341+
RCLCPP_ERROR(
342+
node_->get_logger(), "Detector service '%s' failed to start.",
343+
detector_service_name_.c_str());
344+
return false;
345+
}
332346
} catch (const std::exception & e) {
333-
RCLCPP_WARN(node_->get_logger(),
334-
"Detector service '%s' failed: %s",
347+
RCLCPP_ERROR(
348+
node_->get_logger(), "Calling detector service '%s' failed: %s",
335349
detector_service_name_.c_str(), e.what());
350+
return false;
336351
}
337352
}
338353

339354
// 2. Subscription toggle
340355
// Only subscribe once; will set state to ON on first message
341356
if (subscribe_toggle_ && !detected_pose_sub_) {
342357
detected_pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
343-
"detected_dock_pose", rclcpp::SensorDataQoS(),
358+
"detected_dock_pose", 1,
344359
[this](const geometry_msgs::msg::PoseStamped::SharedPtr pose) {
345360
detected_dock_pose_ = *pose;
346361
detector_state_ = DetectorState::ON;
347362
});
348363
}
349364

350-
detector_state_ = DetectorState::ON;
351-
RCLCPP_INFO(node_->get_logger(), "Detector START requested, state set to ON");
365+
RCLCPP_INFO(node_->get_logger(), "Detector START requested.");
366+
return true;
352367
}
353368

354-
void SimpleNonChargingDock::stopDetection()
369+
bool SimpleNonChargingDock::stopDetection()
355370
{
356371
// Skip if already OFF
357-
if (detector_state_ == DetectorState::OFF) {return;}
372+
if (detector_state_ == DetectorState::OFF) {
373+
return true;
374+
}
358375

359376
// 1. Service STOP request
360377
if (detector_client_) {
361378
auto req = std::make_shared<std_srvs::srv::Trigger::Request>();
362379
try {
363-
detector_client_->invoke(
380+
auto future = detector_client_->invoke(
364381
req,
365382
std::chrono::duration_cast<std::chrono::nanoseconds>(
366383
std::chrono::duration<double>(detector_service_timeout_)));
384+
385+
if (!future || !future->success) {
386+
RCLCPP_ERROR(
387+
node_->get_logger(), "Detector service '%s' failed to stop.",
388+
detector_service_name_.c_str());
389+
return false;
390+
}
367391
} catch (const std::exception & e) {
368-
RCLCPP_WARN(node_->get_logger(),
369-
"Detector service '%s' failed: %s",
392+
RCLCPP_ERROR(
393+
node_->get_logger(), "Calling detector service '%s' failed: %s",
370394
detector_service_name_.c_str(), e.what());
395+
return false;
371396
}
372397
}
373398

@@ -379,6 +404,7 @@ void SimpleNonChargingDock::stopDetection()
379404

380405
detector_state_ = DetectorState::OFF;
381406
RCLCPP_INFO(node_->get_logger(), "Detector STOP requested");
407+
return true;
382408
}
383409

384410
void SimpleNonChargingDock::activate()

‎nav2_docking/opennav_docking/test/test_docking_server.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -134,7 +134,7 @@ def setUp(self) -> None:
134134
self.odom_pub = self.node.create_publisher(Odometry, 'odom', 10)
135135

136136
def wait_for_node_to_be_active(self, node_name, timeout_sec=10.0):
137-
"""Attend qu'un nœud managé passe à l'état actif."""
137+
"""Wait for a managed node to become active."""
138138
client = self.node.create_client(GetState, f'{node_name}/get_state')
139139
if not client.wait_for_service(timeout_sec=2.0):
140140
self.fail(f'Service get_state for {node_name} not available.')
@@ -148,6 +148,7 @@ def wait_for_node_to_be_active(self, node_name, timeout_sec=10.0):
148148
self.node.get_logger().info(f'Node {node_name} is active.')
149149
return
150150
time.sleep(0.5)
151+
# raises AssertionError
151152
self.fail(f'Node {node_name} did not become active within {timeout_sec} seconds.')
152153

153154
def tearDown(self) -> None:

0 commit comments

Comments
 (0)