diff --git a/include/am_super/super_node_mediator.h b/include/am_super/super_node_mediator.h index 2a191a1..ad51abc 100644 --- a/include/am_super/super_node_mediator.h +++ b/include/am_super/super_node_mediator.h @@ -73,6 +73,10 @@ class SuperNodeMediator /** manifest node (generated from manifest param) */ std::vector manifest; + /** Errored out nodes*/ + // The second part of the pair does not matter + std::unordered_map errored_nodes_; + /** * Overall state of the system, cumulative of the nodes * and super together. @@ -93,6 +97,10 @@ class SuperNodeMediator /** Signals if any of the manifested nodes status errored */ bool status_error = false; + + /** Allows supervisor to start the FP automatically when ready */ + bool start_fp_from_super_ = false; + }; /**Encapsulates properties and methods that relate to the transition of states @@ -155,6 +163,9 @@ class SuperNodeMediator /** if True, then command should be sent. if ready_for_transition=true, then this is false*/ bool resend_life_cycle_command; + /** If this is an error transition, then we want to handle their lifecycles accordingly */ + bool error_transition; + /** The command that notifies nodes to continue processing so the state can transition*/ LifeCycleCommand life_cycle_command; @@ -199,7 +210,7 @@ class SuperNodeMediator * @param supervisor is in charge of knowing the state of the system * @return pair with the boolean indicating transition is ready and the optional state if ready. */ - SuperNodeMediator::TransitionInstructions transitionReady(Supervisor supervisor); + SuperNodeMediator::TransitionInstructions transitionReady(Supervisor& supervisor); /** * FIXME: currently public for unit testing diff --git a/src/am_super/am_super.cpp b/src/am_super/am_super.cpp index 1105089..2fc61c8 100644 --- a/src/am_super/am_super.cpp +++ b/src/am_super/am_super.cpp @@ -8,6 +8,7 @@ #include #include #include +#include "std_msgs/msg/int32_multi_array.hpp" #include #include @@ -20,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -197,12 +199,15 @@ class AMSuper rclcpp::Publisher::SharedPtr vstate_summary_pub_; rclcpp::Publisher::SharedPtr system_state_pub_; rclcpp::Publisher::SharedPtr super_status_pub_; + rclcpp::Publisher::SharedPtr super_errored_pub_; rclcpp::Publisher::SharedPtr led_pub_; /** stops the flight plan when SHUTDOWN state */ rclcpp::Publisher::SharedPtr flight_plan_deactivation_pub_; + rclcpp::Publisher::SharedPtr set_gpio_pin_pub_; rclcpp::Subscription::SharedPtr node_state_sub_; rclcpp::Subscription::SharedPtr operator_command_sub_; rclcpp::Subscription::SharedPtr fake_operator_command_sub_; + rclcpp::Subscription::SharedPtr ui_operator_command_sub_; rclcpp::Subscription::SharedPtr controller_state_sub; rclcpp::Subscription::SharedPtr diagnostics_sub; rclcpp::Subscription::SharedPtr current_enu_sub; @@ -226,6 +231,19 @@ class AMSuper * amount of time in seconds without hearing from a node that will cause it to go offline */ double node_timeout_s_; + bool offline_timer_running_ = false; + rclcpp::Time offline_start_time_; // start_timer from here + const rclcpp::Duration offline_allowance_ = am::toDuration(20.0); + bool reset_timer_running_ = false; + rclcpp::Time reset_start_time_; // start_timer from here + const rclcpp::Duration reset_allowance_ = am::toDuration(5.0); // How much time we give am_locator to lock in. + bool reset_required_ = false; + bool restart_required_ = false; + bool offline_restart_required_ = false; + bool flight_plan_running_ = false; + bool first_time_booted_ = false; + rclcpp::Time booting_start_time_ = am::Node::node->now(); + const rclcpp::Duration booting_allowance_ = am::toDuration(20.0); /** * baglogger level @@ -246,6 +264,9 @@ class AMSuper am::getParam("node_timeout_s", node_timeout_s_, 3.1); ROS_INFO_STREAM( "node_timeout_s = " << node_timeout_s_); + am::getParam("start_fp_from_super", supervisor_.start_fp_from_super_, false); + ROS_INFO_STREAM("start_fp_from_super = " << supervisor_.start_fp_from_super_); + /* * create initial node list from manifest */ @@ -287,7 +308,7 @@ class AMSuper // vehicle state: position, velocity, etc. vstate_summary_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATE, am::getSensorQoS(10)); - // system state: BOOTING, READY, AUTO, etc. + // system state: BOOTING, READY, AUTO, etc. ("/system/state") system_state_pub_ = am::Node::node->create_publisher(am_topics::SYSTEM_STATE, am::getSensorQoS(10)); // lifecycle command: CONFIGURE, ACTIVATE, etc. - sent to all nodes to control lifecycle @@ -300,11 +321,18 @@ class AMSuper // super state: manifest info super_status_pub_ = am::Node::node->create_publisher(am_super_topics::SUPER_STATUS, am::getSensorQoS(10)); + // super state: errored_nodes info + super_errored_pub_ = am::Node::node->create_publisher("/super/errored_nodes", am::getSensorQoS(10)); + + // ??? rclcpp::QoS qos_profile(1); qos_profile.reliable(); flight_plan_deactivation_pub_ = am::Node::node->create_publisher(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, qos_profile); + // Tractor Light + set_gpio_pin_pub_ = am::Node::node->create_publisher("/set_gpio_pin", 1); + supervisor_.system_state = SuperState::BOOTING; supervisor_.flt_ctrl_state = SuperNodeMediator::SuperFltCtrlState::INIT; @@ -330,6 +358,10 @@ class AMSuper fake_operator_command_sub_ = am::Node::node->create_subscription(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, 100, std::bind(&AMSuper::fakeOperatorCommandCB, this, std::placeholders::_1)); + ui_operator_command_sub_ = am::Node::node->create_subscription("/ui_operator_cmd", 100, + std::bind(&AMSuper::uiOperatorCommandCB, this, std::placeholders::_1)); + + // controller state: ??? controller_state_sub = am::Node::node->create_subscription(am_super_topics::CONTROLLER_STATE, 100, std::bind(&AMSuper::controllerStateCB, this, std::placeholders::_1)); @@ -402,8 +434,16 @@ class AMSuper void fakeOperatorCommandCB(const std_msgs::msg::Bool::SharedPtr msg) { + // This is listening to see if the flight plan is running or not + flight_plan_running_ = msg->data; + + if (supervisor_.start_fp_from_super_) + { + return; + } + RCLCPP_INFO(am::Node::node->get_logger(), "Received FAKE Operator Command (actually flight_controller command): %i", msg->data); - if (msg->data == true) + if (msg->data == true && !supervisor_.start_fp_from_super_) // We don't want this to be receieved if we are starting the fp elsewhere { node_mediator_.setOperatorCommand(supervisor_, OperatorCommand::LAUNCH); } @@ -415,6 +455,25 @@ class AMSuper LOG_MSG(am_topics::CTRL_FLIGHTPLAN_ACTIVITY_CONTROL, *msg, SU_LOG_LEVEL); } + void uiOperatorCommandCB(const std_msgs::msg::Bool::SharedPtr msg) + { + if (!supervisor_.start_fp_from_super_) + { + return; + } + + RCLCPP_INFO(am::Node::node->get_logger(), "Received UI Operator Command: %i", msg->data); + if (msg->data == true) + { + node_mediator_.setOperatorCommand(supervisor_, OperatorCommand::LAUNCH); + } + else + { + node_mediator_.setOperatorCommand(supervisor_, OperatorCommand::CANCEL); + stopFlightPlan(); + } + } + /** * process state from lifecycle nodes * @param node_name_in @@ -435,7 +494,7 @@ class AMSuper // search for the node in the list bool nodes_changed = false; map::iterator it; - it = supervisor_.nodes.find(node_name); + it = supervisor_.nodes.find(node_name); // This specifically only reacts to the specific node experiencing the callback if (it != supervisor_.nodes.end()) { // if we get here, the node is already in our list @@ -455,15 +514,47 @@ class AMSuper if (nr.status != status) { ROS_INFO_STREAM( node_name << " changed status to = " << life_cycle_mediator_.statusToString(status) << " [09SI]"); + if (nr.manifested && nr.status == LifeCycleStatus::ERROR) + { + // This means we have now Come OUT of error + if (supervisor_.errored_nodes_.find(nr.name) == supervisor_.errored_nodes_.end()) + { + ROS_ERROR_STREAM("We should never be here. Node " << nr.name << " was in the errored_nodes_ list incorrectly"); + } + // Remove the node from the errored_nodes list + supervisor_.errored_nodes_.erase(nr.name); + if (nr.name == "am_locator") + { + reset_timer_running_ = false; + } + } + nr.status = status; nodes_changed = true; - if(nr.manifested && nr.status == LifeCycleStatus::ERROR && supervisor_.system_state != SuperState::BOOTING) + // if(nr.manifested && nr.status == LifeCycleStatus::ERROR && supervisor_.system_state != SuperState::BOOTING) + if(nr.manifested && status == LifeCycleStatus::ERROR) { + if (supervisor_.errored_nodes_.find(nr.name) == supervisor_.errored_nodes_.end()) + { + // The node is not in the errored_nodes_ list, so it should be added + supervisor_.errored_nodes_[nr.name] = true; + } + if (nr.name == "am_locator") + { + if (!reset_timer_running_) + { + reset_start_time_ = am::Node::node->now(); + reset_timer_running_ = true; + } + } supervisor_.status_error = true; - ROS_INFO_STREAM( "Manifested node " << nr.name << " changed status to ERROR. Shutting down nodes... [JHRE]"); + ROS_INFO_STREAM( "Manifested node " << nr.name << " changed status to ERROR. Stopping Flight Plan... [JHRE]"); // TODO: put this back in somehow - need to rethink how am_super influsenes control stopFlightPlan(); + node_mediator_.setOperatorCommand(supervisor_, OperatorCommand::CANCEL); } + + // if (nr.manifested && nr.status == LifeCycleStatus::ACTIVE) } // TODO: need to test the pid stuff - not sure if it is working @@ -481,6 +572,8 @@ class AMSuper nr.pid = pid; nodes_changed = true; } + + nr.last_contact = last_contact; } else @@ -605,6 +698,16 @@ class AMSuper ROS_ERROR_STREAM( "Sending flight plan kill command."); } + /** Send signal to flight controller that flight can be started. */ + void startFlightPlan() + { + std_msgs::msg::Bool msg; + msg.data = true; //false means deactivate + flight_plan_deactivation_pub_->publish(msg); + ROS_ERROR_STREAM( "Sending flight plan start command."); + + } + /** * check for state transition based upon current state and values of member fields. * Will call to modify the system state if transition is necessary. Will also call @@ -654,8 +757,30 @@ class AMSuper } i++; } - } - // } + } + if (transition_instructions.error_transition) + { + ROS_WARN_STREAM("ERROR TRANSITION LIFECYCLE COMMANDS"); + // For each node in the manifest, we must bring it back down to INACTIVE + // If it was in error, we want to bring it down, clean it up, and then bring it back up to INACTIVE + map::iterator it; + for (it = supervisor_.nodes.begin(); it != supervisor_.nodes.end(); it++) + { + SuperNodeMediator::SuperNodeInfo& nr = (*it).second; + + if (nr.state == LifeCycleState::ACTIVE || nr.state == LifeCycleState::DEACTIVATING) + { + sendLifeCycleCommand(nr.name, LifeCycleCommand::DEACTIVATE); + } + else if (nr.state == LifeCycleState::UNCONFIGURED || nr.state == LifeCycleState::CONFIGURING) + { + sendLifeCycleCommand(nr.name, LifeCycleCommand::CONFIGURE); + } + + + } + + } } /** @@ -686,11 +811,21 @@ class AMSuper reportSystemState(); - sendLEDMessage(); + // sendLEDMessage(); + sendTractorLightMessage(); brain_box_msgs::msg::VxState state_msg; state_msg.state = (uint8_t)supervisor_.system_state; vstate_summary_pub_->publish(state_msg); + + if (state == SuperState::AUTO && supervisor_.start_fp_from_super_) + { + startFlightPlan(); + } + if (state == SuperState::READY && !first_time_booted_) + { + first_time_booted_ = true; + } } } @@ -829,6 +964,29 @@ class AMSuper sendLEDMessage(r, g, b, rate); } + // Subject to State + void sendTractorLightMessage() + { + // TODO: This should only be if we are on tractor, where the light is connected to the orin + // Otherwise we risk changing values on the pin unecessarily. + std_msgs::msg::Int32MultiArray msg; + + int32_t gpio_pin_number = 13; + int32_t light_value = 0; // 0 is off, 1 is on + + if (supervisor_.system_state == SuperState::AUTO) + { + light_value = 1; + } + else + { + light_value = 0; + } + + msg.data = {gpio_pin_number, light_value}; + set_gpio_pin_pub_->publish(msg); + } + /** * calculate babysitter timing params * @param hz expected frequency in hz @@ -984,6 +1142,49 @@ class AMSuperNode : public AMLifeCycle brain_box_msgs::msg::SystemState system_state_msg; system_state_msg.state = (uint8_t)am_super_->supervisor_.system_state; system_state_msg.state_string = am_super_->state_mediator_.stateToString(am_super_->supervisor_.system_state); + bool stuck_in_booting_too_long = (((am::Node::node->now() - am_super_->booting_start_time_) > am_super_->booting_allowance_) && (am_super_->supervisor_.errored_nodes_.size() > 0)); + if (am_super_->supervisor_.system_state == SuperState::BOOTING) + { + if (!am_super_->first_time_booted_) + { + if (am_super_->offline_restart_required_) + { + system_state_msg.state_string = "RESTART_OFFLINE"; + // system_state_msg.state_string = "RESTART"; + } + else if (stuck_in_booting_too_long) + { + system_state_msg.state_string = "RESTART"; + } + else + { + if (am_super_->flight_plan_running_) + { + system_state_msg.state_string = "RESET"; + } + else // flight plan is NOT running + { + system_state_msg.state_string = "BOOTING"; + } + } + } + + // Check if we need a reset or a restart, and overwrite accordingly. + // if (am_super_->offline_restart_required_ || am_super_->restart_required_) + else if (am_super_->restart_required_) + { + system_state_msg.state_string = "RESTART"; + } + else if (am_super_->offline_restart_required_) + { + system_state_msg.state_string = "RESTART_OFFLINE"; + // system_state_msg.state_string = "RESTART"; + } + else if (am_super_->reset_required_) + { + system_state_msg.state_string = "RESET"; + } + } am_super_->system_state_pub_->publish(system_state_msg); } @@ -1047,7 +1248,7 @@ class AMSuperNode : public AMLifeCycle } } } - LOG_MSG("/status/super", status_msg, 1); + LOG_MSG("/super/status", status_msg, 1); if (am_super_->super_status_pub_->get_subscription_count() > 0) { am_super_->super_status_pub_->publish(status_msg); @@ -1064,10 +1265,17 @@ class AMSuperNode : public AMLifeCycle ROS_ERROR_STREAM("not online: " << am_super_->node_mediator_.manifestedNodesNotOnlineNamesList(am_super_->supervisor_)); // if && supervisor_.system_state != SuperState::BOOTING - if (am_super_->supervisor_.system_state != SuperState::BOOTING) + // if (am_super_->supervisor_.system_state != SuperState::BOOTING) + if (true) // Seeing if we need this if statement at all. { + if (!am_super_->offline_timer_running_) + { + am_super_->offline_start_time_ = am::Node::node->now(); + am_super_->offline_timer_running_ = true; + } stats_->statStatus = 2; - am_super_->stopFlightPlan(); // This will keep sending until the all nodes are back, and that is ok for now + am_super_->node_mediator_.setOperatorCommand(am_super_->supervisor_, OperatorCommand::CANCEL); + am_super_->stopFlightPlan(); // This will keep sending until the all nodes are back, and that is ok for now } } @@ -1075,12 +1283,160 @@ class AMSuperNode : public AMLifeCycle { // if all manifested nodes are running, report as info ROS_INFO_STREAM_THROTTLE(am_super_->LOG_THROTTLE_S, ss.str()); + + // This should be in any state, if all nodes come back online + if (am_super_->offline_timer_running_) + { + am_super_->offline_timer_running_ = false; + } + if (am_super_->supervisor_.system_state == SuperState::BOOTING) { stats_->statStatus=0; + } } + // Call for an restart if you can't see a node up at this time. + if (am_super_->offline_timer_running_) + { + // Check if we need to restart things + rclcpp::Time time_now = am::Node::node->now(); + if ((time_now - am_super_->offline_start_time_) > am_super_->offline_allowance_) + { + ROS_WARN_STREAM("Offline timer is: " << (time_now - am_super_->offline_start_time_).seconds()); + // ROS_WARN_STREAM((time_now - am_super_->offline_start_time_).seconds()); + am_super_->offline_restart_required_ = true; + // This should already have been done when the offline first happened (about 30 lines above) + // am_super_->node_mediator_.setOperatorCommand(am_super_->supervisor_, OperatorCommand::CANCEL); + // am_super_->stopFlightPlan(); + } + else + { + am_super_->offline_restart_required_ = false; + } + } + else + { + am_super_->offline_restart_required_ = false; + } + + // std_msgs/Header header + // uint8 num_manifested_nodes + // uint8 num_errored_nodes + // string[] reset_nodes + // string[] restart_nodes + brain_box_msgs::msg::Super2ErrorNodes super_errored_msg; + super_errored_msg.num_manifested_nodes = am_super_->supervisor_.manifest.size(); + super_errored_msg.num_errored_nodes = am_super_->supervisor_.errored_nodes_.size(); + if (am_super_->supervisor_.manifest.size() != am_super_->node_mediator_.manifestedNodesOnlineCount(am_super_->supervisor_)) + { + super_errored_msg.offline_nodes.push_back(am_super_->node_mediator_.manifestedNodesNotOnlineNamesList(am_super_->supervisor_)); + } + + // Let's look at the errored_nodes_ and see if we need to call for a reset or a restart from there. + // Reset nodes: am_locator, am_pilot + // Restart nodes: anything else + if (am_super_->supervisor_.errored_nodes_.size()>0) + { + // If either of reset nodes are in there, you have to ask for a reset + // if(am_super_->supervisor_.errored_nodes_.find("am_locator") != am_super_->supervisor_.errored_nodes_.end() || + // am_super_->supervisor_.errored_nodes_.find("am_pilot") != am_super_->supervisor_.errored_nodes_.end()) + // { + // am_super_->reset_required_ = true; + // } + // // Remove the reset request if they are BOTH out of error, which is the negation of the above statement + + + /* Approach 2*/ + // if(am_super_->supervisor_.errored_nodes_.find("am_locator") != am_super_->supervisor_.errored_nodes_.end()) + // { + // am_super_->reset_required_ = true; + // } + // else if (am_super_->supervisor_.errored_nodes_.find("am_pilot") != am_super_->supervisor_.errored_nodes_.end()) + // { + // am_super_->reset_required_ = true; + // } + // else + // { + // am_super_->reset_required_ = false; + + // // But if we are here then there is something ELSE in the error condition, which means we need a restart + // am_super_->restart_required_ = true; + // } + + + + /* Approach 3 */ + am_super_->reset_required_ = false; + am_super_->restart_required_ = false; + for (auto node : am_super_->supervisor_.errored_nodes_) + { + // if (node.first == "am_locator" || node.first == "am_pilot") + if (node.first == "am_locator") + { + // if (!am_super_->flight_plan_running_ && ((am::Node::node->now() - am_super_->reset_start_time_) < am_super_->reset_allowance_)) + // { + // am_super_->reset_required_ = false; + // // super_errored_msg.reset_nodes.push_back(node.first); + // } + // else + // { + + // If it has been more than 5 seconds of trying, then remove the command to automatically try and launch the flight plan. + if (!am_super_->flight_plan_running_) + { + if ((am::Node::node->now() - am_super_->reset_start_time_) > am_super_->reset_allowance_) // We have exceeded our allowance + { + // Turn off the activate command + am_super_->node_mediator_.setOperatorCommand(am_super_->supervisor_, OperatorCommand::CANCEL); + am_super_->reset_required_ = true; + } + // else: you have 5 seconds to get it running + } + else + { + // So the flight plan IS running + am_super_->reset_required_ = true; + super_errored_msg.reset_nodes.push_back(node.first); + am_super_->node_mediator_.setOperatorCommand(am_super_->supervisor_, OperatorCommand::CANCEL); + am_super_->stopFlightPlan(); + } + + + + + // } + } + + else + { + am_super_->restart_required_ = true; + super_errored_msg.restart_nodes.push_back(node.first); + am_super_->node_mediator_.setOperatorCommand(am_super_->supervisor_, OperatorCommand::CANCEL); + am_super_->stopFlightPlan(); + } + } + + } + else + { + // If there are no errored nodes you are good to go + am_super_->reset_required_ = false; + am_super_->restart_required_ = false; + } + + LOG_MSG("/super/errored_nodes", super_errored_msg, 1); + if (am_super_->super_errored_pub_->get_subscription_count() > 0) + { + am_super_->super_errored_pub_->publish(super_errored_msg); + } + // rclcpp::Publisher::SharedPtr super_errored_pub_; + + + + + // log stats fstream newfile; newfile.open("/sys/bus/i2c/devices/7-0040/iio_device/in_power0_input",ios::in); //open a file to perform read operation using file object diff --git a/src/am_super/super_node_mediator.cpp b/src/am_super/super_node_mediator.cpp index c32aa5c..57f56af 100644 --- a/src/am_super/super_node_mediator.cpp +++ b/src/am_super/super_node_mediator.cpp @@ -234,7 +234,13 @@ SuperNodeMediator::StateTransition SuperNodeMediator::getStateTransition(const S ROS_ERROR_STREAM("GetStateTransition: Processing an operator command | " << printOperatorCommand(supervisor.last_op_command_received) << " vs " << printOperatorCommand(transition.operator_command)); return transition; } - } + } + // else if (transition.operator_command == OperatorCommand::LAUNCH && supervisor.start_fp_from_super_) + // { + // // Allows for the mission to be started automatically if the anove parameter is set to true. + // ROS_ERROR_STREAM("GetStateTransition: Allowing automatic launch command without operator when blue light is achieved."); + // return transition; + // } else if(transitionHasControllerState(transition)) { if(supervisor.last_controller_state_received == transition.controller_state) @@ -275,12 +281,13 @@ bool SuperNodeMediator::transitionIsValid(const StateTransition& transition) return transition.to_state != StateTransition::NO_SUPER_STATE; } -SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Supervisor supervisor) +SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Supervisor& supervisor) { // required default state is junk and should not be consulted since not ready TransitionInstructions transition_instructions; transition_instructions.ready_for_transition = false; transition_instructions.resend_life_cycle_command = false; + transition_instructions.error_transition = false; // Hardik: shortcircuit if we have am_super in error // if (supervisor.nodes.at(SUPER_NODE_NAME).status == LifeCycleStatus::ERROR) @@ -290,68 +297,104 @@ SuperNodeMediator::TransitionInstructions SuperNodeMediator::transitionReady(Sup // return transition_instructions; // } + // // Come out of error when looking for the next transition if nothing is in error + // if (supervisor.status_error && supervisor.errored_nodes_.size() == 0) + // { + // ROS_WARN_STREAM("Removing the status_error_ !!"); + // supervisor.status_error = false; + // } - bool are_in_error = false; - for (pair nodePair : supervisor.nodes) - { - SuperNodeInfo node = nodePair.second; - if (node.status == LifeCycleStatus::ERROR) - { - are_in_error = true; - transition_instructions.ready_for_transition = true; - transition_instructions.new_state = getErrorTransition().to_state; - transition_instructions.resend_life_cycle_command = true; - // if (supervisor.system_state == SuperState::READY) - // { - // transition_instructions.life_cycle_command = LifeCycleCommand::CLEANUP; - // } - // else if (supervisor.system_state == SuperState::AUTO) - // { - // transition_instructions.life_cycle_command = LifeCycleCommand::DEACTIVATE; - // } - // return transition_instructions; - break; - } - } - if (are_in_error) + // pair>> check_results = allManifestedNodesCheck(supervisor, transition.check); + // Simplified logic that shortcuts getting any other transition. + ROS_WARN_STREAM("For some reason status_error is true here? : " << supervisor.status_error); + if (supervisor.status_error) { - bool active_true = false; - bool inactive_true = false; - for (pair nodePair : supervisor.nodes) + if (supervisor.errored_nodes_.size()==0 && allManifestedNodesCheck(supervisor, getErrorTransition().check).first) { - SuperNodeInfo node = nodePair.second; - // If things are active, make sure they come down inactive - // If things are inactive, then make sure they come down to Unconfingured - - if (node.state == LifeCycleState::ACTIVE) - { - active_true = true; - } - if (node.state == LifeCycleState::INACTIVE) - { - inactive_true = true; - } - - } - - if (active_true) - { - transition_instructions.life_cycle_command = LifeCycleCommand::DEACTIVATE; - } - else if (inactive_true) - { - transition_instructions.life_cycle_command = LifeCycleCommand::CLEANUP; + // TransitionInstructions pseudo_transition; + // pseudo_transition.ready_for_transition = false; + // pesudo_transition.resend_life_cycle_command = false; + // pseudo_transition.error_transition = false; + // pseudo_transition.new_state = getErrorTransition().to_state; + // if () + ROS_WARN_STREAM("Removing the status_error_ !!"); + supervisor.status_error = false; } else { - // This technically should not happen....? - transition_instructions.life_cycle_command = LifeCycleCommand::CONFIGURE; + // assert(supervisor.errored_nodes_.size() > 0); + transition_instructions.ready_for_transition = true; + transition_instructions.new_state = getErrorTransition().to_state; + transition_instructions.error_transition = true; + return transition_instructions; } - return transition_instructions; - } + + + // // bool are_in_error = false; + // for (pair nodePair : supervisor.nodes) + // { + // SuperNodeInfo node = nodePair.second; + // if (node.status == LifeCycleStatus::ERROR) + // { + // transition_instructions.ready_for_transition = true; + // // are_in_error = true; + // supervisor.status_error = true; + // transition_instructions.new_state = getErrorTransition().to_state; + // transition_instructions.resend_life_cycle_command = true; + // // if (supervisor.system_state == SuperState::READY) + // // { + // // transition_instructions.life_cycle_command = LifeCycleCommand::CLEANUP; + // // } + // // else if (supervisor.system_state == SuperState::AUTO) + // // { + // // transition_instructions.life_cycle_command = LifeCycleCommand::DEACTIVATE; + // // } + // // return transition_instructions; + // break; + // } + // } + + // if (are_in_error) + // { + // bool active_true = false; + // bool inactive_true = false; + + // for (pair nodePair : supervisor.nodes) + // { + // SuperNodeInfo node = nodePair.second; + // // If things are active, make sure they come down inactive + // // If things are inactive, then make sure they come down to Unconfingured + + // if (node.state == LifeCycleState::ACTIVE) + // { + // active_true = true; + // } + // if (node.state == LifeCycleState::INACTIVE) + // { + // inactive_true = true; + // } + + // } + + // if (active_true) + // { + // transition_instructions.life_cycle_command = LifeCycleCommand::DEACTIVATE; + // } + // else if (inactive_true) + // { + // transition_instructions.life_cycle_command = LifeCycleCommand::CLEANUP; + // } + // else + // { + // // This technically should not happen....? + // transition_instructions.life_cycle_command = LifeCycleCommand::CONFIGURE; + // } + // return transition_instructions; + + // } @@ -443,7 +486,8 @@ bool SuperNodeMediator::checkNodesShuttingDownOrFinalized(SuperNodeMediator::Sup bool SuperNodeMediator::checkErrorTransition(SuperNodeMediator::SuperNodeInfo& nr, SuperNodeMediator& node_mediator) { - return nr.state != LifeCycleState::ACTIVE; + return nr.state == LifeCycleState::INACTIVE; + // return nr.state != LifeCycleState::ACTIVE; // return nr.state == LifeCycleState::UNCONFIGURED | nr.state == LifeCycleState::INACTIVE; } @@ -490,6 +534,20 @@ bool SuperNodeMediator::checkSuperError(SuperNodeMediator::SuperNodeInfo& nr, Su } + +// bool SuperNodeMediator::allManifestedBackToInactiveCheck(Supervisor& supervisor) +// { +// for (auto nodePair : supervisor.nodes) +// { +// SuperNodeInfo node = nodePair.second; + +// if (node.manifested) +// { + +// } +// } +// } + // This does the check function on all manifested nodes. pair>> SuperNodeMediator::allManifestedNodesCheck( Supervisor& supervisor, std::function check) @@ -526,6 +584,7 @@ pair>> SuperNodeMediator::allManifestedNode ROS_WARN_STREAM("hardik-allManifestedNodesCheck: 1.3"); string_view node_state = life_cycle_mediator.stateToString(node.state); error_message = "[2OQ0] check failed: node in wrong state " + node.name + ": " + string(node_state); + ROS_WARN_STREAM(error_message); success = false; need_lifecycle_resend = true; }