trajectory_execution_manager.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#ifndef MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_
#define MOVEIT_TRAJECTORY_EXECUTION_MANAGER_TRAJECTORY_EXECUTION_MANAGER_

#include <moveit/robot_model/robot_model.h>
#include <moveit_msgs/RobotTrajectory.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/String.h>
#include <ros/ros.h>
#include <moveit/controller_manager/controller_manager.h>
#include <boost/thread.hpp>
#include <pluginlib/class_loader.h>
#include <boost/scoped_ptr.hpp>

namespace trajectory_execution_manager
{

// Two modes:
// Managed controllers
// Unmanaged controllers: given the trajectory,
class TrajectoryExecutionManager
{
public:

  static const std::string EXECUTION_EVENT_TOPIC;

  typedef boost::function<void(const moveit_controller_manager::ExecutionStatus&)> ExecutionCompleteCallback;

  typedef boost::function<void(std::size_t)> PathSegmentCompleteCallback;

  struct TrajectoryExecutionContext
  {
    std::vector<std::string> controllers_;

    // The trajectory to execute, split in different parts (by joints), each set of joints corresponding to one controller
    std::vector<moveit_msgs::RobotTrajectory> trajectory_parts_;
  };

  TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel);

  TrajectoryExecutionManager(const robot_model::RobotModelConstPtr &kmodel, bool manage_controllers);

  ~TrajectoryExecutionManager();

  bool isManagingControllers() const;

  const moveit_controller_manager::MoveItControllerManagerPtr& getControllerManager() const;

  void processEvent(const std::string &event);

  bool ensureActiveControllersForGroup(const std::string &group);

  bool ensureActiveControllersForJoints(const std::vector<std::string> &joints);

  bool ensureActiveController(const std::string &controller);

  bool ensureActiveControllers(const std::vector<std::string> &controllers);

  bool isControllerActive(const std::string &controller);

  bool areControllersActive(const std::vector<std::string> &controllers);

  bool push(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller = "");

  bool push(const trajectory_msgs::JointTrajectory &trajectory, const std::string &controller = "");

  bool push(const trajectory_msgs::JointTrajectory &trajectory, const std::vector<std::string> &controllers);

  bool push(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers);

  const std::vector<TrajectoryExecutionContext*>& getTrajectories() const;

  void execute(const ExecutionCompleteCallback &callback = ExecutionCompleteCallback(), bool auto_clear = true);

  void execute(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear = true);

  moveit_controller_manager::ExecutionStatus executeAndWait(bool auto_clear = true);

  bool pushAndExecute(const moveit_msgs::RobotTrajectory &trajectory, const std::string &controller = "");

  bool pushAndExecute(const trajectory_msgs::JointTrajectory &trajectory, const std::string &controller = "");

  bool pushAndExecute(const sensor_msgs::JointState &state, const std::string &controller = "");

  bool pushAndExecute(const trajectory_msgs::JointTrajectory &trajectory, const std::vector<std::string> &controllers);

  bool pushAndExecute(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers);

  bool pushAndExecute(const sensor_msgs::JointState &state, const std::vector<std::string> &controllers);

  moveit_controller_manager::ExecutionStatus waitForExecution();

  std::pair<int, int> getCurrentExpectedTrajectoryIndex() const;

  moveit_controller_manager::ExecutionStatus getLastExecutionStatus() const;

  void stopExecution(bool auto_clear = true);

  void clear();

  void enableExecutionDurationMonitoring(bool flag);

  void setAllowedExecutionDurationScaling(double scaling);

  void setExecutionVelocityScaling(double scaling);

private:

  struct ControllerInformation
  {
    std::string name_;
    std::set<std::string> joints_;
    std::set<std::string> overlapping_controllers_;
    moveit_controller_manager::MoveItControllerManager::ControllerState state_;
    ros::Time last_update_;

    bool operator<(ControllerInformation &other) const
    {
      if (joints_.size() != other.joints_.size())
        return joints_.size() < other.joints_.size();
      return name_ < other.name_;
    }
  };

  void initialize();

  void reloadControllerInformation();

  bool configure(TrajectoryExecutionContext &context, const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers);

  void updateControllersState(const ros::Duration &age);
  void updateControllerState(const std::string &controller, const ros::Duration &age);
  void updateControllerState(ControllerInformation &ci, const ros::Duration &age);

  bool distributeTrajectory(const moveit_msgs::RobotTrajectory &trajectory, const std::vector<std::string> &controllers, std::vector<moveit_msgs::RobotTrajectory> &parts);

  bool findControllers(const std::set<std::string> &actuated_joints, std::size_t controller_count, const std::vector<std::string> &available_controllers, std::vector<std::string> &selected_controllers);
  bool checkControllerCombination(std::vector<std::string> &controllers, const std::set<std::string> &actuated_joints);
  void generateControllerCombination(std::size_t start_index, std::size_t controller_count, const std::vector<std::string> &available_controllers,
                                     std::vector<std::string> &selected_controllers, std::vector< std::vector<std::string> > &selected_options,
                                     const std::set<std::string> &actuated_joints);
  bool selectControllers(const std::set<std::string> &actuated_joints, const std::vector<std::string> &available_controllers, std::vector<std::string> &selected_controllers);

  void executeThread(const ExecutionCompleteCallback &callback, const PathSegmentCompleteCallback &part_callback, bool auto_clear);
  bool executePart(std::size_t part_index);
  void continuousExecutionThread();


  void stopExecutionInternal();

  void receiveEvent(const std_msgs::StringConstPtr &event);

  robot_model::RobotModelConstPtr kinematic_model_;
  ros::NodeHandle node_handle_;
  ros::NodeHandle root_node_handle_;
  ros::Subscriber event_topic_subscriber_;

  std::map<std::string, ControllerInformation> known_controllers_;
  bool manage_controllers_;

  // thread used to execute trajectories using the execute() command
  boost::scoped_ptr<boost::thread> execution_thread_;

  // thread used to execute trajectories using pushAndExecute()
  boost::scoped_ptr<boost::thread> continuous_execution_thread_;

  boost::mutex execution_state_mutex_;
  boost::mutex continuous_execution_mutex_;

  boost::condition_variable continuous_execution_condition_;

  // this condition is used to notify the completion of execution for given trajectories
  boost::condition_variable execution_complete_condition_;

  moveit_controller_manager::ExecutionStatus last_execution_status_;
  std::vector<moveit_controller_manager::MoveItControllerHandlePtr> active_handles_;
  int current_context_;
  std::vector<ros::Time> time_index_;
  mutable boost::mutex time_index_mutex_;
  bool execution_complete_;

  bool stop_continuous_execution_;
  bool run_continuous_execution_thread_;
  std::vector<TrajectoryExecutionContext*> trajectories_;
  std::deque<TrajectoryExecutionContext*> continuous_execution_queue_;

  boost::scoped_ptr<pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager> > controller_manager_loader_;
  moveit_controller_manager::MoveItControllerManagerPtr controller_manager_;

  bool verbose_;

  class DynamicReconfigureImpl;
  DynamicReconfigureImpl *reconfigure_impl_;

  bool execution_duration_monitoring_;
  double allowed_execution_duration_scaling_;
  double execution_velocity_scaling_;
};

typedef boost::shared_ptr<TrajectoryExecutionManager> TrajectoryExecutionManagerPtr;
typedef boost::shared_ptr<const TrajectoryExecutionManager> TrajectoryExecutionManagerConstPtr;

}

#endif