manipulation_transforms: manipulation_transforms: server.cpp Source File

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00045 #include <LinearMath/btTransform.h>
00046 #include <ros/ros.h>
00047 #include <geometry_msgs/PoseStamped.h>
00048 
00049 #include "manipulation_transforms/manipulation_transforms_ros.h"
00050 
00051 #include <argp.h>
00052 
00053  const char *argp_program_version = "manipulation_transforms_server 0.1";
00054  const char *argp_program_bug_address = "<jscholz@willowgarage.com>";
00055 
00056  /* Program documentation. */
00057  static char doc[] =
00058                 "Provides a rose service node for mapping between object and gripper poses.  \n\n"
00059                 "This node must be initialized with transforms in a common reference frame for the target "
00060                  "object and each gripper in contact with it.  "
00061                  "After initialization, manipulation_transforms_server provides service calls for querying poses, "
00062                  "twists, and trajectories from object to grippers, or grippers to object.  \n"
00063                  "If not called with the \"-l\" option, the node must be initialized by a call to the service"
00064                  "\'SetGraspTransforms\' before issuing queries.  ";
00065 
00066  /* A description of the arguments we accept. */
00067  static char args_doc[] = "[--load=NAMESPACE]";
00068 
00069  /* The options we understand. */
00070  static struct argp_option options[] = {
00071    {"load",  'l', 0, 0,  "Attempt to load the rigid transforms from parameter server (see doxy for naming convention)", 0},
00072    {"namespace",  'n', "NAMESPACE", 0,  "Use NAMESPACE as the top-level namespace when loading rigid transforms" , 0},
00073    {"frame",  'f', "FRAME", 0,  "Set the reference frame in which all initial and query transforms must be defined" , 0},
00074    {0,0,0,0,0,0}
00075  };
00076 
00077  /* Used by main to communicate with parse_opt. */
00078  struct arguments
00079  {
00080    char *args[2];                /* arg1 & arg2 */
00081    bool load_params;
00082    char *ns;
00083    char *reference_frame;
00084  };
00085 
00086  unsigned int nargs_required = 0;
00087  unsigned int nargs_max = 3;
00088 
00089  /* Parse a single option. */
00090  static error_t parse_opt (int key, char *arg, struct argp_state *state)
00091  {
00092          /* Get the input argument from argp_parse, which we
00093          know is a pointer to our arguments structure. */
00094         struct arguments *arguments;
00095         arguments = (struct arguments*) state->input;
00096 
00097         switch (key) {
00098         case 'l':
00099                 arguments->load_params = true;
00100                 break;
00101 
00102         case 'n':
00103                 arguments->ns = arg;
00104                 break;
00105 
00106         case 'f':
00107                 arguments->reference_frame = arg;
00108                 break;
00109 
00110         case ARGP_KEY_ARG:
00111                 if (state->arg_num >= nargs_max)
00112                         /* Too many arguments. */
00113                         argp_usage(state);
00114                 arguments->args[state->arg_num] = arg;
00115                 break;
00116 
00117         case ARGP_KEY_END:
00118                 if (state->arg_num < nargs_required)
00119                         /* Not enough arguments. */
00120                         argp_usage(state);
00121                 break;
00122 
00123         default:
00124                 //arguments->load_params = false;
00125                 return ARGP_ERR_UNKNOWN;
00126         }
00127         return 0;
00128  }
00129 
00130  /* Our argp parser. */
00131  static struct argp argp = { options, parse_opt, args_doc, doc, NULL, NULL, NULL };
00132 
00133 /* ---- */
00134 /* MAIN */
00135 /* ---- */
00136 int main( int argc, char **argv ) {
00137 
00138   ros::init(argc, argv, "manipulation_transforms_server");
00139   /*
00140    * Initialize server solver object
00141    * Reads initialization transforms for solver from parameter
00142    * server, and sets up a pair of service calls for interacting
00143    * with the solver
00144    */
00145 
00146   struct arguments arguments;
00147 
00148   /* Default values. */
00149   arguments.load_params = false;
00150   arguments.ns = (char*)"";
00151   arguments.reference_frame = (char*)"base_footprint";
00152 
00153   argp_parse (&argp, argc, argv, 0, 0, &arguments);
00154 
00155   ROS_INFO("Starting manipulation_transforms_server node with FRAME = %s, NAMESPACE = %s, load params = %s",
00156                    arguments.reference_frame,
00157                    arguments.ns,
00158                    arguments.load_params ? "false" : "false");
00159 
00160   if (ros::isInitialized()) {
00161           if (arguments.load_params) {
00162                   ManipulationTransformsROS server(arguments.reference_frame, arguments.ns);
00163                   ros::spin();
00164           }
00165           else {
00166                   ManipulationTransformsROS server(arguments.reference_frame);
00167                   ros::spin();
00168           }
00169   }
00170   return 0;
00171 }
00172