joint_states_settler: joint_states_settler: joint_states_settler

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, 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 
00036 
00037 #include <cstdio>
00038 #include <ros/ros.h>
00039 #include <joint_states_settler/joint_states_settler.h>
00040 
00041 using namespace joint_states_settler;
00042 
00048 joint_states_settler::ConfigGoal getParamConfig(ros::NodeHandle& n)
00049 {
00050   joint_states_settler::ConfigGoal config;
00051   config.joint_names.clear() ;
00052   bool found_joint = true ;
00053   int joint_count = 0 ;
00054 
00055   char param_buf[1024] ;
00056   while(found_joint)
00057   {
00058     sprintf(param_buf, "~joint_name_%02u", joint_count) ;
00059     std::string param_name = param_buf ;
00060     std::string cur_joint_name ;
00061     found_joint = n.getParam(param_name, cur_joint_name) ;
00062     if (found_joint)
00063     {
00064       ROS_INFO("[%s] -> %s", param_name.c_str(), cur_joint_name.c_str()) ;
00065       config.joint_names.push_back(cur_joint_name) ;
00066 
00067       // Get the joint tolerance only if we found the joint name
00068       sprintf(param_buf, "~joint_tol_%02u", joint_count) ;
00069       double cur_tol;
00070       bool found_tol;
00071       found_tol = n.getParam(std::string(param_buf), cur_tol) ;
00072       if (!found_tol)
00073         ROS_FATAL("Could not find parameter [%s]", param_buf);
00074       ROS_INFO("[%s]  -> %.3f", param_buf, cur_tol) ;
00075       config.tolerances.push_back(cur_tol);
00076     }
00077     else
00078       ROS_DEBUG("Did not find param [%s]", param_name.c_str()) ;
00079 
00080     joint_count++ ;
00081   }
00082 
00083   double max_step;
00084   n.param("~max_step", max_step, 1000.0);
00085   config.max_step = ros::Duration(max_step);
00086   ROS_INFO("max step: [%.3fs]", config.max_step.toSec());
00087 
00088   int cache_size;
00089   n.param("~cache_size", cache_size, 1000);
00090   if (cache_size < 0)
00091     ROS_FATAL("cache_size < 0. (cache_size==%i)", cache_size);
00092   config.cache_size = (unsigned int) cache_size;
00093   ROS_INFO("cache_size: [%u]", config.cache_size);
00094 
00095   return config;
00096 }
00097 
00098 void jointStatesCallback(ros::Publisher* pub, JointStatesSettler* settler, const sensor_msgs::JointStateConstPtr& msg)
00099 {
00100   pub->publish(settler->add(msg));
00101 }
00102 
00103 int main(int argc, char** argv)
00104 {
00105   ros::init(argc, argv, "joint_states_settler");
00106 
00107   ros::NodeHandle n;
00108 
00109   // Set up the JointStatesSettler
00110   joint_states_settler::ConfigGoal config = getParamConfig(n);
00111   JointStatesSettler settler;
00112   settler.configure(config);
00113 
00114   // Output
00115   ros::Publisher pub = n.advertise<calibration_msgs::Interval>("settled", 1);
00116 
00117   // Input
00118 
00119   boost::function<void (const sensor_msgs::JointStateConstPtr&)> cb = boost::bind(&jointStatesCallback, &pub, &settler, _1);
00120   ros::Subscriber sub = n.subscribe(std::string("joint_states"), 1, cb);
00121 
00122   ros::spin();
00123 }
joint_states_settler_node.cpp
