Make the update loop in ros_control to wait until controller "A" or "B" has obtained a goal

edit

I have written a hardware interface with ros_control for an already implemented controller. With the simulation that is connected to this interface, when I launch the controller manager and other nodes, the robot falls on the ground since my controller "A" (a gravitycompensated...) is either not enabled or has not yet received a goal because of which the command_efforts are near zero.

But the moment I proceed to a flexbe-state where controller "A" receives a goal, the correct joint effort commands become available and the robot gets up from the ground.

Is there a way that I make the update look in the hardware interface to wait until controller "A" has received a goal, so that it doesn't continue with the loop and send zero efforts to the robot? or any other way, perhaps with the controller manager and etc.

My controller interface in the cpp file looks like the following.

#include <muj_hw.h>
#include <iostream>
#include <string>
#include <fstream>

ROBOTHardwareInterface::ROBOTHardwareInterface(ros::NodeHandle& nh) : nh_(nh) {
    init();
    controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
    loop_hz_=1000;
    //ac("muj_simread_joint_state", true);

    ros::Duration update_freq = ros::Duration(1.0/loop_hz_);


    read_client  = nh_.serviceClient<amira_mujoco_py_sim::JointsState>("read_joint_state");
    write_client = nh_.serviceClient<amira_mujoco_py_sim::JointsState>("write_joint_state");    
    non_realtime_loop_ = nh_.createTimer(update_freq, &ROBOTHardwareInterface::update, this);

}

ROBOTHardwareInterface::~ROBOTHardwareInterface() {
}

void ROBOTHardwareInterface::init() {

    //ac(server_name, spinn);
    num_joints_=7;
    joint_names_[0]="joint1";   
    joint_names_[1]="joint2";
    joint_names_[2]="joint3";
    joint_names_[3]="joint4";   
    joint_names_[4]="joint5";
    joint_names_[5]="joint6";
    joint_names_[6]="joint7";


    for (int i = 0; i < num_joints_; ++i) {
        // Create joint state interface
        hardware_interface::JointStateHandle    jointStateHandle (joint_names_    [i],
                                                                 &joint_position_ [i],
                                                                 &joint_velocity_ [i],
                                                                 &joint_effort_   [i]);
        // Create position joint interface
        hardware_interface::JointHandle    jointPositionHandle (jointStateHandle, &joint_position_command_[i]);
        hardware_interface::JointHandle    jointVelocityHandle (jointStateHandle, &joint_velocity_command_[i]);
        hardware_interface::JointHandle    jointEffortHandle   (jointStateHandle, &joint_effort_command_  [i]);

        joint_state_interface_.registerHandle(jointStateHandle);
        position_joint_interface_.registerHandle(jointPositionHandle);
        velocity_joint_interface_.registerHandle(jointVelocityHandle);
        effort_joint_interface_.registerHandle(jointEffortHandle);
      }

    // Registering all joints interfaces    
    registerInterface(&joint_state_interface_);
    registerInterface(&position_joint_interface_);
    registerInterface(&velocity_joint_interface_);
    registerInterface(&effort_joint_interface_);
}

void ROBOTHardwareInterface::update(const ros::TimerEvent& e) {
    elapsed_time_ = ros::Duration(e.current_real - e.last_real);
    read();
    controller_manager_->update(ros::Time::now(), elapsed_time_);
    write(elapsed_time_);
}

void ROBOTHardwareInterface::read() {   
    read_client.waitForExistence();
    bool success = read_client.call(j_state_srv_read);
    for (int i = 0; i < num_joints_; ++i) {
        joint_position_[i] = j_state_srv_read.response.j_state_res.position[i];
        joint_velocity_[i] = j_state_srv_read.response.j_state_res.velocity[i];
        joint_effort_  [i] = j_state_srv_read.response.j_state_res.effort  [i];
    }

                                  }

void ROBOTHardwareInterface::write(ros::Duration elapsed_time) {

    j_state_srv_write.request.j_state_req.effort  = {(float) joint_effort_command_[0],
                                                     (float) joint_effort_command_[1],
                                                     (float) joint_effort_command_[2],
                                                     (float) joint_effort_command_[3],
                                                     (float) joint_effort_command_[4],
                                                     (float) joint_effort_command_[5],
                                                     (float) joint_effort_command_[6]
                                                            };
    write_client.waitForExistence();
    bool success = write_client.call(j_state_srv_write);

                                                               }



int main(int argc, char** argv) 
{
    ros::init(argc, argv, "mujoco_py_hardware_interface");
    ros::NodeHandle nh;
    ROBOTHardwareInterface ROBOT(nh);
    ros::AsyncSpinner spinner(4);
    spinner.start();
    ros::waitForShutdown();
    return 0;
}