Added recover and stop in RobotHW

[destogl]

Proposal for extension of RobotHW with recover and stop functions for easier hardware management. See code comments for explanation of this functions. Hope this can be also useful for many other robots, since we are using it with Schunk modules/robots and rob@work mobile robot.

[mikepurvis]

I'm in favour of this— particularly when using combined_robot_hw, it would be nice to have a way to notify all RobotHW plugins to stop or start up again based on some external system state.

Should there be something added to ControllerManager to actually call the stop function on shutdown, since the comment hints that that will occur?

[bmagyar]

Could you please add a test that could double as a reference implementation for our documentation as well please?

Also, if the rob@work drivers are public that'd be a nice addition :)

[mathias-luedtke]

I am not sure about this..
In which case should these functions get called and by which part of the chain?

I can see @mikepurvis' point with combined_robot_hw, but this PR does not address this fully.

IMHO we need interfaces in the other direction, to notify that some joints are not active anymore.

Also, if the rob@work drivers are public that'd be a nice addition :)

rob@work is the industrial version of Care-O-bot, almost all of the (original) drivers are public.

[destogl]

@mikepurvis : Regarding ControllerManager we have our own generic implementation of ControllerManager since I was annoyed to write for every hardware the same controller manager code.

For the example of the generic controller manager see under.

@bmagyar: Would something like this be suitable as test?

@ipa-mdl: one of the main reasons to implement this is actually better support for hardware drivers provided by your department (e.g. rob@work, schunk_modular_robotics). Without this on any error, or after Emergency-Stop you can only restart the component completely...

#include <ros/ros.h>
#include <std_srvs/Trigger.h>
#include <ros/callback_queue.h>
#include <hardware_interface/internal/demangle_symbol.h>
#include <hardware_interface/internal/interface_manager.h>
#include <controller_manager/controller_manager.h>
#include <hardware_interface/hardware_interface.h>
#include <pluginlib/class_loader.h>
#include <ros/console.h>
#include <ros/node_handle.h>
#include <typeinfo>


boost::shared_ptr<controller_manager::ControllerManager> cm;
boost::shared_ptr<hardware_interface::RobotHW> robot_hw;
boost::shared_ptr<ros::NodeHandle> handle;
bool isInitialized = false;

bool srvCallback_Init(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
  if (robot_hw) {
    
    robot_hw->init((*handle), (*handle));
    cm.reset(new controller_manager::ControllerManager(robot_hw.get(), (*handle)));
    isInitialized = true;
    res.success = true;
    return true;
  } 
  return false;
}

bool srvCallback_Stop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
  if (robot_hw) {
    return robot_hw->stop();
  }
  return false;
}

bool srvCallback_Recover(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
  if (robot_hw) {
    return robot_hw->recover();
  }
  return false;
}

int main(int argc, char** argv) 
{
    
  /// initialize ROS, specify name of node
    ros::init(argc, argv, "robot_hw");
    
    handle.reset(new ros::NodeHandle("~"));
    ros::CallbackQueue callbackQue;
    handle->setCallbackQueue(&callbackQue); 
    ros::AsyncSpinner spinner(0, &callbackQue);
    spinner.start();
    
    pluginlib::ClassLoader<hardware_interface::RobotHW> robot_hw_loader_("hardware_interface", "hardware_interface::RobotHW");
    
    std::string type;
    if (handle->hasParam("robot_hardware_type"))
    {
      handle->getParam("robot_hardware_type", type);
      ROS_INFO("param: %s", type.c_str());
      try
      {
	robot_hw = robot_hw_loader_.createInstance(type);
      }
      catch (const std::runtime_error &ex)
      {
        ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
	handle->shutdown();
	spinner.stop();
      }
    }
    else
    {
      ROS_ERROR("could not find the parameter robot_hardware_type");
      handle->shutdown();
      spinner.stop();
    }
    
    ros::ServiceServer srvServer_Init_ = handle->advertiseService("hw_driver/init", srvCallback_Init);
    ros::ServiceServer srvServer_Stop_ = handle->advertiseService("hw_driver/stop", srvCallback_Stop);
    ros::ServiceServer srvServer_Recover_ = handle->advertiseService("hw_driver/recover", srvCallback_Recover);
    
    
    ros::Time time;
    ros::Duration period(0.1);
    while (ros::ok()) { 
        if (isInitialized && cm) {
	    time = ros::Time::now();
            robot_hw->read(time, period);
            cm->update(time, period);
            robot_hw->write(time, period);
        }
        ros::spinOnce();
        period.sleep();
    }
    spinner.stop();
  return 0;
}

[mathias-luedtke]

Regarding ControllerManager we have our own generic implementation of ControllerManager since I was annoyed to write for every hardware the same controller manager code.

Your code uses the standard ControllerManager..

I have just noticed that RobotHW got read() and write() functions
IMHO these should have been added to a derived, but abstract ComposableRobotHW class.
Now we can combine any RobotHW, but cannot run it, because not all classes actually implement these functions.
Originally, RobotHW was meant to provide state/command access to controllers, but not to grant full, exclusive control over the hardware.

one of the main reasons to implement this is actually better support for hardware drivers provided by your department (e.g. rob@work, schunk_modular_robotics). Without this on any error, or after Emergency-Stop you can only restart the component completely...

Our department switched to ros_canopen, which already implements this (init, halt, recover, shutdown services) ;)
That's one of the reasons we proposed std_srvs/Trigger in the first place..

[destogl]

Your code uses the standard ControllerManager..

Yes. What I meant is that is reused on multiple hardware. Currently for most of the RobotHW implementation there is also CM implementation to start the node (it is not wrong, but not efficient for managing 5+ robots)

Originally, RobotHW was meant to provide state/command access to controllers, but not to grant full, exclusive control over the hardware.

OK, but how should be than responsible for hardware initialization and stopping? In current implementation for initialization is CM but not for stopping. IMHO we can put it in RobotHW interface...

Our department switched to ros_canopen, which already implements this (init, halt, recover, shutdown services) ;)
That's one of the reasons we proposed std_srvs/Trigger in the first place..

I know. I am also using your repositories on daily basis :) Actually this was also inspired with your CM implementation in canopen_motor_node. the problem came when we added other Schunk hardware (not canopen capable) in ros_control (pw70 and SDH). I can also share my code, but currently depends on our internal ros_control extension :)

[bochen87]

This would be really useful - I would strongly vote for this to be merged! I would also propose to add this functionality to the CombinedRobotHW class

[dogoepp]

I am very interested in having such methods in the hardware interface for a simple robot (RobotHW) or a composed one (CombinedRobotHW, which is lacking in this pull request).

In our case, we want to implement a wireless soft emergency stop for our battery-operated robots. This seems to be the best place to stop and restart the robots. On the restart, we would then have the controller manager's update method be called with the restart option enabled.

We had considered the option of switching controllers, but this seems to be an other possible point of failure in the system, which we would like to avoid.

@resibots

[destogl]

Hi all,

we updated our branch to be compatible with combined_robot_hw. Is this no OK?

[jlack1987]

This applies here as well: #357 (comment)