From 71bca7abf7e476b0814e7254ca5701ca1c00a3b1 Mon Sep 17 00:00:00 2001 From: Jarek Potiuk Date: Sat, 24 Feb 2018 13:33:04 +0100 Subject: [PATCH] Refactored intialisation of ROSController/ActionServer --- include/ur_modern_driver/ros/action_server.h | 4 +-- ...=> action_trajectory_follower_interface.h} | 4 +-- .../ros/lowbandwidth_trajectory_follower.h | 4 +-- .../ros/trajectory_follower.h | 4 +-- src/ros/action_server.cpp | 2 +- src/ros_main.cpp | 32 +++++++++---------- 6 files changed, 24 insertions(+), 26 deletions(-) rename include/ur_modern_driver/ros/{trajectory_follower_interface.h => action_trajectory_follower_interface.h} (87%) diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index fe50567..d741714 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/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 q_actual_, qd_actual_; @@ -61,7 +61,7 @@ private: bool updateState(RTShared& data); public: - ActionServer(TrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity); + ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity); void start(); virtual void onRobotStateChange(RobotState state); diff --git a/include/ur_modern_driver/ros/trajectory_follower_interface.h b/include/ur_modern_driver/ros/action_trajectory_follower_interface.h similarity index 87% rename from include/ur_modern_driver/ros/trajectory_follower_interface.h rename to include/ur_modern_driver/ros/action_trajectory_follower_interface.h index a25ddf7..9b5427f 100644 --- a/include/ur_modern_driver/ros/trajectory_follower_interface.h +++ b/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 &trajectory, std::atomic &interrupt) = 0; virtual void stop() = 0; - virtual ~TrajectoryFollowerInterface() {}; + virtual ~ActionTrajectoryFollowerInterface() {}; }; diff --git a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h index e958002..6442dcb 100644 --- a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h +++ b/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 running_; diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index 70dba5c..af37474 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/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 running_; diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index c8546c2..8616c35 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -1,7 +1,7 @@ #include "ur_modern_driver/ros/action_server.h" #include -ActionServer::ActionServer(TrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity) +ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector& 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) diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 523f1d2..5343334 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -138,35 +138,33 @@ int main(int argc, char **argv) auto rt_commander = factory.getCommander(rt_stream); vector *> 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) - controller = new ROSController(*rt_commander, *((TrajectoryFollower *) traj_follower), args.joint_names, args.max_vel_change, args.tcp_link); + TrajectoryFollower *traj_follower = new TrajectoryFollower( + *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);