Big code dump

include/ur_modern_driver/ros/controller.h

@@ -0,0 +1,80 @@
+#pragma once
+#include <atomic>
+#include <ros/ros.h>
+#include <controller_manager/controller_manager.h>
+#include <hardware_interface/force_torque_sensor_interface.h>
+#include <hardware_interface/internal/demangle_symbol.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ur/commander.h"
+#include "ur_modern_driver/ur/consumer.h"
+#include "ur_modern_driver/ur/rt_state.h"
+#include "ur_modern_driver/ros/hardware_interface.h"
+#include "ur_modern_driver/ros/service_stopper.h"
+
+class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service
+{
+private:
+  ros::NodeHandle nh_;
+  ros::Time lastUpdate_;
+  controller_manager::ControllerManager controller_;
+
+  // state interfaces
+  JointInterface joint_interface_;
+  WrenchInterface wrench_interface_;
+  // controller interfaces
+  PositionInterface position_interface_;
+  VelocityInterface velocity_interface_;
+
+  // currently activated controller
+  HardwareInterface* active_interface_;
+  // map of switchable controllers
+  std::map<std::string, HardwareInterface*> available_interfaces_;
+
+  std::atomic<bool> service_enabled_;
+
+  // helper functions to map interfaces
+  template <typename T>
+  void registerInterface(T* interface)
+  {
+    RobotHW::registerInterface<typename T::parent_type>(interface);
+  }
+  template <typename T>
+  void registerControllereInterface(T* interface)
+  {
+    registerInterface(interface);
+    available_interfaces_[hardware_interface::internal::demangledTypeName<typename T::parent_type>()] = interface;
+  }
+
+  void read(RTShared& state);
+  bool update(RTShared& state);
+  bool write();
+
+public:
+  ROSController(URCommander& commander,  std::vector<std::string>& joint_names, double max_vel_change);
+  virtual ~ROSController() { }
+  // from RobotHW
+  void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list);
+  // from URRTPacketConsumer
+  virtual void setupConsumer();
+  virtual bool consume(RTState_V1_6__7& state)
+  {
+    return update(state);
+  }
+  virtual bool consume(RTState_V1_8& state)
+  {
+    return update(state);
+  }
+  virtual bool consume(RTState_V3_0__1& state)
+  {
+    return update(state);
+  }
+  virtual bool consume(RTState_V3_2__3& state)
+  {
+    return update(state);
+  }
+
+  virtual void onRobotStateChange(RobotState state);
+};