manipulation_transforms_ros.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 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.
 *********************************************************************/

#ifndef MANIPULATION_TRANSFORMS_ROS_H_
#define MANIPULATION_TRANSFORMS_ROS_H_

#include <ros/ros.h>
#include <LinearMath/btTransform.h>
#include <vector>

#include "manipulation_transforms/LoadInitialTransforms.h"
#include "manipulation_transforms/SetInitialTransforms.h"
#include "manipulation_transforms/MapObjectPoseToEffectors.h"
#include "manipulation_transforms/MapEffectorPosesToObject.h"
#include "manipulation_transforms/MapObjectTwistToEffectors.h"
#include "manipulation_transforms/MapEffectorTwistsToObject.h"
#include "manipulation_transforms/MapObjectTrajectoryToEffectors.h"
#include "manipulation_transforms/MapEffectorTrajectoriesToObject.h"
#include "manipulation_transforms/MapEffectorTrajectoriesToObject.h"

#include "manipulation_transforms/manipulation_transforms.h"


class ManipulationTransformsROS {
public:
        ManipulationTransformsROS(const std::string &reference_frame = "");

        ManipulationTransformsROS(const std::string &reference_frame, const std::string &ns);

        ManipulationTransformsROS(const std::string &reference_frame, geometry_msgs::PoseStamped &obj_init_pose, std::vector<geometry_msgs::PoseStamped> effector_init_poses);

        virtual ~ManipulationTransformsROS();
        void init_services(const std::string &ns = "");

        bool setInitialTransforms(manipulation_transforms::SetInitialTransforms::Request &req,
                        manipulation_transforms::SetInitialTransforms::Response &resp);

        bool loadInitialTransforms(manipulation_transforms::LoadInitialTransforms::Request &req,
                        manipulation_transforms::LoadInitialTransforms::Response &resp);

        bool mapEffectorPosesToObject(manipulation_transforms::MapEffectorPosesToObject::Request &req,
                        manipulation_transforms::MapEffectorPosesToObject::Response &resp);

        bool mapObjectPoseToEffectors(manipulation_transforms::MapObjectPoseToEffectors::Request &req,
                        manipulation_transforms::MapObjectPoseToEffectors::Response &resp);

        bool mapEffectorTwistsToObject(manipulation_transforms::MapEffectorTwistsToObject::Request &req,
                        manipulation_transforms::MapEffectorTwistsToObject::Response &resp);

        bool mapObjectTwistToEffectors(manipulation_transforms::MapObjectTwistToEffectors::Request &req,
                        manipulation_transforms::MapObjectTwistToEffectors::Response &resp);

        bool mapObjectTrajectoryToEffectors(manipulation_transforms::MapObjectTrajectoryToEffectors::Request &req,
                        manipulation_transforms::MapObjectTrajectoryToEffectors::Response &resp);

        bool mapEffectorTrajectoriesToObject(manipulation_transforms::MapEffectorTrajectoriesToObject::Request &req,
                                manipulation_transforms::MapEffectorTrajectoriesToObject::Response &resp);

private:

        ros::NodeHandle param_nh_; 
        ros::NodeHandle private_nh_; 
        ManipulationTransforms solver_; 

        ros::ServiceServer effector_poses_to_object_service_;
        ros::ServiceServer object_pose_to_effectors_service_;
        ros::ServiceServer effector_twists_to_object_service_;
        ros::ServiceServer object_twist_to_effectors_service_;
        ros::ServiceServer effector_trajectories_to_object_service_;
        ros::ServiceServer object_trajectory_to_effectors_service_;
        ros::ServiceServer load_initial_transforms_service_;
        ros::ServiceServer set_initial_transforms_service_;

        std::string BASE_FRAME_; 
        int n_effectors_;

        btTransform obj_initial_pose_; 
        std::vector<btTransform> effector_init_poses; 
        bool loadParamServerTransforms(const ros::NodeHandle &nh);

        int checkForParamServerTransforms(const ros::NodeHandle &nh, bool warn = false);

};

#endif /* MANIPULATION_TRANSFORMS_ROS_H_ */