joint_limits_aggregator.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 JOINT_LIMITS_UNIFIER_H
#define JOINT_LIMITS_UNIFIER_H

#include "pilz_extensions/joint_limits_extension.h"
#include "pilz_trajectory_generation/joint_limits_container.h"

#include <ros/ros.h>

#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_interface/planning_response.h>
#include <moveit/robot_model/joint_model.h>

#include <map>

namespace pilz {

class JointLimitsAggregator
{
  public:

    static JointLimitsContainer getAggregatedLimits(const ros::NodeHandle& nh,
                                           const std::vector<const moveit::core::JointModel*>& joint_models);

  protected:
    static void updatePositionLimitFromJointModel(const moveit::core::JointModel* joint_model,
                                                  pilz_extensions::JointLimit& joint_limit);

    static void updateVelocityLimitFromJointModel(const moveit::core::JointModel* joint_model,
                                                  pilz_extensions::JointLimit& joint_limit);

    static void checkPositionBoundsThrowing(const moveit::core::JointModel* joint_model,
                                            const pilz_extensions::JointLimit& joint_limit);


    static void checkVelocityBoundsThrowing(const moveit::core::JointModel* joint_model,
                                            const pilz_extensions::JointLimit& joint_limit);


};

class AggregationException : public std::runtime_error
{
  public:
    AggregationException(const std::string error_desc) : std::runtime_error(error_desc) {}
};

class AggregationBoundsViolationException: public AggregationException
{
  public:
    AggregationBoundsViolationException(const std::string error_desc) : AggregationException(error_desc) {}
};

}

#endif // JOINT_LIMITS_UNIFIER_H