Fixed compiler warnings

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)
-  : commander_(commander)
+PositionInterface::  PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names)
+  : 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())