Factory and setup improvements

CMakeLists.txt

@@ -159,13 +159,14 @@ target_link_libraries(ur_hardware_interface
 ## Declare a C++ executable
 set(${PROJECT_NAME}_SOURCES
     src/ros/rt_publisher.cpp
+    src/ros/mb_publisher.cpp
+    src/ros/robot_hardware.cpp
     src/ur/stream.cpp
+    src/ur/commander.cpp
     src/ur/robot_mode.cpp
     src/ur/master_board.cpp
     src/ur/rt_state.cpp
     src/ur/messages.cpp
-    #src/ros_main.cpp
-    #src/ur_ros_wrapper.cpp
     src/ur_driver.cpp
     src/ur_realtime_communication.cpp
     src/ur_communication.cpp

include/ur_modern_driver/event_counter.h

@@ -0,0 +1,86 @@
+#pragma once
+
+#include <cstdlib>
+#include <chrono>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ur/consumer.h"
+
+
+class EventCounter : public URRTPacketConsumer
+{
+private:
+  typedef std::chrono::high_resolution_clock Clock;
+  Clock::time_point events_[250];
+  size_t idx_ = 0;
+
+
+  Clock::time_point last_;
+
+public:
+  void trigger()
+  {
+    //auto now = Clock::now();
+    //LOG_INFO("Time diff: %d ms", std::chrono::duration_cast<std::chrono::microseconds>(now - last_));
+    //last_ = now;
+    //return;
+
+    events_[idx_] = Clock::now();
+    idx_ += 1;
+
+    if(idx_ > 250)
+    {
+      std::chrono::time_point<std::chrono::high_resolution_clock> t_min =
+          std::chrono::time_point<std::chrono::high_resolution_clock>::max();     
+      std::chrono::time_point<std::chrono::high_resolution_clock> t_max =
+          std::chrono::time_point<std::chrono::high_resolution_clock>::min();
+          
+      for(auto const& e : events_)
+      {
+        if(e < t_min)
+          t_min = e;
+        if(e > t_max)
+          t_max = e;
+      }
+
+      auto diff = t_max - t_min;
+      auto secs = std::chrono::duration_cast<std::chrono::seconds>(diff).count();
+      auto ms = std::chrono::duration_cast<std::chrono::microseconds>(diff).count();
+      std::chrono::duration<double> test(t_max - t_min);
+      LOG_INFO("Recieved 250 messages at %f Hz", (250.0/test.count()));
+      idx_ = 0;
+    }
+  }
+
+public:
+  bool consume(RTState_V1_6__7& state)
+  {
+    trigger();
+    return true;
+  }
+  bool consume(RTState_V1_8& state)
+  {
+    trigger();
+    return true;
+  }
+  bool consume(RTState_V3_0__1& state)
+  {
+    trigger();
+    return true;
+  }
+  bool consume(RTState_V3_2__3& state)
+  {
+    trigger();
+    return true;
+  }
+
+  void setupConsumer()
+  {
+    last_ = Clock::now();
+  }
+  void teardownConsumer()
+  {
+  }
+  void stopConsumer()
+  {
+  }
+};

include/ur_modern_driver/ur/factory.h

@@ -9,6 +9,8 @@
 #include "ur_modern_driver/ur/state_parser.h"
 #include "ur_modern_driver/ur/stream.h"
 
+static const int UR_PRIMARY_PORT = 30001;
+
 class URFactory : private URMessagePacketConsumer
 {
 private:
@@ -42,7 +44,7 @@ private:
   }
 
 public:
-  URFactory(std::string& host) : stream_(host, 30001)
+  URFactory(std::string& host) : stream_(host, UR_PRIMARY_PORT)
   {
     URProducer<MessagePacket> prod(stream_, parser_);
     std::vector<unique_ptr<MessagePacket>> results;

src/ros_main.cpp

@@ -7,6 +7,10 @@
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/pipeline.h"
 #include "ur_modern_driver/ros/rt_publisher.h"
+#include "ur_modern_driver/ros/mb_publisher.h"
+#include "ur_modern_driver/ros/io_service.h"
+#include "ur_modern_driver/ros/ros_controller.h"
+#include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/factory.h"
 #include "ur_modern_driver/ur/messages.h"
 #include "ur_modern_driver/ur/parser.h"
@@ -17,9 +21,14 @@
 static const std::string IP_ADDR_ARG("~robot_ip_address");
 static const std::string REVERSE_PORT_ARG("~reverse_port");
 static const std::string SIM_TIME_ARG("~use_sim_time");
+static const std::string ROS_CONTROL_ARG("~use_ros_control");
 static const std::string PREFIX_ARG("~prefix");
 static const std::string BASE_FRAME_ARG("~base_frame");
 static const std::string TOOL_FRAME_ARG("~tool_frame");
+static const std::string JOINT_NAMES_PARAM("hardware_interface/joints");
+
+static const int UR_SECONDARY_PORT = 30002;
+static const int UR_RT_PORT = 30003;
 
 struct ProgArgs
 {
@@ -28,8 +37,12 @@ public:
   std::string prefix;
   std::string base_frame;
   std::string tool_frame;
+  std::vector<std::string> joint_names;
+  double max_acceleration;
+  double max_velocity;
   int32_t reverse_port;
   bool use_sim_time;
+  bool use_ros_control;
 };
 
 bool parse_args(ProgArgs& args)
@@ -41,12 +54,16 @@ bool parse_args(ProgArgs& args)
   }
   ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
   ros::param::param(SIM_TIME_ARG, args.use_sim_time, false);
+  ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false);
   ros::param::param(PREFIX_ARG, args.prefix, std::string());
   ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
   ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
+  ros::param::get(JOINT_NAMES_PARAM, args.joint_names);  
   return true;
 }
 
+#include "ur_modern_driver/event_counter.h"
+
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "ur_driver");
@@ -58,22 +75,47 @@ int main(int argc, char** argv)
   }
 
   URFactory factory(args.host);
-  auto parser = factory.getRTParser();
+  //RT packets
+  auto rt_parser = factory.getRTParser();
+  URStream rt_stream(args.host, UR_RT_PORT);
+  URProducer<RTPacket> rt_prod(rt_stream, *rt_parser);
+  RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame);
+  EventCounter rt_ec;
 
-  URStream s2(args.host, 30003);
-  URProducer<RTPacket> p2(s2, *parser);
-  RTPublisher pub(args.prefix, args.base_frame, args.tool_frame);
+  URCommander rt_commander(rt_stream);
+  vector<IConsumer<RTPacket>*> rt_vec;
 
-  Pipeline<RTPacket> pl(p2, pub);
-
-  pl.run();
-
-  while (ros::ok())
+  if(args.use_ros_control)
   {
-    std::this_thread::sleep_for(std::chrono::milliseconds(33));
+    rt_vec.push_back(new ROSController(rt_commander, args.joint_names));
   }
 
-  pl.stop();
+  //rt_vec.push_back(&rt_pub);
+
+  MultiConsumer<RTPacket> rt_cons(rt_vec);
+  Pipeline<RTPacket> rt_pl(rt_prod, rt_cons);
+
+  //Message packets
+  auto state_parser = factory.getStateParser();
+  URStream state_stream(args.host, UR_SECONDARY_PORT);
+  URProducer<StatePacket> state_prod(state_stream, *state_parser);
+  MBPublisher state_pub;
+  vector<IConsumer<StatePacket>*> state_vec{&state_pub};
+  MultiConsumer<StatePacket> state_cons(state_vec);
+  Pipeline<StatePacket> state_pl(state_prod, state_cons);
+
+  LOG_INFO("Starting main loop");
+
+  rt_pl.run();
+  state_pl.run();
+
+  URCommander state_commander(state_stream);
+  IOService service(state_commander);
+
+  ros::spin();
+
+  rt_pl.stop();
+  state_pl.stop();
 
   return EXIT_SUCCESS;
 }