joint_states_settler.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 <sstream>
40 #include <ros/console.h>
41 
42 using namespace joint_states_settler;
43 
45 {
46  configured_ = false;
47 }
48 
49 bool JointStatesSettler::configure(const joint_states_settler::ConfigGoal& goal)
50 {
51  const unsigned int N = goal.joint_names.size();
52 
53  if (N != goal.tolerances.size())
54  {
55  ROS_ERROR("Invalid configuration for JointStatesSettler:\n"
56  " [joint_names.size() == %u] should equal [tolerances.size() == %u]",
57  goal.joint_names.size(), goal.tolerances.size());
58  return false;
59  }
60 
61  deflater_.setDeflationJointNames(goal.joint_names);
62  tol_ = goal.tolerances;
63  max_step_ = goal.max_step;
64  cache_.clear();
65  cache_.setMaxSize(goal.cache_size);
66 
67  std::ostringstream info;
68  ROS_DEBUG("Configuring JointStatesSettler with the following joints:");
69  for (unsigned int i=0; i<N; i++)
70  ROS_DEBUG_STREAM(" " << goal.joint_names[i] << ": " << goal.tolerances[i]);
71 
72  configured_ = true;
73  return true;
74 }
75 
76 calibration_msgs::Interval JointStatesSettler::add(const sensor_msgs::JointStateConstPtr msg)
77 {
78  if (!configured_)
79  {
80  ROS_WARN("Not yet configured. Going to skip");
81  return calibration_msgs::Interval();
82  }
83 
85  deflater_.deflate(msg, *deflated);
86  cache_.add(deflated);
87 
88  DeflatedCache* bare_cache = (DeflatedCache*)(&cache_);
89 
90  calibration_msgs::Interval interval = settlerlib::IntervalCalc::computeLatestInterval(*bare_cache, tol_, max_step_);
91 
92  return interval;
93 }
94 
95 sensor_msgs::JointState JointStatesSettler::pruneJointState(const sensor_msgs::JointStateConstPtr msg)
96 {
97  sensor_msgs::JointState pruned;
98  deflater_.prune(*msg, pruned);
99  return pruned;
100 }
void prune(const sensor_msgs::JointState &joint_states, sensor_msgs::JointState &pruned_joint_states)
Remove all the joints that we don&#39;t care about.
bool configure(const joint_states_settler::ConfigGoal &goal)
calibration_msgs::Interval add(const sensor_msgs::JointStateConstPtr msg)
void setMaxSize(unsigned int max_size)
#define ROS_WARN(...)
void deflate(const sensor_msgs::JointStateConstPtr &joint_states, DeflatedJointStates &deflated_elem)
Perform the deflation on a joint_states message.
sensor_msgs::JointState pruneJointState(const sensor_msgs::JointStateConstPtr msg)
void add(const M &msg)
#define ROS_DEBUG_STREAM(args)
void setDeflationJointNames(std::vector< std::string > joint_names)
Specify which joints to extract.
static const unsigned int N
static calibration_msgs::Interval computeLatestInterval(const SortedDeque< DeflatedConstPtr > &signal, const std::vector< double > &tolerances, ros::Duration max_spacing)
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


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