joint_states_settler_action.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2009, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
36 
37 #include <boost/thread.hpp>
38 #include <ros/ros.h>
40 
42 #include <joint_states_settler/ConfigAction.h>
43 #include <calibration_msgs/Interval.h>
44 #include <sensor_msgs/JointState.h>
45 
46 using namespace joint_states_settler;
47 
49 {
50 public:
51  JointStatesSettlerAction() : as_("settler_config", false)
52  {
53  as_.registerGoalCallback( boost::bind(&JointStatesSettlerAction::goalCallback, this) );
54  as_.registerPreemptCallback( boost::bind(&JointStatesSettlerAction::preemptCallback, this) );
55  interval_pub_ = nh_.advertise<calibration_msgs::Interval>("settled_interval", 1);
56  pruned_pub_ = nh_.advertise<sensor_msgs::JointState>("chain_state", 1);
57  sub_ = nh_.subscribe("joint_states", 1, &JointStatesSettlerAction::jointStatesCallback, this);
58  as_.start();
59  }
60 
61  void goalCallback()
62  {
63  boost::mutex::scoped_lock lock(run_mutex_);
64 
65  // Stop the previously running goal (if it exists)
66  if (as_.isActive())
67  as_.setPreempted();
68 
69  // Get the new goal from the action server
70  joint_states_settler::ConfigGoalConstPtr goal = as_.acceptNewGoal();
71  assert(goal);
72 
73  // Try to reconfigure the settler object
74  bool success = settler_.configure(*goal);
75 
76  // Detect possible failure
77  if (!success)
78  as_.setAborted();
79  }
80 
82  {
83  boost::mutex::scoped_lock lock(run_mutex_);
84 
85  // Don't need to do any cleanup. Immeadiately turn it off
86  as_.setPreempted();
87  }
88 
89  void jointStatesCallback(const sensor_msgs::JointStateConstPtr& msg)
90  {
91  boost::mutex::scoped_lock lock(run_mutex_);
92 
93  // Don't do anything if we're not actually running
94  if (!as_.isActive())
95  return;
96 
97  // Add the joint state message, and get the latest processed settled interval
98  calibration_msgs::Interval interval = settler_.add(msg);
99  interval_pub_.publish(interval);
100 
101  // Build the joint state message for this subset of joints
102  pruned_pub_.publish(settler_.pruneJointState(msg));
103  }
104 
105 private:
106  boost::mutex run_mutex_;
109 
114 };
115 
116 int main(int argc, char** argv)
117 {
118  ros::init(argc, argv, "joint_states_settler_action");
119  ros::NodeHandle n;
120  JointStatesSettlerAction settler_action;
121  ros::spin();
122  return 0;
123 }
void jointStatesCallback(const sensor_msgs::JointStateConstPtr &msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin()
int main(int argc, char **argv)
actionlib::SimpleActionServer< joint_states_settler::ConfigAction > as_


joint_states_settler
Author(s): Vijay Pradeep, Eitan Marder-Eppstein
autogenerated on Thu Jun 6 2019 19:17:25