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));
+      //quietly ignore the intial version message
-      return false;
+      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;
       }
 

src/ros/action_server.cpp

@@ -13,10 +13,10 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vector<std::string
   , joint_set_(joint_names.begin(), joint_names.end())
   , max_velocity_(max_velocity)
   , interrupt_traj_(false)
-  , running_(false)
   , has_goal_(false)
-  , state_(RobotState::Error)
+  , running_(false)
   , follower_(follower)
+  , state_(RobotState::Error)
 {
 }
 
@@ -291,7 +291,6 @@ void ActionServer::trajectoryThread()
       trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0)));
     }
 
-    int j = 0;
     for(auto const& point : goal->trajectory.points)
     {
       std::array<double, 6> pos, vel;
@@ -302,12 +301,11 @@ void ActionServer::trajectoryThread()
         vel[idx] = point.velocities[i];
       }
       auto t = convert(point.time_from_start);
-      LOG_DEBUG("P[%d] = %ds, %dns -> %dus", j++, point.time_from_start.sec, point.time_from_start.nsec, t.count());
       trajectory.push_back(TrajectoryPoint(pos, vel, t));
     }
 
     double t = std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size()-1].time_from_start).count();
-    LOG_INFO("Executing trajectory with %d points and duration of %4.3fs", trajectory.size(), t);
+    LOG_INFO("Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t);
 
     Result res;
 

src/ros/controller.cpp

@@ -1,10 +1,10 @@
 #include "ur_modern_driver/ros/controller.h"
 
-ROSController::ROSController(URCommander &commander, std::vector<std::string> &joint_names, double max_vel_change)
+ROSController::ROSController(URCommander &commander, TrajectoryFollower& follower, std::vector<std::string> &joint_names, double max_vel_change)
   : controller_(this, nh_)
   , joint_interface_(joint_names)
   , wrench_interface_()
-  , position_interface_(commander, joint_interface_, joint_names)
+  , position_interface_(follower, joint_interface_, joint_names)
   , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change)
 {
   registerInterface(&joint_interface_);

src/ros/hardware_interface.cpp

@@ -57,8 +57,8 @@ void VelocityInterface::reset()
 }
 
 
-PositionInterface::  PositionInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names)
+PositionInterface::  PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names)
-  : commander_(commander)
+  : follower_(follower)
 {
   for (size_t i = 0; i < 6; i++)
   {
@@ -68,15 +68,15 @@ PositionInterface::  PositionInterface(URCommander &commander, hardware_interfac
 
 bool PositionInterface::write()
 {
-
+  return follower_.execute(position_cmd_);
 }
 
 void PositionInterface::start()
 {
-
+  follower_.start();
 }
 
 void PositionInterface::stop()
 {
-
+  follower_.stop();
 }

src/ros/trajectory_follower.cpp

@@ -68,11 +68,10 @@ end
 TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3)
   : running_(false)
   , commander_(commander)
-  , reverse_port_(reverse_port)
+  , server_(reverse_port)
   , servoj_time_(0.008)
   , servoj_lookahead_time_(0.03)
   , servoj_gain_(300.)
-  , server_(reverse_port)
 {
   ros::param::get("~servoj_time", servoj_time_);
   ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_);
@@ -89,7 +88,7 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve
 
   res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
   res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip);
-  res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_));
+  res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
   program_ = res;
 
   if(!server_.bind())

src/ros_main.cpp

@@ -110,7 +110,7 @@ int main(int argc, char **argv)
   if (args.use_ros_control)
   {
     LOG_INFO("ROS control enabled");
-    controller = new ROSController(*rt_commander, args.joint_names, args.max_vel_change);
+    controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change);
     rt_vec.push_back(controller);
     services.push_back(controller);
   }