Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

CF 965 Enable Dynamic Rerouting #20

Merged
merged 1 commit into from
Dec 6, 2024
Merged

CF 965 Enable Dynamic Rerouting #20

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 0 additions & 4 deletions nav2_port_drayage_demo/include/nav2_port_drayage_demo/port_drayage_demo.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -188,8 +188,6 @@ class PortDrayageDemo : public rclcpp_lifecycle::LifecycleNode

auto get_cargo_id() -> std::string { return cargo_id_; }

auto is_actively_executing_operation() -> bool { return actively_executing_operation_; }

private:
////
// Pubs & Subs
Expand Down Expand Up @@ -220,8 +218,6 @@ class PortDrayageDemo : public rclcpp_lifecycle::LifecycleNode
int message_processing_delay_;
// Cargo identified
std::string cargo_id_;
// Flag for whether vehicle is busy
bool actively_executing_operation_ = false;
// Clock
rclcpp::Clock::SharedPtr clock_;
// Previously received Mobility Operation message
Expand Down
30 changes: 5 additions & 25 deletions nav2_port_drayage_demo/src/port_drayage_demo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -110,8 +110,6 @@ auto PortDrayageDemo::on_configure(const rclcpp_lifecycle::State & /* state */)
mobility_operation_publisher_ =
create_publisher<carma_v2x_msgs::msg::MobilityOperation>("outgoing_mobility_operation", 1);

actively_executing_operation_ = false;

return nav2_util::CallbackReturn::SUCCESS;
}

Expand All @@ -130,14 +128,6 @@ auto PortDrayageDemo::on_deactivate(const rclcpp_lifecycle::State & /* state */)
auto PortDrayageDemo::on_mobility_operation_received(
const carma_v2x_msgs::msg::MobilityOperation & msg) -> void
{
if (actively_executing_operation_) {
RCLCPP_WARN_STREAM(
get_logger(), "Ignoring received port drayage operation '"
<< msg.strategy_params << "' while already executing the operation '"
<< previous_mobility_operation_msg_.operation << "'");

return;
}
if (!extract_port_drayage_message(msg)) return;
rclcpp::sleep_for(std::chrono::seconds(message_processing_delay_));
nav2_msgs::action::FollowWaypoints::Goal goal;
Expand All @@ -152,7 +142,6 @@ auto PortDrayageDemo::on_mobility_operation_received(
send_goal_options.result_callback =
std::bind(&PortDrayageDemo::on_result_received, this, std::placeholders::_1);
follow_waypoints_client_->async_send_goal(goal, send_goal_options);
actively_executing_operation_ = true;
}

auto PortDrayageDemo::on_result_received(
Expand All @@ -164,7 +153,6 @@ auto PortDrayageDemo::on_result_received(
// Create arrival message
carma_v2x_msgs::msg::MobilityOperation result = compose_arrival_message();
mobility_operation_publisher_->publish(std::move(result));
actively_executing_operation_ = false;
return;
}
case rclcpp_action::ResultCode::ABORTED:
Expand All @@ -189,19 +177,11 @@ auto PortDrayageDemo::on_rviz_goal_status_received(
const action_msgs::msg::GoalStatusArray & msg) -> void
{
const auto & goal = msg.status_list[msg.status_list.size() - 1];
if (!actively_executing_operation_) {
if (goal.status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || goal.status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) {
previous_mobility_operation_msg_.operation = std::shared_ptr<OperationID>(new OperationID(OperationID::Operation::ENTER_STAGING_AREA));
actively_executing_operation_ = true;
}
} else {
if (goal.status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) {
// Create arrival message
carma_v2x_msgs::msg::MobilityOperation result = compose_arrival_message();
mobility_operation_publisher_->publish(std::move(result));
actively_executing_operation_ = false;
rviz_action_subscription_.reset();
}
if (goal.status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) {
// Create arrival message
carma_v2x_msgs::msg::MobilityOperation result = compose_arrival_message();
mobility_operation_publisher_->publish(std::move(result));
rviz_action_subscription_.reset();
}
}

Expand Down
30 changes: 0 additions & 30 deletions nav2_port_drayage_demo/test/test_port_drayage_demo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,17 +44,14 @@ TEST(PortDrayageTest, cmvIdTest)
mobility_operation_json["cargo"] = true;
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_FALSE(node->is_actively_executing_operation());
cmd.m_header.sender_id = "test_cmv_id";
mobility_operation_json["cmv_id"] = "test_cmv_id";
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
rclcpp_action::ClientGoalHandle<nav2_msgs::action::FollowWaypoints>::WrappedResult result;
result.code = rclcpp_action::ResultCode::SUCCEEDED;
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "new_cargo");
ASSERT_FALSE(node->is_actively_executing_operation());
}

// Test vehicle only responds to incoming mobility operation messages with strategy carma/port_drayage
Expand All @@ -79,15 +76,12 @@ TEST(PortDrayageTest, strategyTest)
mobility_operation_json["cargo"] = true;
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_FALSE(node->is_actively_executing_operation());
cmd.strategy = "carma/port_drayage";
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
rclcpp_action::ClientGoalHandle<nav2_msgs::action::FollowWaypoints>::WrappedResult result;
result.code = rclcpp_action::ResultCode::SUCCEEDED;
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "new_cargo");
ASSERT_FALSE(node->is_actively_executing_operation());
}

// Test vehicle pickup and dropoff
Expand All @@ -113,22 +107,18 @@ TEST(PortDrayageTest, pickupAndDropoffTest)
mobility_operation_json["cargo"] = true;
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
ASSERT_EQ(node->get_cargo_id(), "");
rclcpp_action::ClientGoalHandle<nav2_msgs::action::FollowWaypoints>::WrappedResult result;
result.code = rclcpp_action::ResultCode::SUCCEEDED;
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "new_cargo");
ASSERT_FALSE(node->is_actively_executing_operation());
// DROPOFF
mobility_operation_json["operation"] = "DROPOFF";
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
ASSERT_EQ(node->get_cargo_id(), "new_cargo");
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "");
ASSERT_FALSE(node->is_actively_executing_operation());
}

// Test vehicle ack formatting
Expand Down Expand Up @@ -188,88 +178,68 @@ TEST(PortDrayageTest, fullDemoTest)
mobility_operation_json["cargo"] = true;
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
rclcpp_action::ClientGoalHandle<nav2_msgs::action::FollowWaypoints>::WrappedResult result;
result.code = rclcpp_action::ResultCode::SUCCEEDED;
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "");
ASSERT_FALSE(node->is_actively_executing_operation());
// PICKUP
mobility_operation_json["operation"] = "PICKUP";
mobility_operation_json["cargo_id"] = "CARGO_A";
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "CARGO_A");
ASSERT_FALSE(node->is_actively_executing_operation());
// EXIT STAGING AREA
mobility_operation_json["operation"] = "EXIT_STAGING_AREA";
mobility_operation_json["cargo_id"] = "";
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "CARGO_A");
ASSERT_FALSE(node->is_actively_executing_operation());
// ENTER PORT
mobility_operation_json["operation"] = "ENTER_PORT";
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "CARGO_A");
ASSERT_FALSE(node->is_actively_executing_operation());
// DROPOFF
mobility_operation_json["operation"] = "DROPOFF";
mobility_operation_json["cargo_id"] = "CARGO_A";
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "");
ASSERT_FALSE(node->is_actively_executing_operation());
// PICKUP
mobility_operation_json["operation"] = "PICKUP";
mobility_operation_json["cargo_id"] = "CARGO_B";
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "CARGO_B");
ASSERT_FALSE(node->is_actively_executing_operation());
// PORT CHECKPOINT
mobility_operation_json["operation"] = "PORT_CHECKPOINT";
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "CARGO_B");
ASSERT_FALSE(node->is_actively_executing_operation());
// HOLDING AREA
mobility_operation_json["operation"] = "HOLDING_AREA";
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "CARGO_B");
ASSERT_FALSE(node->is_actively_executing_operation());
// EXIT PORT
mobility_operation_json["operation"] = "EXIT_PORT";
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "CARGO_B");
ASSERT_FALSE(node->is_actively_executing_operation());
// ENTER STAGING AREA
mobility_operation_json["operation"] = "ENTER_STAGING_AREA";
cmd.strategy_params = mobility_operation_json.dump();
node->on_mobility_operation_received(cmd);
ASSERT_TRUE(node->is_actively_executing_operation());
node->on_result_received(result);
ASSERT_EQ(node->get_cargo_id(), "CARGO_B");
ASSERT_FALSE(node->is_actively_executing_operation());
}

int main(int argc, char ** argv)
Expand Down
Loading