Skip to content

Commit

Permalink
Lint
Browse files Browse the repository at this point in the history
Signed-off-by: redvinaa <[email protected]>
  • Loading branch information
redvinaa committed Aug 7, 2024
1 parent 32c8375 commit 6a0b5c4
Show file tree
Hide file tree
Showing 2 changed files with 13 additions and 8 deletions.
6 changes: 3 additions & 3 deletions nav2_docking/opennav_docking/src/docking_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -254,9 +254,9 @@ void DockingServer::dockRobot()
RCLCPP_INFO(get_logger(), "Robot already within pre-staging pose tolerance for dock");
} else {
std::function<bool()> isPreempted = [this]() {
return checkAndWarnIfCancelled(docking_action_server_, "dock_robot") ||
checkAndWarnIfPreempted(docking_action_server_, "dock_robot");
};
return checkAndWarnIfCancelled(docking_action_server_, "dock_robot") ||
checkAndWarnIfPreempted(docking_action_server_, "dock_robot");
};

navigator_->goToPose(
initial_staging_pose,
Expand Down
15 changes: 10 additions & 5 deletions nav2_docking/opennav_docking/test/test_navigator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -102,28 +102,33 @@ TEST(NavigatorTests, TestNavigator)
// Should succeed, action server set to succeed
dummy_navigator_node->setReturn(true);
EXPECT_NO_THROW(
navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), is_preempted_false));
navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
is_preempted_false));

// Should fail, timeout exceeded
EXPECT_THROW(
navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0), is_preempted_false),
navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0),
is_preempted_false),
opennav_docking_core::FailedToStage);

// Should fail, action server set to succeed
dummy_navigator_node->setReturn(false);
EXPECT_THROW(
navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), is_preempted_false),
navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
is_preempted_false),
opennav_docking_core::FailedToStage);

// First should fail, recursion should succeed
dummy_navigator_node->setToggle();
EXPECT_NO_THROW(
navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), is_preempted_false));
navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
is_preempted_false));

// Should fail, preempted
dummy_navigator_node->setReturn(true);
EXPECT_THROW(
navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), is_preempted_true),
navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0),
is_preempted_true),
opennav_docking_core::FailedToStage);

navigator->deactivate();
Expand Down

0 comments on commit 6a0b5c4

Please sign in to comment.