diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index 169ee00d3f..8cbd97c93f 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -81,14 +81,12 @@ class TrajectoryExecutionManager }; /// Load the controller manager plugin, start listening for events on a topic. - TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, - const planning_scene_monitor::CurrentStateMonitorPtr& csm, - const rclcpp::Node::SharedPtr& node); + TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, const robot_model::RobotModelConstPtr& robot_model, + const planning_scene_monitor::CurrentStateMonitorPtr& csm); /// Load the controller manager plugin, start listening for events on a topic. - TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, - const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers, - const rclcpp::Node::SharedPtr& node); + TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, const robot_model::RobotModelConstPtr& robot_model, + const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers); /// Destructor. Cancels all running trajectories (if any) ~TrajectoryExecutionManager(); @@ -307,9 +305,9 @@ class TrajectoryExecutionManager // Name of this class for logging const std::string name_ = "trajectory_execution_manager"; + rclcpp::Node::SharedPtr node_; robot_model::RobotModelConstPtr robot_model_; planning_scene_monitor::CurrentStateMonitorPtr csm_; - rclcpp::Node::SharedPtr node_; rclcpp::Subscription::SharedPtr event_topic_subscriber_; std::map known_controllers_; bool manage_controllers_; @@ -322,6 +320,7 @@ class TrajectoryExecutionManager boost::mutex execution_state_mutex_; boost::mutex continuous_execution_mutex_; + boost::mutex execution_thread_mutex_; boost::condition_variable continuous_execution_condition_; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 74e868c5a8..0467047817 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -82,20 +82,21 @@ class TrajectoryExecutionManager::DynamicReconfigureImpl // dynamic_reconfigure::Server dynamic_reconfigure_server_; }; -TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, - const planning_scene_monitor::CurrentStateMonitorPtr& csm, - const rclcpp::Node::SharedPtr& node) - : robot_model_(robot_model), csm_(csm), node_(node) +TrajectoryExecutionManager::TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, + const robot_model::RobotModelConstPtr& robot_model, + const planning_scene_monitor::CurrentStateMonitorPtr& csm) + : node_(node), robot_model_(robot_model), csm_(csm) { if (!node_->get_parameter("moveit_manage_controllers", manage_controllers_)) manage_controllers_ = false; initialize(); } -TrajectoryExecutionManager::TrajectoryExecutionManager(const robot_model::RobotModelConstPtr& robot_model, +TrajectoryExecutionManager::TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, + const robot_model::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm, - bool manage_controllers, const rclcpp::Node::SharedPtr& node) - : robot_model_(robot_model), csm_(csm), node_(node), manage_controllers_(manage_controllers) + bool manage_controllers) + : node_(node), robot_model_(robot_model), csm_(csm), manage_controllers_(manage_controllers) { initialize(); }