From 1fcfc3b5bc2cf94cea82fbfe9e5b0a7e9e2559d4 Mon Sep 17 00:00:00 2001 From: Denis Stogl Date: Thu, 30 Nov 2017 11:32:48 +0100 Subject: [PATCH 1/6] Added recover and stop in RobotHW --- .../include/hardware_interface/robot_hw.h | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/robot_hw.h b/hardware_interface/include/hardware_interface/robot_hw.h index 277c812c1..809098e4f 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 *\{*/ @@ -162,7 +177,7 @@ class RobotHW : public InterfaceManager * \param period The time passed since the last call to \ref write */ virtual void write(const ros::Time& time, const ros::Duration& period) {} -}; + }; } From 14f5823a906994b9e9ccd1516377a5aac5335017 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 5 Dec 2017 08:06:00 +0100 Subject: [PATCH 2/6] Update robot_hw.h --- hardware_interface/include/hardware_interface/robot_hw.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/robot_hw.h b/hardware_interface/include/hardware_interface/robot_hw.h index 809098e4f..e6a7667de 100644 --- a/hardware_interface/include/hardware_interface/robot_hw.h +++ b/hardware_interface/include/hardware_interface/robot_hw.h @@ -177,7 +177,7 @@ class RobotHW : public InterfaceManager * \param period The time passed since the last call to \ref write */ virtual void write(const ros::Time& time, const ros::Duration& period) {} - }; +}; } From 94c846f873765f3c8689db8616e1f86f4885ce3a Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Tue, 20 Nov 2018 18:04:56 +0100 Subject: [PATCH 3/6] recover and stop in combined_robot_hw --- .../combined_robot_hw/combined_robot_hw.h | 10 ++++++++++ combined_robot_hw/src/combined_robot_hw.cpp | 18 ++++++++++++++++++ 2 files changed, 28 insertions(+) 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..4ecd8ac6f 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,6 +100,16 @@ class CombinedRobotHW : public hardware_interface::RobotHW */ virtual void write(const ros::Time& time, const ros::Duration& period); + /** + * Stops the robot HW + */ + virtual void stop(); + + /** + * Recovers/Reinitializes the robot HW + */ + virtual void recover(); + protected: ros::NodeHandle root_nh_; ros::NodeHandle robot_hw_nh_; diff --git a/combined_robot_hw/src/combined_robot_hw.cpp b/combined_robot_hw/src/combined_robot_hw.cpp index 7c734ee15..797da47bc 100644 --- a/combined_robot_hw/src/combined_robot_hw.cpp +++ b/combined_robot_hw/src/combined_robot_hw.cpp @@ -205,6 +205,24 @@ namespace combined_robot_hw } } + void CombinedRobotHW::stop() + { + std::vector::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + (*robot_hw)->stop(); + } + } + + void CombinedRobotHW::recover() + { + std::vector::iterator robot_hw; + for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) + { + (*robot_hw)->recover(); + } + } + void CombinedRobotHW::filterControllerList(const std::list& list, std::list& filtered_list, hardware_interface::RobotHWSharedPtr robot_hw) From 6bde4845ad81c7d350035c0fb19f8d317d9c0965 Mon Sep 17 00:00:00 2001 From: Daniel Azanov Date: Wed, 5 Dec 2018 15:19:29 +0100 Subject: [PATCH 4/6] adjusted stop() and recover() and writed tests for them --- .../combined_robot_hw/combined_robot_hw.h | 5 +- combined_robot_hw/src/combined_robot_hw.cpp | 29 ++++++--- .../combined_robot_hw_tests/my_robot_hw_1.h | 3 + combined_robot_hw_tests/src/my_robot_hw_1.cpp | 60 +++++++++++++------ .../test/combined_robot_hw_test.cpp | 22 +++++++ 5 files changed, 93 insertions(+), 26 deletions(-) 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 4ecd8ac6f..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 @@ -103,18 +103,19 @@ class CombinedRobotHW : public hardware_interface::RobotHW /** * Stops the robot HW */ - virtual void stop(); + virtual bool stop(); /** * Recovers/Reinitializes the robot HW */ - virtual void recover(); + 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 797da47bc..33e40a040 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,22 +204,38 @@ namespace combined_robot_hw } } - void CombinedRobotHW::stop() + bool CombinedRobotHW::stop() { + bool stop_success = true; + int i = 0; std::vector::iterator robot_hw; for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) { - (*robot_hw)->stop(); + 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; } - void CombinedRobotHW::recover() + bool CombinedRobotHW::recover() { + int i = 0; + bool recover_success; std::vector::iterator robot_hw; for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) { - (*robot_hw)->recover(); + 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, 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) From d11b9b48fd9ee71fdfce879b5bcef6995508ace5 Mon Sep 17 00:00:00 2001 From: muritane <31107191+muritane@users.noreply.github.com> Date: Mon, 10 Dec 2018 14:02:26 +0100 Subject: [PATCH 5/6] initialize recover_success --- combined_robot_hw/src/combined_robot_hw.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/combined_robot_hw/src/combined_robot_hw.cpp b/combined_robot_hw/src/combined_robot_hw.cpp index 33e40a040..c88158300 100644 --- a/combined_robot_hw/src/combined_robot_hw.cpp +++ b/combined_robot_hw/src/combined_robot_hw.cpp @@ -224,7 +224,7 @@ namespace combined_robot_hw bool CombinedRobotHW::recover() { int i = 0; - bool recover_success; + bool recover_success = true; std::vector::iterator robot_hw; for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) { From e0abbb86240776c2084069d31ef5529d44ed10b4 Mon Sep 17 00:00:00 2001 From: muritane <31107191+muritane@users.noreply.github.com> Date: Mon, 10 Dec 2018 14:05:08 +0100 Subject: [PATCH 6/6] tab to spaces --- combined_robot_hw/src/combined_robot_hw.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/combined_robot_hw/src/combined_robot_hw.cpp b/combined_robot_hw/src/combined_robot_hw.cpp index c88158300..90fd576e2 100644 --- a/combined_robot_hw/src/combined_robot_hw.cpp +++ b/combined_robot_hw/src/combined_robot_hw.cpp @@ -206,8 +206,8 @@ namespace combined_robot_hw bool CombinedRobotHW::stop() { + int i = 0; bool stop_success = true; - int i = 0; std::vector::iterator robot_hw; for (robot_hw = robot_hw_list_.begin(); robot_hw != robot_hw_list_.end(); ++robot_hw) {