Updated for latest packets

dniewinski · Oct 16, 2018 · e5fffa1 · e5fffa1
1 parent c4b2c73
commit e5fffa1
Show file tree

Hide file tree

Showing 12 changed files with 127 additions and 10 deletions.
diff --git a/include/ur_modern_driver/event_counter.h b/include/ur_modern_driver/event_counter.h
@@ -70,6 +70,11 @@ class EventCounter : public URRTPacketConsumer
     trigger();
     return true;
   }
+  bool consume(RTState_V3_5__5_1& state)
+  {
+    trigger();
+    return true;
+  }
 
   void setupConsumer()
   {
@@ -81,4 +86,4 @@ class EventCounter : public URRTPacketConsumer
   void stopConsumer()
   {
   }
-};
+};
diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h
@@ -70,4 +70,5 @@ class ActionServer : public Service, public URRTPacketConsumer
   virtual bool consume(RTState_V1_8& state);
   virtual bool consume(RTState_V3_0__1& state);
   virtual bool consume(RTState_V3_2__3& state);
+  virtual bool consume(RTState_V3_5__5_1& state);
 };
diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h
@@ -84,6 +84,11 @@ class ROSController : private hardware_interface::RobotHW, public URRTPacketCons
     read(state);
     return update();
   }
+  virtual bool consume(RTState_V3_5__5_1& state)
+  {
+    read(state);
+    return update();
+  }
 
   virtual void onTimeout();
 

diff --git a/include/ur_modern_driver/ros/rt_publisher.h b/include/ur_modern_driver/ros/rt_publisher.h
@@ -61,6 +61,7 @@ class RTPublisher : public URRTPacketConsumer
   virtual bool consume(RTState_V1_8& state);
   virtual bool consume(RTState_V3_0__1& state);
   virtual bool consume(RTState_V3_2__3& state);
+  virtual bool consume(RTState_V3_5__5_1& state);
 
   virtual void setupConsumer()
   {

diff --git a/include/ur_modern_driver/ur/consumer.h b/include/ur_modern_driver/ur/consumer.h
@@ -19,6 +19,7 @@ class URRTPacketConsumer : public IConsumer<RTPacket>
   virtual bool consume(RTState_V1_8& state) = 0;
   virtual bool consume(RTState_V3_0__1& state) = 0;
   virtual bool consume(RTState_V3_2__3& state) = 0;
+  virtual bool consume(RTState_V3_5__5_1& state) = 0;
 };
 
 class URStatePacketConsumer : public IConsumer<StatePacket>
@@ -47,4 +48,4 @@ class URMessagePacketConsumer : public IConsumer<MessagePacket>
   }
 
   virtual bool consume(VersionMessage& message) = 0;
-};
+};
diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h
@@ -112,12 +112,18 @@ class URFactory : private URMessagePacketConsumer
       else
         return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_8);
     }
-    else
+    else if(major_version_ == 3)
     {
-      if (minor_version_ < 3)
+      if (minor_version_ < 2)
         return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_0__1);
-      else
+      else if (minor_version_ < 5)
         return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_2__3);
+      else
+        return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_5__5_1);
+    }
+    else
+    {
+      return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_5__5_1);
     }
   }
-};
+};
diff --git a/include/ur_modern_driver/ur/rt_parser.h b/include/ur_modern_driver/ur/rt_parser.h
@@ -35,4 +35,5 @@ class URRTStateParser : public URParser<RTPacket>
 typedef URRTStateParser<RTState_V1_6__7> URRTStateParser_V1_6__7;
 typedef URRTStateParser<RTState_V1_8> URRTStateParser_V1_8;
 typedef URRTStateParser<RTState_V3_0__1> URRTStateParser_V3_0__1;
-typedef URRTStateParser<RTState_V3_2__3> URRTStateParser_V3_2__3;
+typedef URRTStateParser<RTState_V3_2__3> URRTStateParser_V3_2__3;
+typedef URRTStateParser<RTState_V3_5__5_1> URRTStateParser_V3_5__5_1;
diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h
@@ -116,4 +116,18 @@ class RTState_V3_2__3 : public RTState_V3_0__1
   static const size_t SIZE = RTState_V3_0__1::SIZE + sizeof(uint64_t) + sizeof(double);
 
   static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!");
-};
+};
+
+class RTState_V3_5__5_1 : public RTState_V3_2__3
+{
+public:
+  bool parseWith(BinParser& bp);
+  virtual bool consumeWith(URRTPacketConsumer& consumer);
+
+  double3_t elbow_position;
+  double3_t elbow_velocity;
+
+  static const size_t SIZE = RTState_V3_2__3::SIZE + sizeof(double3_t) * 2;
+
+  static_assert(RTState_V3_5__5_1::SIZE == 984, "RTState_V3_5__5_1 has mismatched size!");
+};
diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp
@@ -78,6 +78,10 @@ bool ActionServer::consume(RTState_V3_2__3& state)
 {
   return updateState(state);
 }
+bool ActionServer::consume(RTState_V3_5__5_1& state)
+{
+  return updateState(state);
+}
 
 void ActionServer::onGoal(GoalHandle gh)
 {

diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp
@@ -117,3 +117,7 @@ bool RTPublisher::consume(RTState_V3_2__3& state)
 {
   return publish(state);
 }
+bool RTPublisher::consume(RTState_V3_5__5_1& state)
+{
+  return publish(state);
+}
diff --git a/src/ur/rt_state.cpp b/src/ur/rt_state.cpp
@@ -99,6 +99,19 @@ bool RTState_V3_2__3::parseWith(BinParser& bp)
   return true;
 }
 
+bool RTState_V3_5__5_1::parseWith(BinParser& bp)
+{
+  if (!bp.checkSize<RTState_V3_5__5_1>())
+    return false;
+
+  RTState_V3_2__3::parseWith(bp);
+
+  bp.parse(elbow_position);
+  bp.parse(elbow_velocity);
+
+  return true;
+}
+
 bool RTState_V1_6__7::consumeWith(URRTPacketConsumer& consumer)
 {
   return consumer.consume(*this);
@@ -114,4 +127,8 @@ bool RTState_V3_0__1::consumeWith(URRTPacketConsumer& consumer)
 bool RTState_V3_2__3::consumeWith(URRTPacketConsumer& consumer)
 {
   return consumer.consume(*this);
-}
+}
+bool RTState_V3_5__5_1::consumeWith(URRTPacketConsumer& consumer)
+{
+  return consumer.consume(*this);
+}
diff --git a/tests/ur/rt_state.cpp b/tests/ur/rt_state.cpp
@@ -159,6 +159,56 @@ TEST(RTState_V3_2__3, testRandomDataParsing)
   EXPECT_TRUE(bp.empty()) << "did not consume all data";
 }
 
+TEST(RTState_V3_5__5_1, testRandomDataParsing)
+{
+  RandomDataTest rdt(1108);
+  BinParser bp = rdt.getParser(true);
+  RTState_V3_5__5_1 state;
+  EXPECT_TRUE(state.parseWith(bp)) << "parse() returned false";
+
+  ASSERT_EQ(rdt.getNext<double>(), state.time);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qdd_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.m_target);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.q_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.qd_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_actual);
+
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.i_control);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_actual);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_actual);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.tcp_force);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tool_vector_target);
+  ASSERT_EQ(rdt.getNext<cartesian_coord_t>(), state.tcp_speed_target);
+
+  ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_inputs);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.motor_temperatures);
+  ASSERT_EQ(rdt.getNext<double>(), state.controller_time);
+  rdt.skip(sizeof(double));  // skip unused value
+  ASSERT_EQ(rdt.getNext<double>(), state.robot_mode);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.joint_modes);
+  ASSERT_EQ(rdt.getNext<double>(), state.safety_mode);
+  rdt.skip(sizeof(double) * 6);
+  ASSERT_EQ(rdt.getNext<double3_t>(), state.tool_accelerometer_values);
+  rdt.skip(sizeof(double) * 6);
+  ASSERT_EQ(rdt.getNext<double>(), state.speed_scaling);
+  ASSERT_EQ(rdt.getNext<double>(), state.linear_momentum_norm);
+  rdt.skip(sizeof(double) * 2);
+  ASSERT_EQ(rdt.getNext<double>(), state.v_main);
+  ASSERT_EQ(rdt.getNext<double>(), state.v_robot);
+  ASSERT_EQ(rdt.getNext<double>(), state.i_robot);
+  ASSERT_DOUBLE_ARRAY_EQ(rdt.getNext<double>(), state.v_actual);
+  ASSERT_EQ(rdt.getNext<uint64_t>(), state.digital_outputs);
+  ASSERT_EQ(rdt.getNext<double>(), state.program_state);
+
+  ASSERT_EQ(rdt.getNext<double3_t>(), state.elbow_position);
+  ASSERT_EQ(rdt.getNext<double3_t>(), state.elbow_velocity);
+
+  EXPECT_TRUE(bp.empty()) << "did not consume all data";
+}
+
 TEST(RTState_V1_6__7, testTooSmallBuffer)
 {
   RandomDataTest rdt(10);
@@ -189,4 +239,12 @@ TEST(RTState_V3_2__3, testTooSmallBuffer)
   BinParser bp = rdt.getParser(true);
   RTState_V3_2__3 state;
   EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
-}
+}
+
+TEST(RTState_V3_5__5_1, testTooSmallBuffer)
+{
+  RandomDataTest rdt(10);
+  BinParser bp = rdt.getParser(true);
+  RTState_V3_5__5_1 state;
+  EXPECT_FALSE(state.parseWith(bp)) << "parse() should fail when buffer not big enough";
+}