Add ROSControlItem

CMakeLists.txt

@@ -13,6 +13,14 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   image_transport
   choreonoid
+  controller_manager
+  control_toolbox
+  pluginlib
+  hardware_interface
+  transmission_interface
+  joint_limits_interface
+  urdf
+  angles
   )
 
 catkin_package(SKIP_CMAKE_CONFIG_GENERATION SKIP_PKG_CONFIG_GENERATION)

package.xml

@@ -14,7 +14,17 @@
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>image_transport</depend>
+
+  <depend>controller_manager</depend>
+  <depend>control_toolbox</depend>
+  <depend>pluginlib</depend>
+  <depend>hardware_interface</depend>
+  <depend>transmission_interface</depend>
+  <depend>joint_limits_interface</depend>
+  <depend>urdf</depend>
+  <depend>angles</depend>
   <export>
     <build_type>cmake</build_type>
+    <choreonoid_ros plugin="${prefix}/plugins.xml"/>
   </export>
 </package>

plugins.xml

@@ -0,0 +1,10 @@
+<library path="lib/libRobotHWCnoid">
+  <class
+      name="hardware_interface/RobotHWCnoid"
+      type="hardware_interface::RobotHWCnoid"
+      base_class_type="hardware_interface::RobotHWSim<cnoid::ControllerIO*>">
+  <description>
+    A default robot simulation interface which constructs joint handles from an SDF/URDF.
+  </description>
+</class>
+</library>

src/plugin/CMakeLists.txt

@@ -2,10 +2,23 @@ set(target CnoidROSPlugin)
 
 choreonoid_make_gettext_mo_files(${target} mofiles)
 
+# RobotHW inherited ros_control Choreonoid interface class #
+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}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
+
+# Choreonoid Plugins #
 choreonoid_add_plugin(${target}
   ROSPlugin.cpp
   WorldROSItem.cpp
   BodyROSItem.cpp
+  robot_hw_sim.h
+  ROSControlItem.cpp
   deprecated/BodyPublisherItem.cpp
   ${mofiles}
   )
@@ -16,6 +29,7 @@ target_link_libraries(${target}
   ${sensor_msgs_LIBRARIES}
   ${image_transport_LIBRARIES}
   Choreonoid::CnoidBodyPlugin
+  ${catkin_LIBRARIES}
   )
 
 if(CHOREONOID_ENABLE_PYTHON)

src/plugin/ROSControlItem.cpp

@@ -0,0 +1,175 @@
+/////////////////////////////////
+/// @author Ryodo Tanaka
+/////////////////////////////////
+
+#include "ROSControlItem.h"
+#include <cnoid/MessageView>
+#include <cnoid/PutPropertyFunction>
+#include <cnoid/BodyItem>
+#include <cnoid/ItemManager>
+#include <cnoid/Archive>
+#include <sstream>
+#include <fmt/format.h>
+#include "gettext.h"
+
+namespace cnoid
+{
+void ROSControlItem::initializeClass(ExtensionManager* ext)
+{
+  using namespace std;
+  using fmt::format;
+  ext->itemManager().registerClass<ROSControlItem>(N_("ROSControlItem"));
+  ext->itemManager().addCreationPanel<ROSControlItem>();
+}
+
+ROSControlItem::ROSControlItem(void)
+{
+  io_ = nullptr;
+}
+
+ROSControlItem::ROSControlItem(const ROSControlItem& org) : ControllerItem(org)
+{
+  io_ = nullptr;
+}
+
+ROSControlItem::~ROSControlItem()
+{
+  //stop();
+}
+
+Item* ROSControlItem::doDuplicate(void) const
+{
+  return new ROSControlItem(*this);
+}
+
+bool ROSControlItem::store(Archive& archive)
+{
+  archive.write("name space", namespace_);
+
+  return true;
+}
+
+bool ROSControlItem::restore(const Archive& archive)
+{
+  if(!archive.read("name space", namespace_))
+    archive.read("name space", namespace_);
+
+  return true;
+}
+
+void ROSControlItem::doPutProperties(PutPropertyFunction& putProperty)
+{
+  putProperty("Name space",
+    namespace_, changeProperty(namespace_));
+}
+
+bool ROSControlItem::initialize(ControllerIO* io)
+{
+  using namespace std;
+  using namespace cnoid;
+  using namespace hardware_interface;
+
+  stringstream ss;
+
+  // Check body //
+  if (!io->body()) {
+    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);
+
+    return false;
+  }
+
+  // Check that ROS has been initialized
+  if(!ros::isInitialized()) {
+    ss.str("");
+    ss << "ROS master is not initialized." << endl;
+    MessageView::instance()->put(ss.str(), MessageView::Warning);
+
+    return false;
+  }
+
+  // Copy elements to the local arguments //
+  io_ = io;
+  body_ = io->body();
+  tstep_ = io->worldTimeStep();
+  time_ = io->currentTime();
+
+  // ROS Initialize
+  nh_ = ros::NodeHandle(namespace_);
+
+  try {
+    // ros plugin loader //
+    if(!rbt_hw_sim_loader_)
+      rbt_hw_sim_loader_ = make_shared<pluginlib::ClassLoader<RobotHWSim<cnoid::ControllerIO*>>>("choreonoid_ros", "hardware_interface::RobotHWSim<cnoid::ControllerIO*>");
+
+    // load RobotHWCnoid plugin
+    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);
+      }
+    }
+
+    // register ros control manager //
+    if(!manager_)
+      manager_ = make_shared<controller_manager::ControllerManager>(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("");
+  ss << "Loaded cnoid::ROSControlItem" << endl;
+  MessageView::instance()->put(ss.str(), MessageView::Normal);
+
+  return true;
+}
+
+bool ROSControlItem::start(void)
+{  
+  return true;
+}
+
+void ROSControlItem::input(void)
+{
+}
+
+bool ROSControlItem::control(void)
+{
+  int last_sec = static_cast<int>(time_);
+  double last_nsec = (time_ - last_sec) * 1e9;
+  int now_sec = static_cast<int>(io_->currentTime());
+  double now_nsec = (io_->currentTime() - now_sec) * 1e9;
+
+  ros::Time last(last_sec, static_cast<int>(last_nsec));
+  ros::Time now(now_sec, static_cast<int>(now_nsec));
+  ros::Duration period = now - last;
+
+  rbt_hw_sim_->read(now, period);
+  rbt_hw_sim_->write(now, period);
+
+  manager_->update(now, period, false);
+
+  time_ = io_->currentTime();
+
+  return true;
+}
+
+void ROSControlItem::output(void)
+{
+}
+
+void ROSControlItem::stop(void)
+{
+}
+
+} // namespace cnoid

src/plugin/ROSControlItem.h

@@ -0,0 +1,73 @@
+///////////////////////////////////////////////////////////
+/// @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 <cnoid/ControllerItem>
+#include <cnoid/Body>
+
+// ROS //
+#include <ros/ros.h>
+#include <pluginlib/class_loader.h>
+#include <controller_manager/controller_manager.h>
+#include "robot_hw_sim.h"
+
+// STL //
+#include <vector>
+#include <fstream>
+
+// Boost //
+#include <boost/shared_ptr.hpp>
+
+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<pluginlib::ClassLoader<hardware_interface::RobotHWSim<cnoid::ControllerIO*>>> rbt_hw_sim_loader_;
+  boost::shared_ptr<hardware_interface::RobotHWSim<cnoid::ControllerIO*>> rbt_hw_sim_;
+  std::shared_ptr<controller_manager::ControllerManager> manager_;
+
+  std::string namespace_{""};
+
+};
+
+typedef ref_ptr<ROSControlItem> ROSControlItemPtr;
+
+} // namespace cnoid
+
+#endif // CNOID_ROS_PLUGIN_ROS_CONTROL_ITEM_H

src/plugin/ROSPlugin.cpp

@@ -1,5 +1,6 @@
 #include "WorldROSItem.h"
 #include "BodyROSItem.h"
+#include "ROSControlItem.h"
 #include "deprecated/BodyPublisherItem.h"
 #include <cnoid/Plugin>
 #include <cnoid/MessageView>
@@ -28,6 +29,7 @@ class ROSPlugin : public Plugin
 
         WorldROSItem::initializeClass(this);
         BodyROSItem::initializeClass(this);
+        ROSControlItem::initializeClass(this);
         BodyPublisherItem::initializeClass(this);
 
         return true;