Implemented ros control

src/ros/mb_publisher.cpp

@@ -0,0 +1,54 @@
+#include "ur_modern_driver/ros/mb_publisher.h"
+
+inline void appendAnalog(std::vector<ur_msgs::Analog> &vec, double val, uint8_t pin)
+{
+  ur_msgs::Analog ana;
+  ana.pin = pin;
+  ana.state = val;
+  vec.push_back(ana);
+}
+
+void MBPublisher::publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data)
+{
+  appendAnalog(io_msg.analog_in_states, data.analog_input0, 0);
+  appendAnalog(io_msg.analog_in_states, data.analog_input1, 1);
+  appendAnalog(io_msg.analog_out_states, data.analog_output0, 0);
+  appendAnalog(io_msg.analog_out_states, data.analog_output1, 1);
+
+  io_pub_.publish(io_msg);
+}
+
+bool MBPublisher::consume(MasterBoardData_V1_X& data)
+{
+  ur_msgs::IOStates io_msg;
+  appendDigital(io_msg.digital_in_states, data.digital_input_bits);
+  appendDigital(io_msg.digital_out_states, data.digital_output_bits);
+  publish(io_msg, data);
+  return true;
+}
+bool MBPublisher::consume(MasterBoardData_V3_0__1& data)
+{
+  ur_msgs::IOStates io_msg;
+  appendDigital(io_msg.digital_in_states, data.digital_input_bits);
+  appendDigital(io_msg.digital_out_states, data.digital_output_bits);
+  publish(io_msg, data);
+  return true;
+}
+bool MBPublisher::consume(MasterBoardData_V3_2& data)
+{
+  consume(static_cast<MasterBoardData_V3_0__1&>(data));
+  return true;
+}
+
+bool MBPublisher::consume(RobotModeData_V1_X& data)
+{
+  return true;
+}
+bool MBPublisher::consume(RobotModeData_V3_0__1& data)
+{
+  return true;
+}
+bool MBPublisher::consume(RobotModeData_V3_2& data)
+{
+  return true;
+}

src/ros/robot_hardware.cpp

@@ -0,0 +1,76 @@
+#include "ur_modern_driver/ros/robot_hardware.h"
+
+/*
+bool RobotHardware::canSwitch(const std::list<ControllerInfo>& start_list,
+                              const std::list<ControllerInfo>& stop_list) const
+{
+
+  bool running = active_interface_ != nullptr;
+  size_t start_size = start_list.size();
+  size_t stop_size = stop_list.size();
+
+
+  for (auto const& ci : stop_list)
+  {
+    auto it = interfaces_.find(ci.hardware_interface);
+    if(it == interfaces_.end() || it->second != active_interface_)
+      return false;
+  }
+  
+  for (auto const& ci : start_list)
+  {
+    auto it = interfaces_.find(ci.hardware_interface);
+    //we can only start a controller that's already running if we stop it first
+    if(it == interfaces_.end() || (it->second == active_interface_ && stop_size == 0))
+      return false;
+  }
+
+  return true;
+}
+*/
+
+void RobotHardware::doSwitch(const std::list<ControllerInfo>& start_list,
+                             const std::list<ControllerInfo>& stop_list)
+{
+  if(active_interface_ != nullptr && stop_list.size() > 0)
+  {
+    active_interface_->stop();
+    active_interface_ = nullptr;
+  }
+
+  for(auto const& ci : start_list)
+  {
+    auto it = interfaces_.find(ci.hardware_interface);
+    if(it == interfaces_.end())
+      continue;
+    
+    //we can only switch to one of the available interfaces
+    if(std::find(available_interfaces_.begin(), available_interfaces_.end(), it->second) == available_interfaces_.end())
+      continue;
+
+    auto new_interface = static_cast<HardwareInterface*>(it->second);
+
+    if(new_interface == nullptr)
+      continue;
+    
+    LOG_INFO("Starting %s", ci.hardware_interface.c_str());
+    active_interface_ = new_interface;
+    new_interface->start();
+    
+    return;
+  }
+}
+
+void RobotHardware::write()
+{
+  if(active_interface_ == nullptr)
+    return;
+  
+  active_interface_->write();
+}
+
+void RobotHardware::read(RTShared& packet)
+{
+  joint_interface_.update(packet);
+  wrench_interface_.update(packet);
+}