From 3a1a618be5672523a186254bd245279b5c59c0ac Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Tue, 1 Sep 2020 20:48:20 +0900 Subject: [PATCH 01/33] Add ROSControl at instance --- CMakeLists.txt | 1 + package.xml | 2 + robot_hardware_sim.xml | 11 +++ src/plugin/CMakeLists.txt | 3 + src/plugin/ROSControlItem.cpp | 146 ++++++++++++++++++++++++++++++++++ src/plugin/ROSControlItem.h | 83 +++++++++++++++++++ src/plugin/ROSPlugin.cpp | 2 + src/plugin/RobotHWSim.cpp | 68 ++++++++++++++++ src/plugin/RobotHWSim.h | 59 ++++++++++++++ 9 files changed, 375 insertions(+) create mode 100644 robot_hardware_sim.xml create mode 100644 src/plugin/ROSControlItem.cpp create mode 100644 src/plugin/ROSControlItem.h create mode 100644 src/plugin/RobotHWSim.cpp create mode 100644 src/plugin/RobotHWSim.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 2fd0e6e..d07aa59 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs image_transport choreonoid + pluginlib ) # find_package(Qt5 COMPONENTS Widgets REQUIRED) diff --git a/package.xml b/package.xml index f1bcd27..59f07dd 100644 --- a/package.xml +++ b/package.xml @@ -14,7 +14,9 @@ std_msgs sensor_msgs image_transport + pluginlib cmake + diff --git a/robot_hardware_sim.xml b/robot_hardware_sim.xml new file mode 100644 index 0000000..c4aeac1 --- /dev/null +++ b/robot_hardware_sim.xml @@ -0,0 +1,11 @@ + + + + + A default robot simulation interface which constructs joint handles from an SDF/URDF. + + + diff --git a/src/plugin/CMakeLists.txt b/src/plugin/CMakeLists.txt index c78c3f4..9030afa 100644 --- a/src/plugin/CMakeLists.txt +++ b/src/plugin/CMakeLists.txt @@ -6,6 +6,8 @@ choreonoid_add_plugin(${target} ROSPlugin.cpp WorldROSItem.cpp BodyROSItem.cpp + ROSControlItem.cpp + RobotHWSim.cpp deprecated/BodyPublisherItem.cpp ${mofiles} ) @@ -15,6 +17,7 @@ target_link_libraries(${target} ${std_msgs_LIBRARIES} ${sensor_msgs_LIBRARIES} ${image_transport_LIBRARIES} + ${catkin_LIBRARIES} ${CHOREONOID_BODY_PLUGIN_LIBRARIES} ) diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp new file mode 100644 index 0000000..1f94515 --- /dev/null +++ b/src/plugin/ROSControlItem.cpp @@ -0,0 +1,146 @@ +///////////////////////////////// +/// @author Ryodo Tanaka +///////////////////////////////// + +#include "ROSControlItem.h" +#include +#include +#include +#include +#include + +namespace cnoid +{ +void ROSControlItem::initializeClass(ExtensionManager* ext) +{ + ext->itemManager().registerClass("ROSControlItem"); + ext->itemManager().addCreationPanel(); +} + +ROSControlItem::ROSControlItem(void) +{ + io_ = nullptr; +} + +ROSControlItem::ROSControlItem(const ROSControlItem& org) : ControllerItem(org) +{ + io_ = nullptr; +} + +Item* ROSControlItem::doDuplicate(void) const +{ + return new ROSControlItem(*this); +} + +bool ROSControlItem::store(Archive& archive) +{ + archive.write("name space", namespace_); + archive.write("robot_descripton", robot_description_); + + return true; +} + +bool ROSControlItem::restore(const Archive& archive) +{ + if(!archive.read("name space", namespace_)) + archive.read("name space", namespace_); + + if(!archive.read("robot_description", robot_description_)) + archive.read("robot_escription", robot_description_); + + return true; +} + +void ROSControlItem::doPutProperties(PutPropertyFunction& putProperty) +{ + putProperty("Name space for the program.", + namespace_, changeProperty(namespace_)); + + putProperty("Robot description parameter name.", + robot_description_, changeProperty(robot_description_)); +} + +bool ROSControlItem::initialize(ControllerIO* io) +{ + using namespace std; + using namespace cnoid; + stringstream ss; + + // Check body // + if (!io->body()) { + ss.clear(); + ss << "ROSControlItem \"" << displayName() << "\" is invalid." << endl; + ss << "Because it is not assigned to a body." << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); + + return false; + } + + // Check that ROS has been initialized + if(!ros::isInitialized()) { + ss.clear(); + ss << "ROS master is not initialized." << endl; + MessageView::instance()->put(ss.str(), MessageView::Warning); + + return false; + } + + io_ = io; + body_ = io->body(); + tstep_ = io->worldTimeStep(); + time_ = io->currentTime(); + + return true; +} + +bool ROSControlItem::start(void) +{ + using namespace std; + using namespace cnoid; + stringstream ss; + + nh_ = ros::NodeHandle(namespace_); + + try { + // ros plugin loader // + rbt_hw_sim_loader_ = make_shared>("choreonoid_ros", "cnoid::RobotHWSim"); + + // rbt_hw_sim_ = to_std(rbt_hw_sim_loader_->createInstance("cnoid/RobotHWSim")); + // load cnoid::RobotHWSim // + if(!rbt_hw_sim_->initSim(nh_, robot_description_)) { + ss.clear(); + ss << "Could not initialize robot simulation interface" << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); + } + // register ros control manager // + manager_ = make_shared(rbt_hw_sim_.get(), nh_); + } + catch(pluginlib::LibraryLoadException &ex) { + ss.clear(); + ss << "Failed to create robot simulation interface loader : " << ex.what() << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); + } + + ss.clear(); + ss << "Loaded cnoid::ROSControlItem" << endl; + MessageView::instance()->put(ss.str(), MessageView::Normal); + + return true; +} + +void ROSControlItem::input(void) +{ +} +bool ROSControlItem::control(void) +{ +} + +void ROSControlItem::output(void) +{ +} + +void ROSControlItem::stop(void) +{ +} + +} // namespace cnoid diff --git a/src/plugin/ROSControlItem.h b/src/plugin/ROSControlItem.h new file mode 100644 index 0000000..4217b7d --- /dev/null +++ b/src/plugin/ROSControlItem.h @@ -0,0 +1,83 @@ +/////////////////////////////////////////////////////////// +/// @file ROSControlItem.h +/// @brief Choreonoid ControlItem Plugin for ros_control +/// @author Ryodo Tanaka +/////////////////////////////////////////////////////////// + +#ifndef CNOID_ROS_PLUGIN_ROS_CONTROL_ITEM_H +#define CNOID_ROS_PLUGIN_ROS_CONTROL_ITEM_H + +// Choreonoid // +#include +#include + +// ROS // +#include +#include "RobotHWSim.h" +#include +#include + +// STL // +#include +#include +#include +#include + +namespace cnoid +{ +class ROSControlItem : public ControllerItem +{ + public: + static void initializeClass(ExtensionManager* ext); + + ROSControlItem(void); + ROSControlItem(const ROSControlItem& org); + + virtual ~ROSControlItem(); + + virtual bool initialize(ControllerIO* io) override; + virtual bool start(void) override; + virtual void input(void) override; + virtual bool control(void) override; + virtual void output(void) override; + virtual void stop(void) override; + + virtual double timeStep() const override { return tstep_; }; + + protected: + virtual Item* doDuplicate(void) const; + virtual bool store(Archive& archive); + virtual bool restore(const Archive& archive); + void doPutProperties(PutPropertyFunction& putProperty); + + private: + ControllerIO* io_; + Body* body_; + double tstep_; + double time_; + + ros::NodeHandle nh_; + std::shared_ptr> rbt_hw_sim_loader_; + RobotHWSimPtr rbt_hw_sim_; + std::shared_ptr manager_; + + + std::string namespace_{""}; + std::string robot_description_{"robot_description"}; +}; + +typedef std::shared_ptr ROSControlPtr; + +template +boost::shared_ptr to_boost(const std::shared_ptr &p) { + return boost::shared_ptr(p.get(), [p](...) mutable { p.reset(); }); +} + +template +std::shared_ptr to_std(const boost::shared_ptr &p) { + return std::shared_ptr(p.get(), [p](...) mutable { p.reset(); }); +} + +} // namespace cnoid + +#endif // CNOID_ROS_PLUGIN_ROS_CONTROL_ITEM_H diff --git a/src/plugin/ROSPlugin.cpp b/src/plugin/ROSPlugin.cpp index 3f6f807..f1b8f73 100644 --- a/src/plugin/ROSPlugin.cpp +++ b/src/plugin/ROSPlugin.cpp @@ -1,5 +1,6 @@ #include "WorldROSItem.h" #include "BodyROSItem.h" +#include "ROSControlItem.h" #include "deprecated/BodyPublisherItem.h" #include #include @@ -28,6 +29,7 @@ class ROSPlugin : public Plugin WorldROSItem::initializeClass(this); BodyROSItem::initializeClass(this); + ROSControlItem::initializeClass(this); BodyPublisherItem::initializeClass(this); return true; diff --git a/src/plugin/RobotHWSim.cpp b/src/plugin/RobotHWSim.cpp new file mode 100644 index 0000000..ee1496b --- /dev/null +++ b/src/plugin/RobotHWSim.cpp @@ -0,0 +1,68 @@ +///////////////////////////////// +/// @author Ryodo Tanaka +///////////////////////////////// + +#include "RobotHWSim.h" +#include +#include +#include + +namespace cnoid{ + +bool RobotHWSim::initSim(ros::NodeHandle& nh, const std::string& param_name) +{ + nh_ = nh; + bool ret = false; + ret = loadURDF(param_name); +} + +bool RobotHWSim::loadURDF(const std::string& param_name) +{ + using namespace std; + using namespace cnoid; + string urdf_string; + stringstream ss; + while(urdf_string.empty()) { + string search_param_name; + if (nh_.searchParam(param_name, search_param_name)) { + ss.clear(); + ss << "Waiting for URDF model." << endl; + ss << "Finding parameter name : " << search_param_name << endl; + MessageView::instance()->put(ss.str(), MessageView::Warning); + + nh_.getParam(search_param_name, urdf_string); + sleep(1); + } + else { + ss.clear(); + ss << "There are no parameter name : " << search_param_name << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); + return false; + } + } + urdf_model_.initString(urdf_string); + + ss.clear(); + ss << "cnoid::RobotHWSim loaded the URDF model." << endl; + MessageView::instance()->put(ss.str(), MessageView::Normal); + + // load transmissions // + transmission_interface::TransmissionParser::parse(urdf_string, tmss_); + + ss.clear(); + ss << "cnoid::RobotHWSim loaded Transmission." << endl; + MessageView::instance()->put(ss.str(), MessageView::Normal); + + return true; +} + +void RobotHWSim::read(const ros::Time& time, const ros::Duration& period) +{ +} + +void RobotHWSim::write(const ros::Time& time, const ros::Duration& period) +{ +} +} // namespace cnoid + +PLUGINLIB_EXPORT_CLASS(cnoid::RobotHWSim, hardware_interface::RobotHW) diff --git a/src/plugin/RobotHWSim.h b/src/plugin/RobotHWSim.h new file mode 100644 index 0000000..c8cc9bc --- /dev/null +++ b/src/plugin/RobotHWSim.h @@ -0,0 +1,59 @@ +/////////////////////////////////////////////////////////// +/// @file RobotHWSim.h +/// @brief RobotHW interface for Simulation +/// @author Ryodo Tanaka +/////////////////////////////////////////////////////////// + +#ifndef CNOID_ROS_PLUGIN_ROBOT_HW_SIM_H +#define CNOID_ROS_PLUGIN_ROBOT_HW_SIM_H + +// ROS // +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cnoid +{ +class RobotHWSim : public hardware_interface::RobotHW +{ + public: + bool initSim(ros::NodeHandle& nh, const std::string& param_name); + + virtual void read(const ros::Time& time, const ros::Duration& period) override; + virtual void write(const ros::Time& time, const ros::Duration& period) override; + + private: + + bool loadURDF(const std::string& param_name); + + ros::NodeHandle nh_; + std::vector tmss_; + urdf::Model urdf_model_; + std::string robot_description_; + + uint dof_{0}; + hardware_interface::JointStateInterface js_if_; + hardware_interface::PositionJointInterface pj_if_; + hardware_interface::VelocityJointInterface vj_if_; + hardware_interface::EffortJointInterface ej_if_; + + joint_limits_interface::PositionJointSaturationInterface pj_sat_if_; + joint_limits_interface::PositionJointSoftLimitsInterface pj_lim_if_; + joint_limits_interface::VelocityJointSaturationInterface vj_sat_if_; + joint_limits_interface::VelocityJointSoftLimitsInterface vj_lim_if_; + joint_limits_interface::EffortJointSaturationInterface ej_sat_if_; + joint_limits_interface::EffortJointSoftLimitsInterface ej_lim_if_; +}; + +typedef std::shared_ptr RobotHWSimPtr; +} // namespace cnoid + +#endif // CNOID_ROS_PLUGIN_ROBOT_HW_SIM_H From def1be9f78ddfef499a5eb68ae73cc941b72417e Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Thu, 3 Sep 2020 02:55:35 +0900 Subject: [PATCH 02/33] Update plugin setting xml file --- robot_hardware_sim.xml => plugins.xml | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) rename robot_hardware_sim.xml => plugins.xml (65%) diff --git a/robot_hardware_sim.xml b/plugins.xml similarity index 65% rename from robot_hardware_sim.xml rename to plugins.xml index c4aeac1..e46af9f 100644 --- a/robot_hardware_sim.xml +++ b/plugins.xml @@ -1,8 +1,7 @@ - - + A default robot simulation interface which constructs joint handles from an SDF/URDF. From b192f4606e5d3688b24610181e74cef9cb6c142a Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Thu, 3 Sep 2020 02:55:51 +0900 Subject: [PATCH 03/33] Update to inherit RobortHW --- src/plugin/RobotHWSim.cpp | 68 ------------------------ src/plugin/cnoid_hw.cpp | 69 +++++++++++++++++++++++++ src/plugin/{RobotHWSim.h => cnoid_hw.h} | 18 +++---- 3 files changed, 78 insertions(+), 77 deletions(-) delete mode 100644 src/plugin/RobotHWSim.cpp create mode 100644 src/plugin/cnoid_hw.cpp rename src/plugin/{RobotHWSim.h => cnoid_hw.h} (80%) diff --git a/src/plugin/RobotHWSim.cpp b/src/plugin/RobotHWSim.cpp deleted file mode 100644 index ee1496b..0000000 --- a/src/plugin/RobotHWSim.cpp +++ /dev/null @@ -1,68 +0,0 @@ -///////////////////////////////// -/// @author Ryodo Tanaka -///////////////////////////////// - -#include "RobotHWSim.h" -#include -#include -#include - -namespace cnoid{ - -bool RobotHWSim::initSim(ros::NodeHandle& nh, const std::string& param_name) -{ - nh_ = nh; - bool ret = false; - ret = loadURDF(param_name); -} - -bool RobotHWSim::loadURDF(const std::string& param_name) -{ - using namespace std; - using namespace cnoid; - string urdf_string; - stringstream ss; - while(urdf_string.empty()) { - string search_param_name; - if (nh_.searchParam(param_name, search_param_name)) { - ss.clear(); - ss << "Waiting for URDF model." << endl; - ss << "Finding parameter name : " << search_param_name << endl; - MessageView::instance()->put(ss.str(), MessageView::Warning); - - nh_.getParam(search_param_name, urdf_string); - sleep(1); - } - else { - ss.clear(); - ss << "There are no parameter name : " << search_param_name << endl; - MessageView::instance()->put(ss.str(), MessageView::Error); - return false; - } - } - urdf_model_.initString(urdf_string); - - ss.clear(); - ss << "cnoid::RobotHWSim loaded the URDF model." << endl; - MessageView::instance()->put(ss.str(), MessageView::Normal); - - // load transmissions // - transmission_interface::TransmissionParser::parse(urdf_string, tmss_); - - ss.clear(); - ss << "cnoid::RobotHWSim loaded Transmission." << endl; - MessageView::instance()->put(ss.str(), MessageView::Normal); - - return true; -} - -void RobotHWSim::read(const ros::Time& time, const ros::Duration& period) -{ -} - -void RobotHWSim::write(const ros::Time& time, const ros::Duration& period) -{ -} -} // namespace cnoid - -PLUGINLIB_EXPORT_CLASS(cnoid::RobotHWSim, hardware_interface::RobotHW) diff --git a/src/plugin/cnoid_hw.cpp b/src/plugin/cnoid_hw.cpp new file mode 100644 index 0000000..b5d4784 --- /dev/null +++ b/src/plugin/cnoid_hw.cpp @@ -0,0 +1,69 @@ +///////////////////////////////// +/// @author Ryodo Tanaka +///////////////////////////////// + +#include "cnoid_hw.h" +#include +#include +#include + +namespace hardware_interface { + +bool CnoidHW::init(ros::NodeHandle& robot_nh, ros::NodeHandle& robot_hw_nh) +{ + nh_ = robot_hw_nh; + bool ret = false; + ret = loadURDF("robot_description"); + return ret; +} + +bool CnoidHW::loadURDF(const std::string& param_name) +{ + using namespace std; + using namespace cnoid; + string urdf_string; + stringstream ss; + while(urdf_string.empty()) { + string searched_param; + if (nh_.searchParam(param_name, searched_param)) { + if(!nh_.getParam(searched_param, urdf_string)) { + ss.str(""); + ss << "Waiting for URDF model." << endl; + ss << "Finding parameter name : " << searched_param << endl; + MessageView::instance()->put(ss.str(), MessageView::Warning); + sleep(1); + } + } + else { + ss.str(""); + ss << "There are no parameter related to : " << param_name << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); + return false; + } + } + urdf_model_.initString(urdf_string); + + ss.str(""); + ss << "CnoidHW loaded the URDF model." << endl; + MessageView::instance()->put(ss.str(), MessageView::Normal); + + // load transmissions // + transmission_interface::TransmissionParser::parse(urdf_string, tmss_); + + ss.str(""); + ss << "CnoidHW loaded Transmission." << endl; + MessageView::instance()->put(ss.str(), MessageView::Normal); + + return true; +} + +void CnoidHW::read(const ros::Time& time, const ros::Duration& period) +{ +} + +void CnoidHW::write(const ros::Time& time, const ros::Duration& period) +{ +} +} // namespace hardware_interface + +PLUGINLIB_EXPORT_CLASS(hardware_interface::CnoidHW, hardware_interface::RobotHW) diff --git a/src/plugin/RobotHWSim.h b/src/plugin/cnoid_hw.h similarity index 80% rename from src/plugin/RobotHWSim.h rename to src/plugin/cnoid_hw.h index c8cc9bc..a8e7a2b 100644 --- a/src/plugin/RobotHWSim.h +++ b/src/plugin/cnoid_hw.h @@ -4,8 +4,8 @@ /// @author Ryodo Tanaka /////////////////////////////////////////////////////////// -#ifndef CNOID_ROS_PLUGIN_ROBOT_HW_SIM_H -#define CNOID_ROS_PLUGIN_ROBOT_HW_SIM_H +#ifndef HARDWARE_INTERFACE_CNOID_HW_H +#define HARDWARE_INTERFACE_CNOID_HW_H // ROS // #include @@ -20,19 +20,19 @@ #include #include -namespace cnoid +namespace hardware_interface { -class RobotHWSim : public hardware_interface::RobotHW +class CnoidHW : public RobotHW { public: - bool initSim(ros::NodeHandle& nh, const std::string& param_name); + virtual bool init(ros::NodeHandle& robot_nh, ros::NodeHandle& robot_hw_nh) override; virtual void read(const ros::Time& time, const ros::Duration& period) override; virtual void write(const ros::Time& time, const ros::Duration& period) override; private: - bool loadURDF(const std::string& param_name); + bool loadURDF(const std::string& param_name="robot_description"); ros::NodeHandle nh_; std::vector tmss_; @@ -53,7 +53,7 @@ class RobotHWSim : public hardware_interface::RobotHW joint_limits_interface::EffortJointSoftLimitsInterface ej_lim_if_; }; -typedef std::shared_ptr RobotHWSimPtr; -} // namespace cnoid +typedef std::shared_ptr CnoidHWPtr; +} // namespace hardware_interface -#endif // CNOID_ROS_PLUGIN_ROBOT_HW_SIM_H +#endif // HARDWARE_INTERFACE_CNOID_HW_H From e4fe49ecf041838bce78c657cd30f1d649b38f8e Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Thu, 3 Sep 2020 02:56:23 +0900 Subject: [PATCH 04/33] Update to add dependencies --- CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index d07aa59..203e95b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,7 +13,13 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs image_transport choreonoid + controller_manager + control_toolbox pluginlib + hardware_interface + transmission_interface + joint_limits_interface + urdf ) # find_package(Qt5 COMPONENTS Widgets REQUIRED) From 2506b5ef41b1db22ad56679765f788760a644cca Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Thu, 3 Sep 2020 02:56:36 +0900 Subject: [PATCH 05/33] Update to install plugins.xml file --- CMakeLists.txt | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 203e95b..bf58c36 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -48,7 +48,11 @@ link_directories( add_subdirectory(src) -install( - DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +# install( +# DIRECTORY launch/ +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +# ) + +install(FILES plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) From 58b614f3259e5be713bc30d2a39f0b04675fd34e Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Thu, 3 Sep 2020 02:56:49 +0900 Subject: [PATCH 06/33] Update to set default value for argments --- launch/choreonoid.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/choreonoid.launch b/launch/choreonoid.launch index 620de73..64e231c 100644 --- a/launch/choreonoid.launch +++ b/launch/choreonoid.launch @@ -1,5 +1,5 @@ - + From 38c498a603b2f48cbbeea5a34f4cb8eca8ddf3ab Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Thu, 3 Sep 2020 02:57:03 +0900 Subject: [PATCH 07/33] Update dependencies & plugins.xml file name --- package.xml | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 59f07dd..f995d40 100644 --- a/package.xml +++ b/package.xml @@ -14,9 +14,16 @@ std_msgs sensor_msgs image_transport + + controller_manager + control_toolbox pluginlib + hardware_interface + transmission_interface + joint_limits_interface + urdf cmake - + From fc9ac6bba705af8cd0546eacb52d17832b2bca33 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Thu, 3 Sep 2020 02:58:12 +0900 Subject: [PATCH 08/33] Update indent --- src/plugin/BodyROSItem.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/plugin/BodyROSItem.h b/src/plugin/BodyROSItem.h index 0f6256e..55b24a1 100644 --- a/src/plugin/BodyROSItem.h +++ b/src/plugin/BodyROSItem.h @@ -26,8 +26,8 @@ #include #include -namespace cnoid { - +namespace cnoid +{ class BodyROSItem : public ControllerItem { public: From 225f3ca08d77abc80c008e6d30b225784100e9fb Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Thu, 3 Sep 2020 02:58:31 +0900 Subject: [PATCH 09/33] Update to add CnoidHW class --- src/plugin/CMakeLists.txt | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/plugin/CMakeLists.txt b/src/plugin/CMakeLists.txt index 9030afa..3f69d6e 100644 --- a/src/plugin/CMakeLists.txt +++ b/src/plugin/CMakeLists.txt @@ -2,12 +2,22 @@ set(target CnoidROSPlugin) choreonoid_make_gettext_mo_files(${target} mofiles) +# RobotHW inherited ros_control Choreonoid interface class # +add_library(CnoidHW cnoid_hw.cpp) +target_link_libraries(CnoidHW ${catkin_LIBRARIES} ${CHOREONOID_BODY_LIBRARIES} ${CHOREONOID_BODY_PLUGIN_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} CnoidHW + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + +# Choreonoid Plugins # choreonoid_add_plugin(${target} ROSPlugin.cpp WorldROSItem.cpp BodyROSItem.cpp ROSControlItem.cpp - RobotHWSim.cpp deprecated/BodyPublisherItem.cpp ${mofiles} ) @@ -17,8 +27,8 @@ target_link_libraries(${target} ${std_msgs_LIBRARIES} ${sensor_msgs_LIBRARIES} ${image_transport_LIBRARIES} - ${catkin_LIBRARIES} ${CHOREONOID_BODY_PLUGIN_LIBRARIES} + ${catkin_LIBRARIES} ) if(CHOREONOID_ENABLE_PYTHON) From 7ae6737c610623f2e38a649fb778d7af36c01902 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Thu, 3 Sep 2020 02:58:49 +0900 Subject: [PATCH 10/33] Update to utilize CnoidHW --- src/plugin/ROSControlItem.cpp | 73 ++++++++++++++++++++++++----------- src/plugin/ROSControlItem.h | 23 +++-------- 2 files changed, 57 insertions(+), 39 deletions(-) diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp index 1f94515..21d2dbf 100644 --- a/src/plugin/ROSControlItem.cpp +++ b/src/plugin/ROSControlItem.cpp @@ -5,15 +5,20 @@ #include "ROSControlItem.h" #include #include +#include #include #include #include +#include +#include "gettext.h" namespace cnoid { void ROSControlItem::initializeClass(ExtensionManager* ext) { - ext->itemManager().registerClass("ROSControlItem"); + using namespace std; + using fmt::format; + ext->itemManager().registerClass(N_("ROSControlItem")); ext->itemManager().addCreationPanel(); } @@ -24,7 +29,12 @@ ROSControlItem::ROSControlItem(void) ROSControlItem::ROSControlItem(const ROSControlItem& org) : ControllerItem(org) { - io_ = nullptr; + io_ = nullptr; +} + +ROSControlItem::~ROSControlItem() +{ + stop(); } Item* ROSControlItem::doDuplicate(void) const @@ -35,7 +45,6 @@ Item* ROSControlItem::doDuplicate(void) const bool ROSControlItem::store(Archive& archive) { archive.write("name space", namespace_); - archive.write("robot_descripton", robot_description_); return true; } @@ -45,19 +54,13 @@ bool ROSControlItem::restore(const Archive& archive) if(!archive.read("name space", namespace_)) archive.read("name space", namespace_); - if(!archive.read("robot_description", robot_description_)) - archive.read("robot_escription", robot_description_); - return true; } void ROSControlItem::doPutProperties(PutPropertyFunction& putProperty) { - putProperty("Name space for the program.", + putProperty("Name space", namespace_, changeProperty(namespace_)); - - putProperty("Robot description parameter name.", - robot_description_, changeProperty(robot_description_)); } bool ROSControlItem::initialize(ControllerIO* io) @@ -68,7 +71,7 @@ bool ROSControlItem::initialize(ControllerIO* io) // Check body // if (!io->body()) { - ss.clear(); + ss.str(""); ss << "ROSControlItem \"" << displayName() << "\" is invalid." << endl; ss << "Because it is not assigned to a body." << endl; MessageView::instance()->put(ss.str(), MessageView::Error); @@ -78,13 +81,14 @@ bool ROSControlItem::initialize(ControllerIO* io) // Check that ROS has been initialized if(!ros::isInitialized()) { - ss.clear(); + ss.str(""); ss << "ROS master is not initialized." << endl; MessageView::instance()->put(ss.str(), MessageView::Warning); return false; } - + + // Copy elements to the local arfuments // io_ = io; body_ = io->body(); tstep_ = io->worldTimeStep(); @@ -96,19 +100,18 @@ bool ROSControlItem::initialize(ControllerIO* io) bool ROSControlItem::start(void) { using namespace std; - using namespace cnoid; + using namespace hardware_interface; stringstream ss; nh_ = ros::NodeHandle(namespace_); try { // ros plugin loader // - rbt_hw_sim_loader_ = make_shared>("choreonoid_ros", "cnoid::RobotHWSim"); - - // rbt_hw_sim_ = to_std(rbt_hw_sim_loader_->createInstance("cnoid/RobotHWSim")); - // load cnoid::RobotHWSim // - if(!rbt_hw_sim_->initSim(nh_, robot_description_)) { - ss.clear(); + rbt_hw_sim_loader_ = make_shared>("choreonoid_ros", "hardware_interface::RobotHW"); + rbt_hw_sim_ = rbt_hw_sim_loader_->createInstance("hardware_interface/CnoidHW"); + // load hardware_interface // + if(!rbt_hw_sim_->init(nh_, nh_)) { + ss.str(""); ss << "Could not initialize robot simulation interface" << endl; MessageView::instance()->put(ss.str(), MessageView::Error); } @@ -116,12 +119,12 @@ bool ROSControlItem::start(void) manager_ = make_shared(rbt_hw_sim_.get(), nh_); } catch(pluginlib::LibraryLoadException &ex) { - ss.clear(); + ss.str(""); ss << "Failed to create robot simulation interface loader : " << ex.what() << endl; MessageView::instance()->put(ss.str(), MessageView::Error); } - ss.clear(); + ss.str(""); ss << "Loaded cnoid::ROSControlItem" << endl; MessageView::instance()->put(ss.str(), MessageView::Normal); @@ -130,13 +133,39 @@ bool ROSControlItem::start(void) void ROSControlItem::input(void) { + time_ = io_->currentTime(); + + using namespace std; + using namespace cnoid; + stringstream ss; + ss.str(""); + ss << "input() : " << time_ << endl; + MessageView::instance()->put(ss.str(), MessageView::Normal); } bool ROSControlItem::control(void) { + time_ = io_->currentTime(); + + using namespace std; + using namespace cnoid; + stringstream ss; + ss.str(""); + ss << "control() : " << time_ << endl; + MessageView::instance()->put(ss.str(), MessageView::Normal); + } void ROSControlItem::output(void) { + time_ = io_->currentTime(); + + using namespace std; + using namespace cnoid; + stringstream ss; + ss.str(""); + ss << "output() : " << time_ << endl; + MessageView::instance()->put(ss.str(), MessageView::Normal); + } void ROSControlItem::stop(void) diff --git a/src/plugin/ROSControlItem.h b/src/plugin/ROSControlItem.h index 4217b7d..867961a 100644 --- a/src/plugin/ROSControlItem.h +++ b/src/plugin/ROSControlItem.h @@ -13,14 +13,15 @@ // ROS // #include -#include "RobotHWSim.h" #include #include +#include // STL // #include #include -#include + +// Boost // #include namespace cnoid @@ -57,26 +58,14 @@ class ROSControlItem : public ControllerItem double time_; ros::NodeHandle nh_; - std::shared_ptr> rbt_hw_sim_loader_; - RobotHWSimPtr rbt_hw_sim_; + std::shared_ptr> rbt_hw_sim_loader_; + boost::shared_ptr rbt_hw_sim_; std::shared_ptr manager_; - std::string namespace_{""}; - std::string robot_description_{"robot_description"}; }; -typedef std::shared_ptr ROSControlPtr; - -template -boost::shared_ptr to_boost(const std::shared_ptr &p) { - return boost::shared_ptr(p.get(), [p](...) mutable { p.reset(); }); -} - -template -std::shared_ptr to_std(const boost::shared_ptr &p) { - return std::shared_ptr(p.get(), [p](...) mutable { p.reset(); }); -} +typedef ref_ptr ROSControlItemPtr; } // namespace cnoid From 179c3508c41f36054bba1f4d7850cab787f0c34f Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 17:17:25 +0900 Subject: [PATCH 11/33] Delete to change file name --- src/plugin/cnoid_hw.cpp | 69 ----------------------------------------- src/plugin/cnoid_hw.h | 59 ----------------------------------- 2 files changed, 128 deletions(-) delete mode 100644 src/plugin/cnoid_hw.cpp delete mode 100644 src/plugin/cnoid_hw.h diff --git a/src/plugin/cnoid_hw.cpp b/src/plugin/cnoid_hw.cpp deleted file mode 100644 index b5d4784..0000000 --- a/src/plugin/cnoid_hw.cpp +++ /dev/null @@ -1,69 +0,0 @@ -///////////////////////////////// -/// @author Ryodo Tanaka -///////////////////////////////// - -#include "cnoid_hw.h" -#include -#include -#include - -namespace hardware_interface { - -bool CnoidHW::init(ros::NodeHandle& robot_nh, ros::NodeHandle& robot_hw_nh) -{ - nh_ = robot_hw_nh; - bool ret = false; - ret = loadURDF("robot_description"); - return ret; -} - -bool CnoidHW::loadURDF(const std::string& param_name) -{ - using namespace std; - using namespace cnoid; - string urdf_string; - stringstream ss; - while(urdf_string.empty()) { - string searched_param; - if (nh_.searchParam(param_name, searched_param)) { - if(!nh_.getParam(searched_param, urdf_string)) { - ss.str(""); - ss << "Waiting for URDF model." << endl; - ss << "Finding parameter name : " << searched_param << endl; - MessageView::instance()->put(ss.str(), MessageView::Warning); - sleep(1); - } - } - else { - ss.str(""); - ss << "There are no parameter related to : " << param_name << endl; - MessageView::instance()->put(ss.str(), MessageView::Error); - return false; - } - } - urdf_model_.initString(urdf_string); - - ss.str(""); - ss << "CnoidHW loaded the URDF model." << endl; - MessageView::instance()->put(ss.str(), MessageView::Normal); - - // load transmissions // - transmission_interface::TransmissionParser::parse(urdf_string, tmss_); - - ss.str(""); - ss << "CnoidHW loaded Transmission." << endl; - MessageView::instance()->put(ss.str(), MessageView::Normal); - - return true; -} - -void CnoidHW::read(const ros::Time& time, const ros::Duration& period) -{ -} - -void CnoidHW::write(const ros::Time& time, const ros::Duration& period) -{ -} -} // namespace hardware_interface - -PLUGINLIB_EXPORT_CLASS(hardware_interface::CnoidHW, hardware_interface::RobotHW) diff --git a/src/plugin/cnoid_hw.h b/src/plugin/cnoid_hw.h deleted file mode 100644 index a8e7a2b..0000000 --- a/src/plugin/cnoid_hw.h +++ /dev/null @@ -1,59 +0,0 @@ -/////////////////////////////////////////////////////////// -/// @file RobotHWSim.h -/// @brief RobotHW interface for Simulation -/// @author Ryodo Tanaka -/////////////////////////////////////////////////////////// - -#ifndef HARDWARE_INTERFACE_CNOID_HW_H -#define HARDWARE_INTERFACE_CNOID_HW_H - -// ROS // -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace hardware_interface -{ -class CnoidHW : public RobotHW -{ - public: - virtual bool init(ros::NodeHandle& robot_nh, ros::NodeHandle& robot_hw_nh) override; - - virtual void read(const ros::Time& time, const ros::Duration& period) override; - virtual void write(const ros::Time& time, const ros::Duration& period) override; - - private: - - bool loadURDF(const std::string& param_name="robot_description"); - - ros::NodeHandle nh_; - std::vector tmss_; - urdf::Model urdf_model_; - std::string robot_description_; - - uint dof_{0}; - hardware_interface::JointStateInterface js_if_; - hardware_interface::PositionJointInterface pj_if_; - hardware_interface::VelocityJointInterface vj_if_; - hardware_interface::EffortJointInterface ej_if_; - - joint_limits_interface::PositionJointSaturationInterface pj_sat_if_; - joint_limits_interface::PositionJointSoftLimitsInterface pj_lim_if_; - joint_limits_interface::VelocityJointSaturationInterface vj_sat_if_; - joint_limits_interface::VelocityJointSoftLimitsInterface vj_lim_if_; - joint_limits_interface::EffortJointSaturationInterface ej_sat_if_; - joint_limits_interface::EffortJointSoftLimitsInterface ej_lim_if_; -}; - -typedef std::shared_ptr CnoidHWPtr; -} // namespace hardware_interface - -#endif // HARDWARE_INTERFACE_CNOID_HW_H From d928baf955a7deef2bf303751270fd0b375bbdb3 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 17:17:36 +0900 Subject: [PATCH 12/33] Update to add robot_hw_sim class --- src/plugin/robot_hw_sim.h | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 src/plugin/robot_hw_sim.h diff --git a/src/plugin/robot_hw_sim.h b/src/plugin/robot_hw_sim.h new file mode 100644 index 0000000..7834937 --- /dev/null +++ b/src/plugin/robot_hw_sim.h @@ -0,0 +1,29 @@ +/////////////////////////////////////////////////////////// +/// @file robot_hw_sim.h +/// @brief ros_control interface for Simulator +/// @author Ryodo Tanaka +/////////////////////////////////////////////////////////// + +#ifndef HARDWARE_INTERFACE_ROBOT_HW_SIM_H +#define HARDWARE_INTERFACE_ROBOT_HW_SIM_H + +#include +#include + +namespace hardware_interface +{ +template +class RobotHWSim : public RobotHW +{ + public: + RobotHWSim(void) = default; + ~RobotHWSim(void) = default; + + virtual bool initSim(const ros::NodeHandle& nh, Ts... args) = 0; + virtual void read(const ros::Time& time, const ros::Duration& period) override; + virtual void write(const ros::Time& time, const ros::Duration& period) override; +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE_ROBOT_HW_SIM_H From 832f9ff7b6493cc63a43d9598520e823d3038d72 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 17:18:06 +0900 Subject: [PATCH 13/33] Update to utilize template for base class --- plugins.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/plugins.xml b/plugins.xml index e46af9f..1e992e0 100644 --- a/plugins.xml +++ b/plugins.xml @@ -1,8 +1,8 @@ - + + name="hardware_interface/RobotHWCnoid" + type="hardware_interface::RobotHWCnoid" + base_class_type="hardware_interface::RobotHWSim"> A default robot simulation interface which constructs joint handles from an SDF/URDF. From a7512052d109c790afc2b226cf90259d4d4328f5 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 17:18:36 +0900 Subject: [PATCH 14/33] Update to utilize RobotHwCnoid --- src/plugin/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/plugin/CMakeLists.txt b/src/plugin/CMakeLists.txt index 3f69d6e..922e16d 100644 --- a/src/plugin/CMakeLists.txt +++ b/src/plugin/CMakeLists.txt @@ -3,10 +3,10 @@ set(target CnoidROSPlugin) choreonoid_make_gettext_mo_files(${target} mofiles) # RobotHW inherited ros_control Choreonoid interface class # -add_library(CnoidHW cnoid_hw.cpp) -target_link_libraries(CnoidHW ${catkin_LIBRARIES} ${CHOREONOID_BODY_LIBRARIES} ${CHOREONOID_BODY_PLUGIN_LIBRARIES}) +add_library(RobotHWCnoid robot_hw_cnoid.cpp) +target_link_libraries(RobotHWCnoid ${catkin_LIBRARIES} ${CHOREONOID_BODY_LIBRARIES} ${CHOREONOID_BODY_PLUGIN_LIBRARIES}) -install(TARGETS ${PROJECT_NAME} CnoidHW +install(TARGETS ${PROJECT_NAME} RobotHWCnoid LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} From 46af6637a8318115110e0f5bc5027390548067b5 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 17:18:52 +0900 Subject: [PATCH 15/33] Update to compile robot_hw_sim --- src/plugin/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/plugin/CMakeLists.txt b/src/plugin/CMakeLists.txt index 922e16d..bf8a70e 100644 --- a/src/plugin/CMakeLists.txt +++ b/src/plugin/CMakeLists.txt @@ -17,6 +17,7 @@ choreonoid_add_plugin(${target} ROSPlugin.cpp WorldROSItem.cpp BodyROSItem.cpp + robot_hw_sim.h ROSControlItem.cpp deprecated/BodyPublisherItem.cpp ${mofiles} From 5331391e3303a143c48001713ba17b37bb3f6ab6 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 17:19:36 +0900 Subject: [PATCH 16/33] Update to utilize templated robot_hw_sim class --- src/plugin/ROSControlItem.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp index 21d2dbf..4317b19 100644 --- a/src/plugin/ROSControlItem.cpp +++ b/src/plugin/ROSControlItem.cpp @@ -107,18 +107,17 @@ bool ROSControlItem::start(void) try { // ros plugin loader // - rbt_hw_sim_loader_ = make_shared>("choreonoid_ros", "hardware_interface::RobotHW"); - rbt_hw_sim_ = rbt_hw_sim_loader_->createInstance("hardware_interface/CnoidHW"); + rbt_hw_sim_loader_ = make_shared>>("choreonoid_ros", "hardware_interface::RobotHWSim"); + rbt_hw_sim_ = rbt_hw_sim_loader_->createInstance("hardware_interface/RobotHWCnoid"); // load hardware_interface // - if(!rbt_hw_sim_->init(nh_, nh_)) { + if(!rbt_hw_sim_->initSim(nh_, io_)) { ss.str(""); ss << "Could not initialize robot simulation interface" << endl; MessageView::instance()->put(ss.str(), MessageView::Error); } // register ros control manager // manager_ = make_shared(rbt_hw_sim_.get(), nh_); - } - catch(pluginlib::LibraryLoadException &ex) { + } catch(pluginlib::LibraryLoadException &ex) { ss.str(""); ss << "Failed to create robot simulation interface loader : " << ex.what() << endl; MessageView::instance()->put(ss.str(), MessageView::Error); From 078fc0713b8d2437678fd61b51530637d9b038b6 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 17:20:10 +0900 Subject: [PATCH 17/33] Update for input(), control(), output() --- src/plugin/ROSControlItem.cpp | 55 +++++++++++++++++++++-------------- src/plugin/ROSControlItem.h | 6 ++-- 2 files changed, 36 insertions(+), 25 deletions(-) diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp index 4317b19..9cc7263 100644 --- a/src/plugin/ROSControlItem.cpp +++ b/src/plugin/ROSControlItem.cpp @@ -100,6 +100,7 @@ bool ROSControlItem::initialize(ControllerIO* io) bool ROSControlItem::start(void) { using namespace std; + using namespace cnoid; using namespace hardware_interface; stringstream ss; @@ -126,45 +127,55 @@ bool ROSControlItem::start(void) ss.str(""); ss << "Loaded cnoid::ROSControlItem" << endl; MessageView::instance()->put(ss.str(), MessageView::Normal); + + cnoid::Link* turret_y = io_->body()->link("TURRET_Y"); + turret_y->setActuationMode(turret_y->actuationMode()); + cnoid::Link* turret_p = io_->body()->link("TURRET_P"); + turret_p->setActuationMode(turret_p->actuationMode()); return true; } void ROSControlItem::input(void) { - time_ = io_->currentTime(); - - using namespace std; - using namespace cnoid; - stringstream ss; - ss.str(""); - ss << "input() : " << time_ << endl; - MessageView::instance()->put(ss.str(), MessageView::Normal); } bool ROSControlItem::control(void) { - time_ = io_->currentTime(); + int last_sec = static_cast(time_); + double last_nsec = (time_ - last_sec) * 1e9; + int now_sec = static_cast(io_->currentTime()); + double now_nsec = (io_->currentTime() - now_sec) * 1e9; - using namespace std; - using namespace cnoid; - stringstream ss; - ss.str(""); - ss << "control() : " << time_ << endl; - MessageView::instance()->put(ss.str(), MessageView::Normal); + ros::Time last(last_sec, static_cast(last_nsec)); + ros::Time now(now_sec, static_cast(now_nsec)); + ros::Duration period = now - last; + + cnoid::Link* turret_y = io_->body()->link("TURRET_Y"); + cnoid::Link* turret_p = io_->body()->link("TURRET_P"); + + turret_y->q_target() = 0; + turret_p->q_target() = 0; + // rbt_hw_sim_->read(now, period); + // rbt_hw_sim_->write(now, period); + + time_ = io_->currentTime(); } void ROSControlItem::output(void) { - time_ = io_->currentTime(); + // int last_sec = static_cast(time_); + // double last_nsec = (time_ - last_sec) * 1e9; + // int now_sec = static_cast(io_->currentTime()); + // double now_nsec = (io_->currentTime() - now_sec) * 1e9; - using namespace std; - using namespace cnoid; - stringstream ss; - ss.str(""); - ss << "output() : " << time_ << endl; - MessageView::instance()->put(ss.str(), MessageView::Normal); + // ros::Time last(last_sec, static_cast(last_nsec)); + // ros::Time now(now_sec, static_cast(now_nsec)); + // ros::Duration period = now - last; + // rbt_hw_sim_->write(now, period); + + // time_ = io_->currentTime(); } void ROSControlItem::stop(void) diff --git a/src/plugin/ROSControlItem.h b/src/plugin/ROSControlItem.h index 867961a..efac826 100644 --- a/src/plugin/ROSControlItem.h +++ b/src/plugin/ROSControlItem.h @@ -15,7 +15,7 @@ #include #include #include -#include +#include "robot_hw_sim.h" // STL // #include @@ -58,8 +58,8 @@ class ROSControlItem : public ControllerItem double time_; ros::NodeHandle nh_; - std::shared_ptr> rbt_hw_sim_loader_; - boost::shared_ptr rbt_hw_sim_; + std::shared_ptr>> rbt_hw_sim_loader_; + boost::shared_ptr> rbt_hw_sim_; std::shared_ptr manager_; std::string namespace_{""}; From d768ce90e493d9dcba03c2ce27a7414805c532ef Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 17:20:36 +0900 Subject: [PATCH 18/33] Add RoboHWCnoid class --- src/plugin/robot_hw_cnoid.cpp | 361 ++++++++++++++++++++++++++++++++++ src/plugin/robot_hw_cnoid.h | 93 +++++++++ 2 files changed, 454 insertions(+) create mode 100644 src/plugin/robot_hw_cnoid.cpp create mode 100644 src/plugin/robot_hw_cnoid.h diff --git a/src/plugin/robot_hw_cnoid.cpp b/src/plugin/robot_hw_cnoid.cpp new file mode 100644 index 0000000..f678cb7 --- /dev/null +++ b/src/plugin/robot_hw_cnoid.cpp @@ -0,0 +1,361 @@ +///////////////////////////////// +/// @author Ryodo Tanaka +///////////////////////////////// + +#include "robot_hw_cnoid.h" +// ROS // +#include +// Cnoid // +#include +// STL // +#include +#include +#include + +namespace hardware_interface { + +bool RobotHWCnoid::initSim(const ros::NodeHandle& nh, cnoid::ControllerIO* args) +{ + using namespace std; + using namespace cnoid; + namespace hi = hardware_interface; + stringstream ss; + + nh_ = nh; + io_ = args; + + if(!loadURDF("robot_description")) + return false; + + dof_ = tmss_.size(); + joint_names_.resize(dof_); + ctrl_type_.resize(dof_); + data_.resize(dof_); + command_.resize(dof_); + + joint_types_.resize(dof_); + limits_.resize(dof_); + + unordered_map if_map; + if_map["PositionJointInterface"] = ControlType::POSITION; + if_map["VelocityJointInterface"] = ControlType::VELOCITY; + if_map["EffortJointInterface"] = ControlType::EFFORT; + if_map["hardware_interface/PositionJointInterface"] = ControlType::POSITION; + if_map["hardware_interface/VelocityJointInterface"] = ControlType::VELOCITY; + if_map["hardware_interface/EffortJointInterface"] = ControlType::EFFORT; + + // Initialize values + for(uint i=0; i joint_ifs = tmss_[i].joints_[0].hardware_interfaces_; + if(tmss_[i].actuators_.empty() && tmss_[i].actuators_[0].hardware_interfaces_.empty()) { + ss.str(""); + ss << " should be nested inside the element, not . "; + ss << "The transmission will be properly loaded, but please update "; + ss << "your robot model to remain compatible with future versions of the plugin." << endl; + MessageView::instance()->put(ss.str(), MessageView::Warning); + } + if(joint_ifs.empty()) { + ss.str(""); + ss << tmss_[i].name_ << " on " << tmss_[i].joints_[0].name_; + ss << " does not specify any hardware interface." << endl; + MessageView::instance()->put(ss.str(), MessageView::Warning); + continue; + } else if(joint_ifs.size() > 1) { + ss.str(""); + ss << tmss_[i].joints_[0].name_ << " of transmission "; + ss << " specifies multiple hardware interfaces. "; + ss << "Currently the default robot hardware simulation interface only supports one. Using the first entry" << endl; + MessageView::instance()->put(ss.str(), MessageView::Warning); + } + + // Get joint names // + joint_names_[i] = tmss_[i].joints_[0].name_; + + // Get cnoid::Link // + Link* link_tmp = io_->body()->link(joint_names_[i].c_str()); + Link* link = io_->body()->link(link_tmp->index()); + if(link == nullptr) { + ss.str(""); + ss << "This robot has a joint named \"" << joint_names_[i]; + ss << "\" which is not in the choreonoid model." << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); + return false; + } + links_.emplace_back(link); + + // Store current position, velocity, effort // + data_[i].position = link->q(); + data_[i].velocity = link->dq(); + data_[i].effort = link->ddq(); + // Set commands by current data // + command_[i].position = data_[i].position; + command_[i].velocity = data_[i].velocity; + command_[i].effort = data_[i].effort; + + // Create joint state interface // + js_if_.registerHandle( + hi::JointStateHandle(joint_names_[i], &data_[i].position, &data_[i].velocity,&data_[i].effort)); + + // Set Control type from loaded cnoid::Link or hardware_interface // + if(link->actuationMode()){ + link->setActuationMode(link->actuationMode()); + switch(link->actuationMode()) { + case Link::JOINT_ANGLE: + ctrl_type_[i] = ControlType::POSITION; + joint_handle_ = hi::JointHandle( + js_if_.getHandle(joint_names_[i]), &command_[i].position); + pj_if_.registerHandle(joint_handle_); + break; + case Link::JOINT_VELOCITY: + case Link::JOINT_SURFACE_VELOCITY: + ctrl_type_[i] = ControlType::VELOCITY; + joint_handle_ = hi::JointHandle( + js_if_.getHandle(joint_names_[i]), &command_[i].velocity); + vj_if_.registerHandle(joint_handle_); + break; + case Link::JOINT_EFFORT: + ctrl_type_[i] = ControlType::EFFORT; + joint_handle_ = hi::JointHandle( + js_if_.getHandle(joint_names_[i]), &command_[i].effort); + ej_if_.registerHandle(joint_handle_); + break; + default: + break; + } + } else { + switch(if_map.at(joint_ifs[0])) { + case ControlType::POSITION : + ctrl_type_[i] = ControlType::POSITION; + joint_handle_ = hi::JointHandle( + js_if_.getHandle(joint_names_[i]), &command_[i].position); + pj_if_.registerHandle(joint_handle_); + link->setActuationMode(Link::JOINT_ANGLE); + break; + case ControlType::VELOCITY : + ctrl_type_[i] = ControlType::VELOCITY; + joint_handle_ = hi::JointHandle( + js_if_.getHandle(joint_names_[i]), &command_[i].velocity); + vj_if_.registerHandle(joint_handle_); + link->setActuationMode(Link::JOINT_VELOCITY); + break; + case ControlType::EFFORT : + ctrl_type_[i] = ControlType::EFFORT; + joint_handle_ = hi::JointHandle( + js_if_.getHandle(joint_names_[i]), &command_[i].effort); + ej_if_.registerHandle(joint_handle_); + link->setActuationMode(Link::JOINT_EFFORT); + break; + default: + break; + }; + } + + registerJointLimits(i); + + cout << joint_names_[i] << " : " << links_[i]->actuationModeString() << endl; + + // set first value // + link->q_target() = command_[i].position; + link->dq_target() = command_[i].velocity; + link->u() = command_[i].effort; + // links_[i]->q_target() = command_[i].position; + // links_[i]->dq_target() = command_[i].velocity; + // links_[i]->u() = command_[i].effort; + } + + // Register interfaces + registerInterface(&js_if_); + registerInterface(&pj_if_); + registerInterface(&vj_if_); + registerInterface(&ej_if_); + + return true; +} + +bool RobotHWCnoid::loadURDF(const std::string& param_name) +{ + using namespace std; + using namespace cnoid; + string urdf_string; + stringstream ss; + while(urdf_string.empty()) { + string searched_param; + if (nh_.searchParam(param_name, searched_param)) { + if(!nh_.getParam(searched_param, urdf_string)) { + ss.str(""); + ss << "Waiting for URDF model." << endl; + ss << "Finding parameter name : " << searched_param << endl; + MessageView::instance()->put(ss.str(), MessageView::Warning); + sleep(1); + } + } + else { + ss.str(""); + ss << "There are no parameter related to : " << param_name << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); + return false; + } + } + urdf_model_ = make_shared(); + urdf_model_->initString(urdf_string); + + ss.str(""); + ss << "RobotHWCnoid loaded the URDF model." << endl; + MessageView::instance()->put(ss.str(), MessageView::Normal); + + // load transmissions // + transmission_interface::TransmissionParser::parse(urdf_string, tmss_); + + ss.str(""); + ss << "RobotHWCnoid loaded Transmission." << endl; + MessageView::instance()->put(ss.str(), MessageView::Normal); + + return true; +} + +bool RobotHWCnoid::registerJointLimits(const uint& i) +{ + using namespace std; + using namespace cnoid; + stringstream ss; + + joint_types_[i] = urdf::Joint::UNKNOWN; + limits_[i].lower.position = -numeric_limits::max(); + limits_[i].upper.position = numeric_limits::max(); + limits_[i].lower.velocity = 0; + limits_[i].upper.velocity = numeric_limits::max(); + limits_[i].lower.effort = 0; + limits_[i].upper.effort = numeric_limits::max(); + joint_limits_interface::JointLimits limits; + bool has_limits = false; + joint_limits_interface::SoftJointLimits soft_limits; + bool has_soft_limits = false; + + if(urdf_model_ == nullptr) { + ss.str(""); + ss << "URDF model is NULL. Abort." << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); + return false; + } else { + const urdf::JointConstSharedPtr urdf_joint = + urdf_model_->getJoint(joint_names_[i]); + if (!urdf_joint) { + ss.str(""); + ss << "URDF model joint is NULL. Abort." << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); + return false; + } else { + joint_types_[i] = urdf_joint->type; + // Get limits from the URDF file. + if (joint_limits_interface::getJointLimits(urdf_joint, limits)) + has_limits = true; + if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) + has_soft_limits = true; + } + } + // Limits from parameter // + if (joint_limits_interface::getJointLimits(joint_names_[i], nh_, limits)) + has_limits = true; + + // return if there are no limits // + if (!has_limits) + return false; + + // Check joint_type // + if(joint_types_[i] == urdf::Joint::UNKNOWN) { + if(limits.has_position_limits) + joint_types_[i] = urdf::Joint::REVOLUTE; + else { + if(limits.angle_wraparound) + joint_types_[i] = urdf::Joint::CONTINUOUS; + else + joint_types_[i] = urdf::Joint::PRISMATIC; + } + } + + // set limits // + if(limits.has_position_limits) { + limits_[i].lower.position = limits.min_position; + limits_[i].upper.position = limits.max_position; + } + if(limits.has_velocity_limits) + limits_[i].upper.velocity = limits.max_velocity; + if(limits.has_effort_limits) + limits_[i].upper.effort = limits.max_effort; + + // soft limits // + if (has_soft_limits) { + switch (ctrl_type_[i]) { + case ControlType::POSITION : { + const joint_limits_interface::PositionJointSoftLimitsHandle + limits_handle(joint_handle_, limits, soft_limits); + pj_lim_if_.registerHandle(limits_handle); + break; + } + case ControlType::VELOCITY : { + const joint_limits_interface::VelocityJointSoftLimitsHandle + limits_handle(joint_handle_, limits, soft_limits); + vj_lim_if_.registerHandle(limits_handle); + break; + } + case ControlType::EFFORT : { + const joint_limits_interface::EffortJointSoftLimitsHandle + limits_handle(joint_handle_, limits, soft_limits); + ej_lim_if_.registerHandle(limits_handle); + break; + } + default: + break; + }; + } else { + switch (ctrl_type_[i]) { + case ControlType::POSITION : { + const joint_limits_interface::PositionJointSaturationHandle + sat_handle(joint_handle_, limits); + pj_sat_if_.registerHandle(sat_handle); + break; + } + case ControlType::VELOCITY : { + const joint_limits_interface::VelocityJointSaturationHandle + sat_handle(joint_handle_, limits); + vj_sat_if_.registerHandle(sat_handle); + break; + } + case ControlType::EFFORT : { + const joint_limits_interface::EffortJointSaturationHandle + sat_handle(joint_handle_, limits); + ej_sat_if_.registerHandle(sat_handle); + break; + } + default: + break; + }; + } + + return true; +} + + +void RobotHWCnoid::read(const ros::Time& time, const ros::Duration& period) +{ + using namespace std; + for(uint i=0; iq(); + data_[i].velocity = links_[i]->dq(); + data_[i].effort = links_[i]->u(); + } +} + +void RobotHWCnoid::write(const ros::Time& time, const ros::Duration& period) +{ + using namespace std; + for(uint i=0; i<2; i++) { + links_[i]->q_target() = command_[i].position; + // links_[i]->dq_target() = command_[i].velocity; + // links_[i]->u() = command_[i].effort; + } +} +} // namespace hardware_interface + +PLUGINLIB_EXPORT_CLASS(hardware_interface::RobotHWCnoid, hardware_interface::RobotHWSim) diff --git a/src/plugin/robot_hw_cnoid.h b/src/plugin/robot_hw_cnoid.h new file mode 100644 index 0000000..2365920 --- /dev/null +++ b/src/plugin/robot_hw_cnoid.h @@ -0,0 +1,93 @@ +/////////////////////////////////////////////////////////// +/// @file robot_hw_cnoid.h +/// @brief ros_control interface for Choreonoid +/// @author Ryodo Tanaka +/////////////////////////////////////////////////////////// + +#ifndef HARDWARE_INTERFACE_ROBOT_HW_CNOID_H +#define HARDWARE_INTERFACE_ROBOT_HW_CNOID_H + +// ROS // +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "robot_hw_sim.h" + +// STL // +#include +#include + +// Cnoid // +#include + +namespace hardware_interface +{ +class RobotHWCnoid : public RobotHWSim +{ + public: + virtual bool initSim(const ros::NodeHandle& nh, cnoid::ControllerIO* args) final; + virtual void read(const ros::Time& time, const ros::Duration& period) final; + virtual void write(const ros::Time& time, const ros::Duration& period) final; + + private: + + enum ControlType { POSITION, VELOCITY, EFFORT, POSITION_PID, VELOCITY_PID, EFFORT_PID }; + template struct Limits { T lower; T upper; }; + struct Data{ double position; double velocity; double effort; }; + + bool loadURDF(const std::string& param_name="robot_description"); + bool registerJointLimits(const uint& i); + + // arguments // + ros::NodeHandle nh_; + cnoid::ControllerIO* io_; + + // transmissions // + std::vector tmss_; + + // urdf model // + std::shared_ptr urdf_model_; + + // Joint Handle // + hardware_interface::JointHandle joint_handle_; + + // Interface // + hardware_interface::JointStateInterface js_if_; + hardware_interface::PositionJointInterface pj_if_; + hardware_interface::VelocityJointInterface vj_if_; + hardware_interface::EffortJointInterface ej_if_; + + // cnoid::Link // + std::vector links_; + + // Joint limits // + std::vector> limits_; + std::vector joint_types_; + joint_limits_interface::PositionJointSaturationInterface pj_sat_if_; + joint_limits_interface::PositionJointSoftLimitsInterface pj_lim_if_; + joint_limits_interface::VelocityJointSaturationInterface vj_sat_if_; + joint_limits_interface::VelocityJointSoftLimitsInterface vj_lim_if_; + joint_limits_interface::EffortJointSaturationInterface ej_sat_if_; + joint_limits_interface::EffortJointSoftLimitsInterface ej_lim_if_; + + uint dof_{0}; + std::vector joint_names_; + std::vector ctrl_type_; + std::vector data_; + std::vector command_; + std::unordered_map joint_map_; +}; + +typedef std::shared_ptr RobotHWCnoidPtr; +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE_ROBOT_HW_CNOID_H From 78f1d8d870de70c614db3e32306b87869a806e95 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 20:29:17 +0900 Subject: [PATCH 19/33] Update add dependencies --- CMakeLists.txt | 1 + package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index bf58c36..c1f613e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS transmission_interface joint_limits_interface urdf + angles ) # find_package(Qt5 COMPONENTS Widgets REQUIRED) diff --git a/package.xml b/package.xml index f995d40..8c92b14 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,7 @@ transmission_interface joint_limits_interface urdf + angles cmake From 57910d2386768e56817167563b2f4bf7fdcb464c Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 20:29:34 +0900 Subject: [PATCH 20/33] Update controller manager to be update for every control() loop --- src/plugin/ROSControlItem.cpp | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp index 9cc7263..0a8f951 100644 --- a/src/plugin/ROSControlItem.cpp +++ b/src/plugin/ROSControlItem.cpp @@ -127,11 +127,6 @@ bool ROSControlItem::start(void) ss.str(""); ss << "Loaded cnoid::ROSControlItem" << endl; MessageView::instance()->put(ss.str(), MessageView::Normal); - - cnoid::Link* turret_y = io_->body()->link("TURRET_Y"); - turret_y->setActuationMode(turret_y->actuationMode()); - cnoid::Link* turret_p = io_->body()->link("TURRET_P"); - turret_p->setActuationMode(turret_p->actuationMode()); return true; } @@ -150,14 +145,11 @@ bool ROSControlItem::control(void) ros::Time now(now_sec, static_cast(now_nsec)); ros::Duration period = now - last; - cnoid::Link* turret_y = io_->body()->link("TURRET_Y"); - cnoid::Link* turret_p = io_->body()->link("TURRET_P"); + rbt_hw_sim_->read(now, period); + rbt_hw_sim_->write(now, period); + + manager_->update(now, period, false); - turret_y->q_target() = 0; - turret_p->q_target() = 0; - - // rbt_hw_sim_->read(now, period); - // rbt_hw_sim_->write(now, period); time_ = io_->currentTime(); } From fcc3bf7333051d3cb002ec05478950d4aab786fa Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 20:30:13 +0900 Subject: [PATCH 21/33] Update to utilize ctrl_types instead of ctrl_type --- src/plugin/robot_hw_cnoid.cpp | 18 +++++++++--------- src/plugin/robot_hw_cnoid.h | 2 +- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/plugin/robot_hw_cnoid.cpp b/src/plugin/robot_hw_cnoid.cpp index f678cb7..6398380 100644 --- a/src/plugin/robot_hw_cnoid.cpp +++ b/src/plugin/robot_hw_cnoid.cpp @@ -29,7 +29,7 @@ bool RobotHWCnoid::initSim(const ros::NodeHandle& nh, cnoid::ControllerIO* args) dof_ = tmss_.size(); joint_names_.resize(dof_); - ctrl_type_.resize(dof_); + ctrl_types_.resize(dof_); data_.resize(dof_); command_.resize(dof_); @@ -102,20 +102,20 @@ bool RobotHWCnoid::initSim(const ros::NodeHandle& nh, cnoid::ControllerIO* args) link->setActuationMode(link->actuationMode()); switch(link->actuationMode()) { case Link::JOINT_ANGLE: - ctrl_type_[i] = ControlType::POSITION; + ctrl_types_[i] = ControlType::POSITION; joint_handle_ = hi::JointHandle( js_if_.getHandle(joint_names_[i]), &command_[i].position); pj_if_.registerHandle(joint_handle_); break; case Link::JOINT_VELOCITY: case Link::JOINT_SURFACE_VELOCITY: - ctrl_type_[i] = ControlType::VELOCITY; + ctrl_types_[i] = ControlType::VELOCITY; joint_handle_ = hi::JointHandle( js_if_.getHandle(joint_names_[i]), &command_[i].velocity); vj_if_.registerHandle(joint_handle_); break; case Link::JOINT_EFFORT: - ctrl_type_[i] = ControlType::EFFORT; + ctrl_types_[i] = ControlType::EFFORT; joint_handle_ = hi::JointHandle( js_if_.getHandle(joint_names_[i]), &command_[i].effort); ej_if_.registerHandle(joint_handle_); @@ -126,21 +126,21 @@ bool RobotHWCnoid::initSim(const ros::NodeHandle& nh, cnoid::ControllerIO* args) } else { switch(if_map.at(joint_ifs[0])) { case ControlType::POSITION : - ctrl_type_[i] = ControlType::POSITION; + ctrl_types_[i] = ControlType::POSITION; joint_handle_ = hi::JointHandle( js_if_.getHandle(joint_names_[i]), &command_[i].position); pj_if_.registerHandle(joint_handle_); link->setActuationMode(Link::JOINT_ANGLE); break; case ControlType::VELOCITY : - ctrl_type_[i] = ControlType::VELOCITY; + ctrl_types_[i] = ControlType::VELOCITY; joint_handle_ = hi::JointHandle( js_if_.getHandle(joint_names_[i]), &command_[i].velocity); vj_if_.registerHandle(joint_handle_); link->setActuationMode(Link::JOINT_VELOCITY); break; case ControlType::EFFORT : - ctrl_type_[i] = ControlType::EFFORT; + ctrl_types_[i] = ControlType::EFFORT; joint_handle_ = hi::JointHandle( js_if_.getHandle(joint_names_[i]), &command_[i].effort); ej_if_.registerHandle(joint_handle_); @@ -286,7 +286,7 @@ bool RobotHWCnoid::registerJointLimits(const uint& i) // soft limits // if (has_soft_limits) { - switch (ctrl_type_[i]) { + switch (ctrl_types_[i]) { case ControlType::POSITION : { const joint_limits_interface::PositionJointSoftLimitsHandle limits_handle(joint_handle_, limits, soft_limits); @@ -309,7 +309,7 @@ bool RobotHWCnoid::registerJointLimits(const uint& i) break; }; } else { - switch (ctrl_type_[i]) { + switch (ctrl_types_[i]) { case ControlType::POSITION : { const joint_limits_interface::PositionJointSaturationHandle sat_handle(joint_handle_, limits); diff --git a/src/plugin/robot_hw_cnoid.h b/src/plugin/robot_hw_cnoid.h index 2365920..f536e24 100644 --- a/src/plugin/robot_hw_cnoid.h +++ b/src/plugin/robot_hw_cnoid.h @@ -81,7 +81,7 @@ class RobotHWCnoid : public RobotHWSim uint dof_{0}; std::vector joint_names_; - std::vector ctrl_type_; + std::vector ctrl_types_; std::vector data_; std::vector command_; std::unordered_map joint_map_; From 14ad5b54c3fee7229926e65219120355e296a580 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Fri, 4 Sep 2020 20:30:31 +0900 Subject: [PATCH 22/33] Update to implement read() write() function --- src/plugin/robot_hw_cnoid.cpp | 52 ++++++++++++++++++++++------------- 1 file changed, 33 insertions(+), 19 deletions(-) diff --git a/src/plugin/robot_hw_cnoid.cpp b/src/plugin/robot_hw_cnoid.cpp index 6398380..82bfa58 100644 --- a/src/plugin/robot_hw_cnoid.cpp +++ b/src/plugin/robot_hw_cnoid.cpp @@ -5,6 +5,7 @@ #include "robot_hw_cnoid.h" // ROS // #include +#include // Cnoid // #include // STL // @@ -152,16 +153,6 @@ bool RobotHWCnoid::initSim(const ros::NodeHandle& nh, cnoid::ControllerIO* args) } registerJointLimits(i); - - cout << joint_names_[i] << " : " << links_[i]->actuationModeString() << endl; - - // set first value // - link->q_target() = command_[i].position; - link->dq_target() = command_[i].velocity; - link->u() = command_[i].effort; - // links_[i]->q_target() = command_[i].position; - // links_[i]->dq_target() = command_[i].velocity; - // links_[i]->u() = command_[i].effort; } // Register interfaces @@ -339,21 +330,44 @@ bool RobotHWCnoid::registerJointLimits(const uint& i) void RobotHWCnoid::read(const ros::Time& time, const ros::Duration& period) { - using namespace std; for(uint i=0; iq(); + double position = links_[i]->q(); + if (joint_types_[i] == urdf::Joint::PRISMATIC) + data_[i].position = position; + else + data_[i].position += angles::shortest_angular_distance(data_[i].position, position); + data_[i].velocity = links_[i]->dq(); - data_[i].effort = links_[i]->u(); + data_[i].effort = links_[i]->ddq(); } } void RobotHWCnoid::write(const ros::Time& time, const ros::Duration& period) -{ - using namespace std; - for(uint i=0; i<2; i++) { - links_[i]->q_target() = command_[i].position; - // links_[i]->dq_target() = command_[i].velocity; - // links_[i]->u() = command_[i].effort; +{ + pj_sat_if_.enforceLimits(period); + pj_lim_if_.enforceLimits(period); + vj_sat_if_.enforceLimits(period); + vj_lim_if_.enforceLimits(period); + ej_sat_if_.enforceLimits(period); + ej_lim_if_.enforceLimits(period); + + for(uint i=0; iq_target() = command_[i].position; + break; + } + case ControlType::VELOCITY: { + links_[i]->dq_target() = command_[i].velocity; + break; + } + case ControlType::EFFORT: { + links_[i]->u() = command_[i].effort; + break; + } + default: + break; + }; } } } // namespace hardware_interface From 2e3c11007e934696e5b9be104fdd57ac8c575635 Mon Sep 17 00:00:00 2001 From: Ryodo Tanaka Date: Tue, 29 Sep 2020 15:45:01 +0900 Subject: [PATCH 23/33] Fix to remove not existing program to install --- src/plugin/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/plugin/CMakeLists.txt b/src/plugin/CMakeLists.txt index bf8a70e..0c268c5 100644 --- a/src/plugin/CMakeLists.txt +++ b/src/plugin/CMakeLists.txt @@ -6,7 +6,7 @@ choreonoid_make_gettext_mo_files(${target} mofiles) add_library(RobotHWCnoid robot_hw_cnoid.cpp) target_link_libraries(RobotHWCnoid ${catkin_LIBRARIES} ${CHOREONOID_BODY_LIBRARIES} ${CHOREONOID_BODY_PLUGIN_LIBRARIES}) -install(TARGETS ${PROJECT_NAME} RobotHWCnoid +install(TARGETS RobotHWCnoid LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} From 018f114917140716f8627410150ec557e0e1aa3f Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Sun, 23 May 2021 19:51:25 +0900 Subject: [PATCH 24/33] Update for controller item --- src/plugin/ROSControlItem.cpp | 124 +++++++++++++++++++++++++--------- src/plugin/ROSControlItem.h | 2 + 2 files changed, 94 insertions(+), 32 deletions(-) diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp index 0a8f951..e28dad4 100644 --- a/src/plugin/ROSControlItem.cpp +++ b/src/plugin/ROSControlItem.cpp @@ -3,6 +3,9 @@ ///////////////////////////////// #include "ROSControlItem.h" +#include +#include +#include #include #include #include @@ -67,6 +70,8 @@ bool ROSControlItem::initialize(ControllerIO* io) { using namespace std; using namespace cnoid; + using namespace hardware_interface; + stringstream ss; // Check body // @@ -93,40 +98,94 @@ bool ROSControlItem::initialize(ControllerIO* io) body_ = io->body(); tstep_ = io->worldTimeStep(); time_ = io->currentTime(); - - return true; -} - -bool ROSControlItem::start(void) -{ - using namespace std; - using namespace cnoid; - using namespace hardware_interface; - stringstream ss; - + + // ROS Initialize nh_ = ros::NodeHandle(namespace_); try { // ros plugin loader // - rbt_hw_sim_loader_ = make_shared>>("choreonoid_ros", "hardware_interface::RobotHWSim"); - rbt_hw_sim_ = rbt_hw_sim_loader_->createInstance("hardware_interface/RobotHWCnoid"); - // load hardware_interface // - if(!rbt_hw_sim_->initSim(nh_, io_)) { - ss.str(""); - ss << "Could not initialize robot simulation interface" << endl; - MessageView::instance()->put(ss.str(), MessageView::Error); + if(rbt_hw_sim_loader_ == nullptr) + rbt_hw_sim_loader_ = make_shared>>("choreonoid_ros", "hardware_interface::RobotHWSim"); + if(rbt_hw_sim_ == nullptr) { + rbt_hw_sim_ = rbt_hw_sim_loader_->createInstance("hardware_interface/RobotHWCnoid"); + // load hardware_interface // + if(!rbt_hw_sim_->initSim(nh_, io_)) { + ss.str(""); + ss << "Could not initialize robot simulation interface" << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); + } } // register ros control manager // - manager_ = make_shared(rbt_hw_sim_.get(), nh_); + if(manager_ == nullptr) + manager_ = make_shared(rbt_hw_sim_.get(), nh_); + else { + for(auto c : controller_names_) { + ros::ServiceClient sc = nh_.serviceClient("controller_manager/load_controller"); + controller_manager_msgs::LoadController load; + load.request.name = c; + sc.call(load); + } + } } catch(pluginlib::LibraryLoadException &ex) { ss.str(""); ss << "Failed to create robot simulation interface loader : " << ex.what() << endl; MessageView::instance()->put(ss.str(), MessageView::Error); } - + + ros::ServiceClient lcsc = nh_.serviceClient("controller_manager/list_controllers"); + controller_manager_msgs::ListControllers lc; + if(!lcsc.call(lc)) { + ss.str(""); + ss << "Failed to lad cnoid::ROSControllerItem" << endl; + MessageView::instance()->put(ss.str(), MessageView::Normal); + } else { + for(auto c : lc.response.controller) { + controller_names_.emplace_back(c.name); + cout << c << endl; + } + } + ss.str(""); ss << "Loaded cnoid::ROSControlItem" << endl; MessageView::instance()->put(ss.str(), MessageView::Normal); + + std::cout << "You are in initialize()" << std::endl; + + return true; +} + +bool ROSControlItem::start(void) +{ + std::cout << "You are in start()" << std::endl; + + // using namespace std; + // using namespace cnoid; + // using namespace hardware_interface; + // stringstream ss; + + // nh_ = ros::NodeHandle(namespace_); + + // try { + // // ros plugin loader // + // rbt_hw_sim_loader_ = make_shared>>("choreonoid_ros", "hardware_interface::RobotHWSim"); + // rbt_hw_sim_ = rbt_hw_sim_loader_->createInstance("hardware_interface/RobotHWCnoid"); + // // load hardware_interface // + // if(!rbt_hw_sim_->initSim(nh_, io_)) { + // ss.str(""); + // ss << "Could not initialize robot simulation interface" << endl; + // MessageView::instance()->put(ss.str(), MessageView::Error); + // } + // // register ros control manager // + // manager_ = make_shared(rbt_hw_sim_.get(), nh_); + // } catch(pluginlib::LibraryLoadException &ex) { + // ss.str(""); + // ss << "Failed to create robot simulation interface loader : " << ex.what() << endl; + // MessageView::instance()->put(ss.str(), MessageView::Error); + // } + + // ss.str(""); + // ss << "Loaded cnoid::ROSControlItem" << endl; + // MessageView::instance()->put(ss.str(), MessageView::Normal); return true; } @@ -156,22 +215,23 @@ bool ROSControlItem::control(void) void ROSControlItem::output(void) { - // int last_sec = static_cast(time_); - // double last_nsec = (time_ - last_sec) * 1e9; - // int now_sec = static_cast(io_->currentTime()); - // double now_nsec = (io_->currentTime() - now_sec) * 1e9; - - // ros::Time last(last_sec, static_cast(last_nsec)); - // ros::Time now(now_sec, static_cast(now_nsec)); - // ros::Duration period = now - last; - - // rbt_hw_sim_->write(now, period); - - // time_ = io_->currentTime(); } void ROSControlItem::stop(void) { + std::cout << "You are in stop()" << std::endl; + + for(auto c : controller_names_) { + ros::ServiceClient sc = nh_.serviceClient("controller_manager/unload_controller"); + controller_manager_msgs::UnloadController unload; + unload.request.name = c; + sc.call(unload); + } + + + rbt_hw_sim_.reset(); + rbt_hw_sim_loader_.reset(); +// manager_.reset(); } } // namespace cnoid diff --git a/src/plugin/ROSControlItem.h b/src/plugin/ROSControlItem.h index efac826..ab0bf9d 100644 --- a/src/plugin/ROSControlItem.h +++ b/src/plugin/ROSControlItem.h @@ -61,8 +61,10 @@ class ROSControlItem : public ControllerItem std::shared_ptr>> rbt_hw_sim_loader_; boost::shared_ptr> rbt_hw_sim_; std::shared_ptr manager_; + std::vector controller_names_; std::string namespace_{""}; + }; typedef ref_ptr ROSControlItemPtr; From e1983111bacea4e4191b16a19a1ee3656fddf9e1 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Mon, 24 May 2021 21:29:28 +0900 Subject: [PATCH 25/33] Update to utilize library directly --- src/plugin/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/plugin/CMakeLists.txt b/src/plugin/CMakeLists.txt index 1030ebd..11811b7 100644 --- a/src/plugin/CMakeLists.txt +++ b/src/plugin/CMakeLists.txt @@ -3,8 +3,8 @@ set(target CnoidROSPlugin) choreonoid_make_gettext_mo_files(${target} mofiles) # RobotHW inherited ros_control Choreonoid interface class # -add_library(RobotHWCnoid robot_hw_cnoid.cpp) -target_link_libraries(RobotHWCnoid ${catkin_LIBRARIES} ${CHOREONOID_BODY_LIBRARIES} ${CHOREONOID_BODY_PLUGIN_LIBRARIES}) +add_library(RobotHWCnoid robot_hw_sim.h robot_hw_cnoid.cpp) +target_link_libraries(RobotHWCnoid ${catkin_LIBRARIES} Choreonoid::CnoidBody Choreonoid::CnoidBodyPlugin) install(TARGETS RobotHWCnoid LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} From 142133f3f47b811b6d31c1d97d113a5c2785b8df Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Mon, 24 May 2021 21:30:04 +0900 Subject: [PATCH 26/33] Update to add catkin depends --- src/plugin/CMakeLists.txt | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/plugin/CMakeLists.txt b/src/plugin/CMakeLists.txt index 11811b7..b0a5740 100644 --- a/src/plugin/CMakeLists.txt +++ b/src/plugin/CMakeLists.txt @@ -28,12 +28,8 @@ target_link_libraries(${target} ${std_msgs_LIBRARIES} ${sensor_msgs_LIBRARIES} ${image_transport_LIBRARIES} - # <<<<<<< HEAD - # ${CHOREONOID_BODY_PLUGIN_LIBRARIES} - # ${catkin_LIBRARIES} - # ======= Choreonoid::CnoidBodyPlugin - # >>>>>>> choreonoid/master + ${catkin_LIBRARIES} ) if(CHOREONOID_ENABLE_PYTHON) From 3c9613669764cc709836a6e84acd3d8d7753de6d Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Mon, 24 May 2021 21:30:17 +0900 Subject: [PATCH 27/33] Fix typo --- src/plugin/ROSControlItem.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp index e28dad4..69605ee 100644 --- a/src/plugin/ROSControlItem.cpp +++ b/src/plugin/ROSControlItem.cpp @@ -93,7 +93,7 @@ bool ROSControlItem::initialize(ControllerIO* io) return false; } - // Copy elements to the local arfuments // + // Copy elements to the local arguments // io_ = io; body_ = io->body(); tstep_ = io->worldTimeStep(); From f9eccadddbd776bb9cfce15afe7338e2ff16e8f3 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Mon, 24 May 2021 21:31:05 +0900 Subject: [PATCH 28/33] Update to delete unnecessary part --- src/plugin/ROSControlItem.cpp | 78 ----------------------------------- 1 file changed, 78 deletions(-) diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp index 69605ee..0d5c93d 100644 --- a/src/plugin/ROSControlItem.cpp +++ b/src/plugin/ROSControlItem.cpp @@ -104,88 +104,23 @@ bool ROSControlItem::initialize(ControllerIO* io) try { // ros plugin loader // - if(rbt_hw_sim_loader_ == nullptr) - rbt_hw_sim_loader_ = make_shared>>("choreonoid_ros", "hardware_interface::RobotHWSim"); - if(rbt_hw_sim_ == nullptr) { - rbt_hw_sim_ = rbt_hw_sim_loader_->createInstance("hardware_interface/RobotHWCnoid"); - // load hardware_interface // - if(!rbt_hw_sim_->initSim(nh_, io_)) { - ss.str(""); - ss << "Could not initialize robot simulation interface" << endl; - MessageView::instance()->put(ss.str(), MessageView::Error); - } } // register ros control manager // - if(manager_ == nullptr) - manager_ = make_shared(rbt_hw_sim_.get(), nh_); - else { - for(auto c : controller_names_) { - ros::ServiceClient sc = nh_.serviceClient("controller_manager/load_controller"); - controller_manager_msgs::LoadController load; - load.request.name = c; - sc.call(load); - } - } } catch(pluginlib::LibraryLoadException &ex) { ss.str(""); ss << "Failed to create robot simulation interface loader : " << ex.what() << endl; MessageView::instance()->put(ss.str(), MessageView::Error); } - ros::ServiceClient lcsc = nh_.serviceClient("controller_manager/list_controllers"); - controller_manager_msgs::ListControllers lc; - if(!lcsc.call(lc)) { - ss.str(""); - ss << "Failed to lad cnoid::ROSControllerItem" << endl; - MessageView::instance()->put(ss.str(), MessageView::Normal); - } else { - for(auto c : lc.response.controller) { - controller_names_.emplace_back(c.name); - cout << c << endl; - } - } - ss.str(""); ss << "Loaded cnoid::ROSControlItem" << endl; MessageView::instance()->put(ss.str(), MessageView::Normal); - - std::cout << "You are in initialize()" << std::endl; return true; } bool ROSControlItem::start(void) { - std::cout << "You are in start()" << std::endl; - - // using namespace std; - // using namespace cnoid; - // using namespace hardware_interface; - // stringstream ss; - - // nh_ = ros::NodeHandle(namespace_); - - // try { - // // ros plugin loader // - // rbt_hw_sim_loader_ = make_shared>>("choreonoid_ros", "hardware_interface::RobotHWSim"); - // rbt_hw_sim_ = rbt_hw_sim_loader_->createInstance("hardware_interface/RobotHWCnoid"); - // // load hardware_interface // - // if(!rbt_hw_sim_->initSim(nh_, io_)) { - // ss.str(""); - // ss << "Could not initialize robot simulation interface" << endl; - // MessageView::instance()->put(ss.str(), MessageView::Error); - // } - // // register ros control manager // - // manager_ = make_shared(rbt_hw_sim_.get(), nh_); - // } catch(pluginlib::LibraryLoadException &ex) { - // ss.str(""); - // ss << "Failed to create robot simulation interface loader : " << ex.what() << endl; - // MessageView::instance()->put(ss.str(), MessageView::Error); - // } - - // ss.str(""); - // ss << "Loaded cnoid::ROSControlItem" << endl; - // MessageView::instance()->put(ss.str(), MessageView::Normal); return true; } @@ -219,19 +154,6 @@ void ROSControlItem::output(void) void ROSControlItem::stop(void) { - std::cout << "You are in stop()" << std::endl; - - for(auto c : controller_names_) { - ros::ServiceClient sc = nh_.serviceClient("controller_manager/unload_controller"); - controller_manager_msgs::UnloadController unload; - unload.request.name = c; - sc.call(unload); - } - - - rbt_hw_sim_.reset(); - rbt_hw_sim_loader_.reset(); -// manager_.reset(); } } // namespace cnoid From 01a1334bf2dbeafd9c89d99b511ef97a7c189e18 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Mon, 24 May 2021 21:31:42 +0900 Subject: [PATCH 29/33] Update initialization materials --- src/plugin/ROSControlItem.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp index 0d5c93d..83b69ca 100644 --- a/src/plugin/ROSControlItem.cpp +++ b/src/plugin/ROSControlItem.cpp @@ -104,12 +104,25 @@ bool ROSControlItem::initialize(ControllerIO* io) try { // ros plugin loader // + rbt_hw_sim_loader_ = make_shared>>("choreonoid_ros", "hardware_interface::RobotHWSim"); + + // load RobotHWCnoid plugin + rbt_hw_sim_ = rbt_hw_sim_loader_->createInstance("hardware_interface/RobotHWCnoid"); + + // load hardware_interface // + if(!rbt_hw_sim_->initSim(nh_, io_)) { + ss.str(""); + ss << "Could not initialize robot simulation interface" << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); } + // register ros control manager // + manager_ = make_shared(rbt_hw_sim_.get(), nh_); } catch(pluginlib::LibraryLoadException &ex) { ss.str(""); ss << "Failed to create robot simulation interface loader : " << ex.what() << endl; MessageView::instance()->put(ss.str(), MessageView::Error); + return false; } ss.str(""); @@ -121,7 +134,6 @@ bool ROSControlItem::initialize(ControllerIO* io) bool ROSControlItem::start(void) { - return true; } From d48c5d1fc124d3e05a30e03996f01145f2859295 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Mon, 24 May 2021 21:31:58 +0900 Subject: [PATCH 30/33] Fix to return true --- src/plugin/ROSControlItem.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp index 83b69ca..2a6b419 100644 --- a/src/plugin/ROSControlItem.cpp +++ b/src/plugin/ROSControlItem.cpp @@ -155,9 +155,10 @@ bool ROSControlItem::control(void) rbt_hw_sim_->write(now, period); manager_->update(now, period, false); - time_ = io_->currentTime(); + + return true; } void ROSControlItem::output(void) From 141140a4676becd048173969e3e1f5fb0fb31bc8 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Mon, 24 May 2021 21:32:26 +0900 Subject: [PATCH 31/33] Update to move header from h to cpp file --- src/plugin/robot_hw_cnoid.cpp | 2 +- src/plugin/robot_hw_cnoid.h | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/plugin/robot_hw_cnoid.cpp b/src/plugin/robot_hw_cnoid.cpp index 82bfa58..89fe830 100644 --- a/src/plugin/robot_hw_cnoid.cpp +++ b/src/plugin/robot_hw_cnoid.cpp @@ -6,6 +6,7 @@ // ROS // #include #include +#include // Cnoid // #include // STL // @@ -371,5 +372,4 @@ void RobotHWCnoid::write(const ros::Time& time, const ros::Duration& period) } } } // namespace hardware_interface - PLUGINLIB_EXPORT_CLASS(hardware_interface::RobotHWCnoid, hardware_interface::RobotHWSim) diff --git a/src/plugin/robot_hw_cnoid.h b/src/plugin/robot_hw_cnoid.h index f536e24..4fbc13d 100644 --- a/src/plugin/robot_hw_cnoid.h +++ b/src/plugin/robot_hw_cnoid.h @@ -18,7 +18,6 @@ #include #include #include -#include #include "robot_hw_sim.h" @@ -37,7 +36,7 @@ class RobotHWCnoid : public RobotHWSim virtual bool initSim(const ros::NodeHandle& nh, cnoid::ControllerIO* args) final; virtual void read(const ros::Time& time, const ros::Duration& period) final; virtual void write(const ros::Time& time, const ros::Duration& period) final; - + private: enum ControlType { POSITION, VELOCITY, EFFORT, POSITION_PID, VELOCITY_PID, EFFORT_PID }; From 9c35dc451b8316bebd3fd08a6eb5469d6b38c26d Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Mon, 24 May 2021 22:03:37 +0900 Subject: [PATCH 32/33] Update for overroad function --- src/plugin/robot_hw_sim.h | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/plugin/robot_hw_sim.h b/src/plugin/robot_hw_sim.h index 7834937..763e553 100644 --- a/src/plugin/robot_hw_sim.h +++ b/src/plugin/robot_hw_sim.h @@ -16,12 +16,9 @@ template class RobotHWSim : public RobotHW { public: - RobotHWSim(void) = default; - ~RobotHWSim(void) = default; - virtual bool initSim(const ros::NodeHandle& nh, Ts... args) = 0; - virtual void read(const ros::Time& time, const ros::Duration& period) override; - virtual void write(const ros::Time& time, const ros::Duration& period) override; + virtual void read(const ros::Time& time, const ros::Duration& period) override = 0; + virtual void write(const ros::Time& time, const ros::Duration& period) override = 0; }; } // namespace hardware_interface From 44189cd4bed410d8188fd908c9c57ec690cf61b7 Mon Sep 17 00:00:00 2001 From: RyodoTanaka Date: Wed, 26 May 2021 04:17:19 +0900 Subject: [PATCH 33/33] Update to avoid nullptr --- src/plugin/ROSControlItem.cpp | 29 ++++++++++++++++------------- src/plugin/ROSControlItem.h | 1 - 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/plugin/ROSControlItem.cpp b/src/plugin/ROSControlItem.cpp index 2a6b419..e1b92ab 100644 --- a/src/plugin/ROSControlItem.cpp +++ b/src/plugin/ROSControlItem.cpp @@ -3,9 +3,6 @@ ///////////////////////////////// #include "ROSControlItem.h" -#include -#include -#include #include #include #include @@ -37,7 +34,7 @@ ROSControlItem::ROSControlItem(const ROSControlItem& org) : ControllerItem(org) ROSControlItem::~ROSControlItem() { - stop(); + //stop(); } Item* ROSControlItem::doDuplicate(void) const @@ -104,20 +101,25 @@ bool ROSControlItem::initialize(ControllerIO* io) try { // ros plugin loader // - rbt_hw_sim_loader_ = make_shared>>("choreonoid_ros", "hardware_interface::RobotHWSim"); + if(!rbt_hw_sim_loader_) + rbt_hw_sim_loader_ = make_shared>>("choreonoid_ros", "hardware_interface::RobotHWSim"); // load RobotHWCnoid plugin - rbt_hw_sim_ = rbt_hw_sim_loader_->createInstance("hardware_interface/RobotHWCnoid"); + if(!rbt_hw_sim_) { + rbt_hw_sim_ = rbt_hw_sim_loader_->createInstance("hardware_interface/RobotHWCnoid"); - // load hardware_interface // - if(!rbt_hw_sim_->initSim(nh_, io_)) { - ss.str(""); - ss << "Could not initialize robot simulation interface" << endl; - MessageView::instance()->put(ss.str(), MessageView::Error); + // load hardware_interface // + if(!rbt_hw_sim_->initSim(nh_, io_)) { + ss.str(""); + ss << "Could not initialize robot simulation interface" << endl; + MessageView::instance()->put(ss.str(), MessageView::Error); + } } // register ros control manager // - manager_ = make_shared(rbt_hw_sim_.get(), nh_); + if(!manager_) + manager_ = make_shared(rbt_hw_sim_.get(), nh_); + } catch(pluginlib::LibraryLoadException &ex) { ss.str(""); ss << "Failed to create robot simulation interface loader : " << ex.what() << endl; @@ -133,13 +135,14 @@ bool ROSControlItem::initialize(ControllerIO* io) } bool ROSControlItem::start(void) -{ +{ return true; } void ROSControlItem::input(void) { } + bool ROSControlItem::control(void) { int last_sec = static_cast(time_); diff --git a/src/plugin/ROSControlItem.h b/src/plugin/ROSControlItem.h index ab0bf9d..87fb682 100644 --- a/src/plugin/ROSControlItem.h +++ b/src/plugin/ROSControlItem.h @@ -61,7 +61,6 @@ class ROSControlItem : public ControllerItem std::shared_ptr>> rbt_hw_sim_loader_; boost::shared_ptr> rbt_hw_sim_; std::shared_ptr manager_; - std::vector controller_names_; std::string namespace_{""};