Fixed compiler warnings

include/ur_modern_driver/ros/action_server.h

@@ -32,9 +32,6 @@ private:
   std::set<std::string> joint_set_;
   double max_velocity_;
 
-  RobotState state_;
-  std::array<double, 6> q_actual_, qd_actual_;
-
   GoalHandle curr_gh_;
   std::atomic<bool> interrupt_traj_;
   std::atomic<bool> has_goal_, running_;
@@ -44,6 +41,9 @@ private:
 
   TrajectoryFollower& follower_;
 
+  RobotState state_;
+  std::array<double, 6> q_actual_, qd_actual_;
+
   void onGoal(GoalHandle gh);
   void onCancel(GoalHandle gh);
 

include/ur_modern_driver/ros/controller.h

@@ -55,7 +55,7 @@ private:
   void reset();
 
 public:
-  ROSController(URCommander& commander,  std::vector<std::string>& joint_names, double max_vel_change);
+  ROSController(URCommander& commander, TrajectoryFollower& follower, 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);

include/ur_modern_driver/ros/hardware_interface.h

@@ -6,6 +6,7 @@
 #include <algorithm>
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/rt_state.h"
+#include "ur_modern_driver/ros/trajectory_follower.h"
 
 class HardwareInterface
 {
@@ -56,11 +57,11 @@ public:
 class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface
 {
 private:
-  URCommander &commander_;
+  TrajectoryFollower& follower_;
   std::array<double, 6> position_cmd_;
 
 public:
-  PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names);
+  PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names);
   virtual bool write();
   virtual void start();
   virtual void stop();

include/ur_modern_driver/ros/trajectory_follower.h

@@ -33,12 +33,12 @@ struct TrajectoryPoint
 class TrajectoryFollower 
 {
 private:
-  double servoj_time_, servoj_lookahead_time_, servoj_gain_;
   std::atomic<bool> running_;
   std::array<double, 6> last_positions_;
   URCommander &commander_;
   URServer server_;
-  int reverse_port_;
+
+  double servoj_time_, servoj_lookahead_time_, servoj_gain_;
   std::string program_;
 
   template <typename T>

include/ur_modern_driver/ur/state_parser.h

@@ -33,17 +33,16 @@ public:
     bp.parse(packet_size);
     bp.parse(type);
 
-    //quietly ignore the intial version message
-    if (type == message_type::ROBOT_MESSAGE || ((int)type) == 5)
-    {
-      bp.consume();
-      return true;
-    }
-
     if (type != message_type::ROBOT_STATE)
     {
-      LOG_WARN("Invalid state message type recieved: %u", static_cast<uint8_t>(type));
-      return false;
+      //quietly ignore the intial version message
+      if(type != message_type::ROBOT_MESSAGE)
+      {
+        LOG_WARN("Invalid state message type recieved: %u", static_cast<uint8_t>(type));
+      }
+
+      bp.consume();
+      return true;
     }
 
     while (!bp.empty())
@@ -60,8 +59,6 @@ public:
         return false;
       }
 
-      //LOG_DEBUG("sub-packet size: %" PRIu32, sub_size);
-
       // deconstruction of a sub parser will increment the position of the parent parser
       BinParser sbp(bp, sub_size);
       sbp.consume(sizeof(sub_size));
@@ -73,7 +70,6 @@ public:
       if (packet == nullptr)
       {
         sbp.consume();
-        //LOG_DEBUG("Skipping sub-packet of type %d", type);
         continue;
       }