joint_states_settler_action.cpp
Go to the documentation of this file.
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 <boost/thread.hpp>
00038 #include <ros/ros.h>
00039 #include <actionlib/server/simple_action_server.h>
00040 
00041 #include <joint_states_settler/joint_states_settler.h>
00042 #include <joint_states_settler/ConfigAction.h>
00043 #include <calibration_msgs/Interval.h>
00044 #include <sensor_msgs/JointState.h>
00045 
00046 using namespace joint_states_settler;
00047 
00048 class JointStatesSettlerAction
00049 {
00050 public:
00051   JointStatesSettlerAction() : as_("settler_config", false)
00052   {
00053     as_.registerGoalCallback( boost::bind(&JointStatesSettlerAction::goalCallback, this) );
00054     as_.registerPreemptCallback( boost::bind(&JointStatesSettlerAction::preemptCallback, this) );
00055     interval_pub_ = nh_.advertise<calibration_msgs::Interval>("settled_interval", 1);
00056     pruned_pub_ = nh_.advertise<sensor_msgs::JointState>("chain_state", 1);
00057     sub_ = nh_.subscribe("joint_states", 1, &JointStatesSettlerAction::jointStatesCallback, this);
00058     as_.start();
00059   }
00060 
00061   void goalCallback()
00062   {
00063     boost::mutex::scoped_lock lock(run_mutex_);
00064 
00065     // Stop the previously running goal (if it exists)
00066     if (as_.isActive())
00067       as_.setPreempted();
00068 
00069     // Get the new goal from the action server
00070     joint_states_settler::ConfigGoalConstPtr goal = as_.acceptNewGoal();
00071     assert(goal);
00072 
00073     // Try to reconfigure the settler object
00074     bool success = settler_.configure(*goal);
00075 
00076     // Detect possible failure
00077     if (!success)
00078       as_.setAborted();
00079   }
00080 
00081   void preemptCallback()
00082   {
00083     boost::mutex::scoped_lock lock(run_mutex_);
00084 
00085     // Don't need to do any cleanup. Immeadiately turn it off
00086     as_.setPreempted();
00087   }
00088 
00089   void jointStatesCallback(const sensor_msgs::JointStateConstPtr& msg)
00090   {
00091     boost::mutex::scoped_lock lock(run_mutex_);
00092 
00093     // Don't do anything if we're not actually running
00094     if (!as_.isActive())
00095       return;
00096 
00097     // Add the joint state message, and get the latest processed settled interval
00098     calibration_msgs::Interval interval = settler_.add(msg);
00099     interval_pub_.publish(interval);
00100 
00101     // Build the joint state message for this subset of joints
00102     pruned_pub_.publish(settler_.pruneJointState(msg));
00103   }
00104 
00105 private:
00106   boost::mutex run_mutex_;
00107   actionlib::SimpleActionServer<joint_states_settler::ConfigAction> as_;
00108   JointStatesSettler settler_;
00109 
00110   ros::Publisher interval_pub_;
00111   ros::Publisher pruned_pub_;
00112   ros::Subscriber sub_;
00113   ros::NodeHandle nh_;
00114 };
00115 
00116 int main(int argc, char** argv)
00117 {
00118   ros::init(argc, argv, "joint_states_settler_action");
00119   ros::NodeHandle n;
00120   JointStatesSettlerAction settler_action;
00121   ros::spin();
00122   return 0;
00123 }


joint_states_settler
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Sat Jun 8 2019 19:41:54