Refactored intialisation of ROSController/ActionServer

include/ur_modern_driver/ros/action_server.h

@@ -39,7 +39,7 @@ private:
   std::condition_variable tj_cv_;
   std::thread tj_thread_;
 
-  TrajectoryFollowerInterface& follower_;
+  ActionTrajectoryFollowerInterface& follower_;
 
   RobotState state_;
   std::array<double, 6> q_actual_, qd_actual_;
@@ -61,7 +61,7 @@ private:
   bool updateState(RTShared& data);
 
 public:
-  ActionServer(TrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity);
+  ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity);
 
   void start();
   virtual void onRobotStateChange(RobotState state);

include/ur_modern_driver/ros/action_trajectory_follower_interface.h

@@ -22,11 +22,11 @@ struct TrajectoryPoint
   }
 };
 
-class TrajectoryFollowerInterface
+class ActionTrajectoryFollowerInterface
 {
 public:
   virtual bool start() = 0;
   virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0;
   virtual void stop() = 0;
-  virtual ~TrajectoryFollowerInterface() {};
+  virtual ~ActionTrajectoryFollowerInterface() {};
 };

include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h

@@ -11,9 +11,9 @@
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/server.h"
-#include "ur_modern_driver/ros/trajectory_follower_interface.h"
+#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
 
-class LowBandwidthTrajectoryFollower:  public TrajectoryFollowerInterface
+class LowBandwidthTrajectoryFollower:  public ActionTrajectoryFollowerInterface
 {
 private:
   std::atomic<bool> running_;

include/ur_modern_driver/ros/trajectory_follower.h

@@ -11,9 +11,9 @@
 #include "ur_modern_driver/log.h"
 #include "ur_modern_driver/ur/commander.h"
 #include "ur_modern_driver/ur/server.h"
-#include "ur_modern_driver/ros/trajectory_follower_interface.h"
+#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
 
-class TrajectoryFollower : public TrajectoryFollowerInterface
+class TrajectoryFollower : public ActionTrajectoryFollowerInterface
 {
 private:
   std::atomic<bool> running_;

src/ros/action_server.cpp

@@ -1,7 +1,7 @@
 #include "ur_modern_driver/ros/action_server.h"
 #include <cmath>
 
-ActionServer::ActionServer(TrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity)
+ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity)
   : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
         boost::bind(&ActionServer::onCancel, this, _1), false)
   , joint_names_(joint_names)

src/ros_main.cpp

@@ -138,35 +138,33 @@ int main(int argc, char **argv)
   auto rt_commander = factory.getCommander(rt_stream);
   vector<IConsumer<RTPacket> *> rt_vec{ &rt_pub };
 
-  TrajectoryFollowerInterface *traj_follower(nullptr);
-
-  if (args.use_lowbandwidth_trajectory_follower && !args.use_ros_control)
-  {
-    LOG_INFO("Use low bandwidth trajectory follower");
-    traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander, local_ip, args.reverse_port,factory.isVersion3());
-    // We are leaking the follower here, but it's OK as this is once-a-lifetime event
-  }
-  else
-  {
-    LOG_INFO("Use standard trajectory follower");
-    traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
-    // We are leaking the follower here, but it's OK as this is once-a-lifetime event
-  }
-
   INotifier *notifier(nullptr);
   ROSController *controller(nullptr);
   ActionServer *action_server(nullptr);
   if (args.use_ros_control)
   {
     LOG_INFO("ROS control enabled");
-    // Note - we are sure that TrajectoryFollower is used here (see the args.use_ros_control above)
+    TrajectoryFollower *traj_follower = new TrajectoryFollower(
-    controller = new ROSController(*rt_commander, *((TrajectoryFollower *) traj_follower), args.joint_names, args.max_vel_change, args.tcp_link);
+        *rt_commander, local_ip, args.reverse_port, factory.isVersion3());
+    controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link);
     rt_vec.push_back(controller);
     services.push_back(controller);
   }
   else
   {
     LOG_INFO("ActionServer enabled");
+    ActionTrajectoryFollowerInterface *traj_follower(nullptr);
+    if (args.use_lowbandwidth_trajectory_follower)
+    {
+      LOG_INFO("Use low bandwidth trajectory follower");
+      traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander,
+           local_ip, args.reverse_port,factory.isVersion3());
+    }
+    else
+    {
+      LOG_INFO("Use standard trajectory follower");
+      traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
+    }
     action_server = new ActionServer(*traj_follower, args.joint_names, args.max_velocity);
     rt_vec.push_back(action_server);
     services.push_back(action_server);