diff --git a/combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h b/combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h index e53ce02af..8e52a79d6 100644 --- a/combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h +++ b/combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h @@ -100,11 +100,22 @@ class CombinedRobotHW : public hardware_interface::RobotHW */ virtual void write(const ros::Time& time, const ros::Duration& period); + /** + * Stops the robot HW + */ + virtual bool stop(); + + /** + * Recovers/Reinitializes the robot HW + */ + virtual bool recover(); + protected: ros::NodeHandle root_nh_; ros::NodeHandle robot_hw_nh_; pluginlib::ClassLoader robot_hw_loader_; std::vector robot_hw_list_; + std::vector robot_hw_names; virtual bool loadRobotHW(const std::string& name); diff --git a/combined_robot_hw/src/combined_robot_hw.cpp b/combined_robot_hw/src/combined_robot_hw.cpp index 7c734ee15..90fd576e2 100644 --- a/combined_robot_hw/src/combined_robot_hw.cpp +++ b/combined_robot_hw/src/combined_robot_hw.cpp @@ -39,16 +39,15 @@ namespace combined_robot_hw root_nh_ = root_nh; robot_hw_nh_ = robot_hw_nh; - std::vector robots; std::string param_name = "robot_hardware"; - if (!robot_hw_nh.getParam(param_name, robots)) + if (!robot_hw_nh.getParam(param_name, robot_hw_names)) { ROS_ERROR_STREAM("Could not find '" << param_name << "' parameter (namespace: " << robot_hw_nh.getNamespace() << ")."); return false; } std::vector::iterator it; - for (it = robots.begin(); it != robots.end(); it++) + for (it = robot_hw_names.begin(); it != robot_hw_names.end(); it++) { if (!loadRobotHW(*it)) { @@ -205,6 +204,40 @@ namespace combined_robot_hw } } + bool CombinedRobotHW::stop() + { + int i = 0; + bool stop_success = true; + std::vector::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + if ((*robot_hw)->stop() == false) + { + ROS_ERROR("Stopping robot HW '%s' failed", robot_hw_names[i].c_str()); + stop_success = false; + } + i++; + } + return stop_success; + } + + bool CombinedRobotHW::recover() + { + int i = 0; + bool recover_success = true; + std::vector::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + if ((*robot_hw)->recover() == false) + { + ROS_ERROR("Recovering/Reinitializing robot HW '%s' failed", robot_hw_names[i].c_str()); + recover_success = false; + } + i++; + } + return recover_success; + } + void CombinedRobotHW::filterControllerList(const std::list& list, std::list& filtered_list, hardware_interface::RobotHWSharedPtr robot_hw) diff --git a/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h index fc3a19f9f..c2c47e0ce 100644 --- a/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h +++ b/combined_robot_hw_tests/include/combined_robot_hw_tests/my_robot_hw_1.h @@ -43,8 +43,11 @@ class MyRobotHW1 : public hardware_interface::RobotHW MyRobotHW1(); virtual ~MyRobotHW1(){}; virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh); + void setInitValues(); virtual void read(const ros::Time& time, const ros::Duration& period); virtual void write(const ros::Time& time, const ros::Duration& period); + virtual bool recover(); + virtual bool stop(); virtual bool prepareSwitch(const std::list& start_list, const std::list& stop_list); virtual void doSwitch(const std::list& start_list, diff --git a/combined_robot_hw_tests/src/my_robot_hw_1.cpp b/combined_robot_hw_tests/src/my_robot_hw_1.cpp index 5b4dd262e..867a6bdad 100644 --- a/combined_robot_hw_tests/src/my_robot_hw_1.cpp +++ b/combined_robot_hw_tests/src/my_robot_hw_1.cpp @@ -49,25 +49,10 @@ bool MyRobotHW1::init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) joint_name_.resize(3); joint_name_[0] = "test_joint1"; - joint_position_[0] = 1.0; - joint_velocity_[0] = 0.0; - joint_effort_[0] = 0.1; - joint_effort_command_[0] = 3.0; - joint_velocity_command_[0] = 0.0; - joint_name_[1] = "test_joint2"; - joint_position_[1] = 1.0; - joint_velocity_[1] = 0.0; - joint_effort_[1] = 0.1; - joint_effort_command_[1] = 0.0; - joint_velocity_command_[1] = 0.0; - joint_name_[2] = "test_joint3"; - joint_position_[2] = 1.0; - joint_velocity_[2] = 0.0; - joint_effort_[2] = 0.1; - joint_effort_command_[2] = 0.0; - joint_velocity_command_[2] = 0.0; + + setInitValues(); // Populate hardware interfaces js_interface_.registerHandle(JointStateHandle(joint_name_[0], &joint_position_[0], &joint_velocity_[0], &joint_effort_[0])); @@ -101,6 +86,47 @@ void MyRobotHW1::write(const ros::Time& time, const ros::Duration& period) joint_effort_command_[1] = joint_effort_command_[0]; } +void MyRobotHW1::setInitValues() +{ + joint_position_[0] = 1.0; + joint_velocity_[0] = 0.0; + joint_effort_[0] = 0.1; + joint_effort_command_[0] = 3.0; + joint_velocity_command_[0] = 0.0; + + joint_position_[1] = 1.0; + joint_velocity_[1] = 0.0; + joint_effort_[1] = 0.1; + joint_effort_command_[1] = 0.0; + joint_velocity_command_[1] = 0.0; + + joint_position_[2] = 1.0; + joint_velocity_[2] = 0.0; + joint_effort_[2] = 0.1; + joint_effort_command_[2] = 0.0; + joint_velocity_command_[2] = 0.0; +} + +bool MyRobotHW1::recover() +{ + if (joint_effort_command_[0] == 10.0) + { + return false; + } + setInitValues(); + return true; +} + +bool MyRobotHW1::stop() +{ + if (joint_effort_command_[0] == 10.0) + { + return false; + } + joint_effort_command_[0] = 0.0; + return true; +} + bool MyRobotHW1::prepareSwitch(const std::list& start_list, const std::list& stop_list) { diff --git a/combined_robot_hw_tests/test/combined_robot_hw_test.cpp b/combined_robot_hw_tests/test/combined_robot_hw_test.cpp index 21e02e963..d246b3916 100644 --- a/combined_robot_hw_tests/test/combined_robot_hw_test.cpp +++ b/combined_robot_hw_tests/test/combined_robot_hw_test.cpp @@ -94,6 +94,28 @@ TEST(CombinedRobotHWTests, combinationOk) robot_hw.write(ros::Time::now(), period); ej_handle = ej_interface->getHandle("test_joint2"); ASSERT_FLOAT_EQ(3.5, ej_handle.getCommand()); + + // Test recover function + ej_handle = ej_interface->getHandle("test_joint1"); + ej_handle.setCommand(3.5); + bool recover_success = robot_hw.recover(); + ASSERT_TRUE(recover_success); + ASSERT_FLOAT_EQ(3.0, ej_handle.getCommand()); + ej_handle.setCommand(10.0); + recover_success = robot_hw.recover(); + ASSERT_FALSE(recover_success); + ASSERT_FLOAT_EQ(10.0, ej_handle.getCommand()); + + // Test stop function + ej_handle = ej_interface->getHandle("test_joint1"); + ej_handle.setCommand(3.5); + bool stop_success = robot_hw.stop(); + ASSERT_TRUE(stop_success); + ASSERT_FLOAT_EQ(0.0, ej_handle.getCommand()); + ej_handle.setCommand(10.0); + stop_success = robot_hw.stop(); + ASSERT_FALSE(stop_success); + ASSERT_FLOAT_EQ(10.0, ej_handle.getCommand()); } TEST(CombinedRobotHWTests, switchOk) diff --git a/hardware_interface/include/hardware_interface/robot_hw.h b/hardware_interface/include/hardware_interface/robot_hw.h index 9dcaf215c..294e7a2ee 100644 --- a/hardware_interface/include/hardware_interface/robot_hw.h +++ b/hardware_interface/include/hardware_interface/robot_hw.h @@ -78,6 +78,21 @@ class RobotHW : public InterfaceManager * \returns True if initialization was successful */ virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) {return true;} + + /** \brief The stop function is called to stop the RobotHW from a + * non-realtime thread. Usually on controller manager shutdown. + * + * \returns True if stopping was successful + */ + virtual bool stop() {return true;} + + /** \brief The recover function is called to recover/reinitialize the RobotHW after and + * error state (e.g. emergency stop, safety stop, hardware limits) from a non-realtime + * thread. + * + * \returns True if recovering was successful + */ + virtual bool recover() {return true;} /** \name Resource Management *\{*/