command_list_manager.h

Go to the documentation of this file.

/*
 * Copyright (c) 2018 Pilz GmbH & Co. KG
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.

 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#ifndef COMMAND_LIST_MANAGER_H
#define COMMAND_LIST_MANAGER_H

#include <string>

#include <boost/optional.hpp>

#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_pipeline/planning_pipeline.h>
#include <moveit_msgs/MotionPlanResponse.h>

#include "pilz_msgs/MotionSequenceRequest.h"
#include "pilz_trajectory_generation/trajectory_blender.h"
#include "pilz_trajectory_generation/plan_components_builder.h"
#include "pilz_trajectory_generation/trajectory_generation_exceptions.h"

namespace pilz_trajectory_generation
{

using RobotTrajCont = std::vector<robot_trajectory::RobotTrajectoryPtr>;

// List of exceptions which can be thrown by the CommandListManager class.
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NegativeBlendRadiusException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LastBlendRadiusNotZeroException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(StartStateSetException, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(OverlappingBlendRadiiException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(PlanningPipelineException, moveit_msgs::MoveItErrorCodes::FAILURE);

class CommandListManager
{
public:
  CommandListManager(const ros::NodeHandle& nh, const robot_model::RobotModelConstPtr& model);

  RobotTrajCont solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                      const planning_pipeline::PlanningPipelinePtr& planning_pipeline,
                      const pilz_msgs::MotionSequenceRequest& req_list);

private:
  using MotionResponseCont = std::vector<planning_interface::MotionPlanResponse>;
  using RobotState_OptRef = boost::optional<const robot_state::RobotState& >;
  using RadiiCont = std::vector<double>;
  using GroupNamesCont = std::vector<std::string>;

private:
  void checkForOverlappingRadii(const MotionResponseCont& resp_cont,
                                const RadiiCont &radii) const;

  MotionResponseCont solveSequenceItems(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                        const planning_pipeline::PlanningPipelinePtr& planning_pipeline,
                                        const pilz_msgs::MotionSequenceRequest &req_list) const;

  bool checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A,
                            const double radii_A,
                            const robot_trajectory::RobotTrajectory& traj_B,
                            const double radii_B) const;

private:
  static RobotState_OptRef getPreviousEndState(const MotionResponseCont &motion_plan_responses,
                                               const std::string &group_name);

  static void setStartState(const MotionResponseCont &motion_plan_responses,
                            const std::string &group_name,
                            moveit_msgs::RobotState& start_state);

  static RadiiCont extractBlendRadii(const moveit::core::RobotModel &model,
                                     const pilz_msgs::MotionSequenceRequest &req_list);

  static bool isInvalidBlendRadii(const moveit::core::RobotModel &model,
                                  const pilz_msgs::MotionSequenceItem& item_A,
                                  const pilz_msgs::MotionSequenceItem& item_B);

  static void checkForNegativeRadii(const pilz_msgs::MotionSequenceRequest &req_list);

  static void checkLastBlendRadiusZero(const pilz_msgs::MotionSequenceRequest &req_list);

  static void checkStartStatesOfGroup(const pilz_msgs::MotionSequenceRequest &req_list,
                                      const std::string& group_name);

  static void checkStartStates(const pilz_msgs::MotionSequenceRequest &req_list);

  static GroupNamesCont getGroupNames(const pilz_msgs::MotionSequenceRequest &req_list);

private:
  ros::NodeHandle nh_;

  moveit::core::RobotModelConstPtr model_;

  PlanComponentsBuilder plan_comp_builder_;
};

inline void CommandListManager::checkLastBlendRadiusZero(const pilz_msgs::MotionSequenceRequest &req_list)
{
  if(req_list.items.back().blend_radius != 0.0)
  {
    throw LastBlendRadiusNotZeroException("The last blending radius must be zero");
  }
}

}

#endif // COMMAND_LIST_MANAGER_H