Skip to content

Commit

Permalink
Fix/driver status drops (#2248)
Browse files Browse the repository at this point in the history 
* add carla supportive

* add other drivers

* comment

* remove unnecessary code

* fix

* change from wall_timer to simulation timer

* fix

* fixes

* changes

* remove unnecessary changes

* clean

* increase size

* Update drivers_controller_node.cpp
  • Loading branch information
@MishkaMN
MishkaMN authored Dec 27, 2023
1 parent 2c4f98a commit 50f51fb
Show file tree
Hide file tree
Showing 3 changed files with 46 additions and 36 deletions.
20 changes: 10 additions & 10 deletions subsystem_controllers/src/base_subsystem_controller/base_subsystem_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ using std_msec = std::chrono::milliseconds;
namespace subsystem_controllers
{
BaseSubsystemController::BaseSubsystemController(const rclcpp::NodeOptions &options)
: CarmaLifecycleNode(options),
: CarmaLifecycleNode(options),
lifecycle_mgr_(get_node_base_interface(), get_node_graph_interface(), get_node_logging_interface(), get_node_services_interface())
{
// Declare parameters
Expand All @@ -35,7 +35,7 @@ namespace subsystem_controllers
base_config_.subsystem_namespace = this->declare_parameter<std::string>("subsystem_namespace", base_config_.subsystem_namespace);
base_config_.full_subsystem_required = this->declare_parameter<bool>("full_subsystem_required", base_config_.full_subsystem_required);
base_config_.unmanaged_required_nodes = this->declare_parameter<std::vector<std::string>>("unmanaged_required_nodes", base_config_.unmanaged_required_nodes);

// Handle fact that parameter vectors cannot be empty
if (base_config_.required_subsystem_nodes.size() == 1 && base_config_.required_subsystem_nodes[0].empty()) {
base_config_.required_subsystem_nodes.clear();
Expand Down Expand Up @@ -78,7 +78,7 @@ namespace subsystem_controllers
alert.source_node = get_node_base_interface()->get_fully_qualified_name();
publish_system_alert(alert);

// TODO: It might be worth trying to deactivate or shutdown after alerting the larger system,
// TODO: It might be worth trying to deactivate or shutdown after alerting the larger system,
// but not clear on if that will increase instability of shutdown process
}
else
Expand All @@ -93,7 +93,7 @@ namespace subsystem_controllers
{
RCLCPP_INFO_STREAM(get_logger(), "Subsystem trying to configure");

// Reset config
// Reset config
base_config_ = BaseSubSystemControllerConfig();

// Load Parameters
Expand Down Expand Up @@ -141,7 +141,7 @@ namespace subsystem_controllers

RCLCPP_INFO_STREAM(get_logger(), "New config: " << base_config_);
}


if (!trigger_managed_nodes_configure_from_base_class_) {
return CallbackReturn::SUCCESS;
Expand Down Expand Up @@ -279,7 +279,7 @@ namespace subsystem_controllers
std::vector<std::string> BaseSubsystemController::get_nodes_in_namespace(const std::string &node_namespace) const
{

// These two services are exposed by lifecycle nodes.
// These two services are exposed by lifecycle nodes.
static const std::string CHANGE_STATE_SRV = "/change_state";
static const std::string GET_STATE_SRV = "/get_state";

Expand Down Expand Up @@ -307,8 +307,8 @@ namespace subsystem_controllers

////
// In the following section of code we check if the current node in the target namespace is actually a lifecycle node
// This check is done by evaluating if that nodes exposes the change_state and get_state lifecycle services.
// If the node does not expose these services then it cannot be managed by this component.
// This check is done by evaluating if that nodes exposes the change_state and get_state lifecycle services.
// If the node does not expose these services then it cannot be managed by this component.
// However, this does not result in an error as the node could be wrapped by a lifecycle component wrapper
////

Expand All @@ -332,7 +332,7 @@ namespace subsystem_controllers
const std::string cs_srv = node + CHANGE_STATE_SRV;
const std::string gs_srv = node + GET_STATE_SRV;

if (services_and_types.find(cs_srv) != services_and_types.end()
if (services_and_types.find(cs_srv) != services_and_types.end()
&& services_and_types.find(gs_srv) != services_and_types.end()
&& std::find(services_and_types.at(cs_srv).begin(), services_and_types.at(cs_srv).end(), CHANGE_STATE_TYPE) != services_and_types.at(cs_srv).end()
&& std::find(services_and_types.at(gs_srv).begin(), services_and_types.at(gs_srv).end(), GET_STATE_TYPE) != services_and_types.at(gs_srv).end())
Expand All @@ -343,7 +343,7 @@ namespace subsystem_controllers
} else {
// Current node is not a lifecycle node so log a warning
RCLCPP_WARN_STREAM(get_logger(), "Failed to find lifecycle services for node: " << node << " this node will not be managed. NOTE: If this node is wrapped by a lifecycle component that is managed then this is not an issue.");

continue;

}
Expand Down
22 changes: 13 additions & 9 deletions subsystem_controllers/src/drivers_controller/driver_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,12 +34,12 @@ namespace subsystem_controllers
: critical_drivers_(critical_driver_names.begin(), critical_driver_names.end()),
camera_entries_(camera_entries.begin(), camera_entries.end()),
driver_timeout_(driver_timeout)
{
{
// Intialize entry manager
em_ = std::make_shared<EntryManager>(critical_driver_names, camera_entries);

}


carma_msgs::msg::SystemAlert DriverManager::handle_spin(long time_now,long start_up_timestamp,long startup_duration)
{
Expand All @@ -52,7 +52,7 @@ namespace subsystem_controllers
alert.description = "All essential drivers are ready";
alert.type = carma_msgs::msg::SystemAlert::DRIVERS_READY;
return alert;
}
}
else if(starting_up_ && (time_now - start_up_timestamp <= startup_duration))
{
alert.description = "System is starting up...";
Expand All @@ -75,9 +75,9 @@ namespace subsystem_controllers
{
alert.description = "Unknown problem assessing essential driver availability";
alert.type = carma_msgs::msg::SystemAlert::FATAL;
return alert;
return alert;
}

}


Expand All @@ -87,13 +87,13 @@ namespace subsystem_controllers
Entry driver_status(msg->status == carma_driver_msgs::msg::DriverStatus::OPERATIONAL || msg->status == carma_driver_msgs::msg::DriverStatus::DEGRADED,
msg->name,current_time);

em_->update_entry(driver_status);
em_->update_entry(driver_status);
}


void DriverManager::evaluate_sensor(int &sensor_input,bool available,long current_time,long timestamp,long driver_timeout)
{

if((!available) || (current_time-timestamp > driver_timeout))
{
sensor_input=0;
Expand All @@ -102,7 +102,7 @@ namespace subsystem_controllers
{
sensor_input=1;
}

}

std::string DriverManager::are_critical_drivers_operational(long current_time)
Expand All @@ -113,6 +113,10 @@ namespace subsystem_controllers
std::vector<Entry> driver_list = em_->get_entries(); //Real time driver list from driver status
for(auto i = driver_list.begin(); i < driver_list.end(); ++i)
{
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("subsystem_controller"), "i->name_: " << i->name_
<< ", current_time: " << current_time
<< ", i->timestamp_: " << i->timestamp_
<< ", difference: " << current_time-(i->timestamp_) );
if(em_->is_entry_required(i->name_))
{
evaluate_sensor(ssc,i->available_,current_time,i->timestamp_,driver_timeout_);
Expand All @@ -130,7 +134,7 @@ namespace subsystem_controllers
}

/////////////////////
//Decision making
//Decision making
if (ssc == 1 && camera == 1)
{
return "s_1_c_1";
Expand Down
40 changes: 23 additions & 17 deletions subsystem_controllers/src/drivers_controller/drivers_controller_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,9 +28,9 @@ namespace subsystem_controllers

config_.startup_duration_ = declare_parameter<int>("startup_duration", config_.startup_duration_);
config_.driver_timeout_ = declare_parameter<double>("required_driver_timeout", config_.driver_timeout_);

// carma-config parameters
config_.ros1_required_drivers_ = declare_parameter<std::vector<std::string>>("ros1_required_drivers", config_.ros1_required_drivers_);
config_.ros1_required_drivers_ = declare_parameter<std::vector<std::string>>("ros1_required_drivers", config_.ros1_required_drivers_);
config_.ros1_camera_drivers_ = declare_parameter<std::vector<std::string>>("ros1_camera_drivers", config_.ros1_camera_drivers_);
config_.excluded_namespace_nodes_ = declare_parameter<std::vector<std::string>>("excluded_namespace_nodes", config_.excluded_namespace_nodes_);

Expand All @@ -48,8 +48,8 @@ namespace subsystem_controllers
config_ = DriversControllerConfig();

// Load required plugins and default enabled plugins
get_parameter<std::vector<std::string>>("ros1_required_drivers", config_.ros1_required_drivers_);
get_parameter<std::vector<std::string>>("ros1_camera_drivers", config_.ros1_camera_drivers_);
get_parameter<std::vector<std::string>>("ros1_required_drivers", config_.ros1_required_drivers_);
get_parameter<std::vector<std::string>>("ros1_camera_drivers", config_.ros1_camera_drivers_);
get_parameter<int>("startup_duration", config_.startup_duration_);
get_parameter<double>("required_driver_timeout", config_.driver_timeout_);
get_parameter<std::vector<std::string>>("excluded_namespace_nodes", config_.excluded_namespace_nodes_);
Expand All @@ -66,7 +66,7 @@ namespace subsystem_controllers
if (config_.excluded_namespace_nodes_.size() == 1 && config_.excluded_namespace_nodes_[0].empty()) {
config_.excluded_namespace_nodes_.clear();
}

auto base_managed_nodes = lifecycle_mgr_.get_managed_nodes();
// Update managed nodes
// Collect namespace nodes not managed by other subsystem controllers - manually specified in carma-config
Expand All @@ -75,21 +75,21 @@ namespace subsystem_controllers
lifecycle_mgr_.set_managed_nodes(updated_managed_nodes);

driver_manager_ = std::make_shared<DriverManager>(
config_.ros1_required_drivers_,
config_.ros1_camera_drivers_,
config_.ros1_required_drivers_,
config_.ros1_camera_drivers_,
config_.driver_timeout_
);

// record starup time
start_up_timestamp_ = this->now().nanoseconds() / 1e6;

driver_status_sub_ = create_subscription<carma_driver_msgs::msg::DriverStatus>("driver_discovery", 1, std::bind(&DriversControllerNode::driver_discovery_cb, this, std::placeholders::_1));
driver_status_sub_ = create_subscription<carma_driver_msgs::msg::DriverStatus>("driver_discovery", 20, std::bind(&DriversControllerNode::driver_discovery_cb, this, std::placeholders::_1));

timer_ = create_timer(get_clock(), std::chrono::milliseconds(1000), std::bind(&DriversControllerNode::timer_callback,this));

// Configure our drivers
bool success = lifecycle_mgr_.configure(std::chrono::milliseconds(base_config_.service_timeout_ms), std::chrono::milliseconds(base_config_.call_timeout_ms)).empty();

if (success)
{
RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to configure");
Expand All @@ -106,21 +106,27 @@ namespace subsystem_controllers
carma_ros2_utils::CallbackReturn DriversControllerNode::handle_on_activate(const rclcpp_lifecycle::State &prev_state)
{

// Reset to automatically trigger state transitions from base class
// Reset to automatically trigger state transitions from base class
trigger_managed_nodes_configure_from_base_class_ = true;

// return only either SUCCESS or FAILURE
auto base_return = BaseSubsystemController::handle_on_activate(prev_state); // This will activate all base_managed_nodes

if (base_return != carma_ros2_utils::CallbackReturn::SUCCESS) {
RCLCPP_ERROR(get_logger(), "Driver Controller could not activate");
return base_return;

if (base_return == CallbackReturn::SUCCESS) {
RCLCPP_INFO(get_logger(), "Driver Controller activated!");
}
else if (base_return == CallbackReturn::FAILURE)
{
RCLCPP_ERROR(get_logger(), "Driver Controller encountered FAILURE when trying to activate the subsystems... please check which driver failed to activate...");
}

return base_return;

}

void DriversControllerNode::timer_callback()
{

long time_now = this->now().nanoseconds() / 1e6;
rclcpp::Duration sd(config_.startup_duration_, 0);
long start_duration = sd.nanoseconds()/1e6;
Expand All @@ -132,7 +138,7 @@ namespace subsystem_controllers
if (!prev_alert) {
prev_alert = dm;
publish_system_alert(dm);
}
}
else if ( prev_alert->type == dm.type && prev_alert->description.compare(dm.description) == 0) { // Do not publish duplicate alerts
RCLCPP_DEBUG_STREAM(get_logger(), "No change to alert status");
}
Expand Down

0 comments on commit 50f51fb

Please sign in to comment.