filter_base.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2011, Southwest Research Institute
 * Copyright (C) 2019, FUJISOFT
 * 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 Southwest Research Institute, 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.
 */

#ifndef FILTER_BASE_H_
#define FILTER_BASE_H_

#include <typeinfo>
#include "ros/assert.h"
#include "ros/console.h"
#include "ros/ros.h"
#include <moveit/planning_request_adapter/planning_request_adapter.h>
#include <class_loader/class_loader.h>

namespace fsrobo_r_trajectory_filters
{

struct MessageAdapter
{

  struct Request
  {
    trajectory_msgs::JointTrajectory trajectory;
  } request;
};

template<typename T>
  class FilterBase : public planning_request_adapter::PlanningRequestAdapter
  {
  public:

    FilterBase() :
        planning_request_adapter::PlanningRequestAdapter(), nh_("~"), configured_(false), filter_type_("FilterBase"), filter_name_(
            "Unimplemented")
    {

    }

    virtual ~FilterBase()
    {
    }
    ;

    virtual bool update(const T& data_in, T& data_out)=0;

    std::string getType()
    {
      return filter_type_;
    }
    ;

    inline const std::string& getName()
    {
      return filter_name_;
    }
    ; // original FilterBase method

  protected:

    virtual bool configure()=0;

    std::string filter_name_;

    std::string filter_type_;

    bool configured_;

    ros::NodeHandle nh_;

  protected:

    virtual bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr &planning_scene,
                              const planning_interface::MotionPlanRequest &req,
                              planning_interface::MotionPlanResponse &res,
                              std::vector<std::size_t> &added_path_index) const
    {

      // non const pointer to this
      FilterBase<MessageAdapter> *p = const_cast<FilterBase<MessageAdapter>*>(this);

      // calling the configure method
      if (!configured_ && p->configure())
      {
        p->configured_ = true;
      }

      // moveit messages for saving trajectory data
      moveit_msgs::RobotTrajectory robot_trajectory_in, robot_trajectory_out;
      MessageAdapter trajectory_in, trajectory_out; // mapping structures

      // calling planner first
      bool result = planner(planning_scene, req, res);

      // applying filter to planned trajectory
      if (result && res.trajectory_)
      {
        // mapping arguments into message adapter struct
        res.trajectory_->getRobotTrajectoryMsg(robot_trajectory_in);
        trajectory_in.request.trajectory = robot_trajectory_in.joint_trajectory;

        // applying arm navigation filter to planned trajectory
        p->update(trajectory_in, trajectory_out);

        // saving filtered trajectory into moveit message.
        robot_trajectory_out.joint_trajectory = trajectory_out.request.trajectory;
        res.trajectory_->setRobotTrajectoryMsg(planning_scene->getCurrentState(), robot_trajectory_out);

      }

      return result;
    }

    virtual std::string getDescription() const
    {
      // non const pointer to this
      FilterBase<MessageAdapter> *p = const_cast<FilterBase<MessageAdapter>*>(this);

      std::stringstream ss;
      ss << "Trajectory filter '" << p->getName() << "' of type '" << p->getType() << "'";
      return ss.str();
    }
  };

}

#endif /* FILTER_BASE_H_ */