diff --git a/nav2_port_drayage_demo/include/nav2_port_drayage_demo/port_drayage_demo.hpp b/nav2_port_drayage_demo/include/nav2_port_drayage_demo/port_drayage_demo.hpp index f6b6b0a..324399f 100644 --- a/nav2_port_drayage_demo/include/nav2_port_drayage_demo/port_drayage_demo.hpp +++ b/nav2_port_drayage_demo/include/nav2_port_drayage_demo/port_drayage_demo.hpp @@ -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 @@ -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 diff --git a/nav2_port_drayage_demo/src/port_drayage_demo.cpp b/nav2_port_drayage_demo/src/port_drayage_demo.cpp index c658094..6e59cef 100644 --- a/nav2_port_drayage_demo/src/port_drayage_demo.cpp +++ b/nav2_port_drayage_demo/src/port_drayage_demo.cpp @@ -110,8 +110,6 @@ auto PortDrayageDemo::on_configure(const rclcpp_lifecycle::State & /* state */) mobility_operation_publisher_ = create_publisher("outgoing_mobility_operation", 1); - actively_executing_operation_ = false; - return nav2_util::CallbackReturn::SUCCESS; } @@ -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; @@ -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( @@ -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: @@ -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(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(); } } diff --git a/nav2_port_drayage_demo/test/test_port_drayage_demo.cpp b/nav2_port_drayage_demo/test/test_port_drayage_demo.cpp index 020cf57..03a1210 100644 --- a/nav2_port_drayage_demo/test/test_port_drayage_demo.cpp +++ b/nav2_port_drayage_demo/test/test_port_drayage_demo.cpp @@ -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::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 @@ -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::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 @@ -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::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 @@ -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::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)