joint_states_deflater.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, 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 #include <ros/console.h>
37 
38 using namespace std;
39 using namespace joint_states_settler;
40 
41 JointStatesDeflater::JointStatesDeflater()
42 {
43  mapping_.clear();
44 }
45 
46 void JointStatesDeflater::setDeflationJointNames(std::vector<std::string> joint_names)
47 {
48  joint_names_ = joint_names;
49  mapping_.resize(joint_names_.size());
50 }
51 
52 void JointStatesDeflater::deflate(const sensor_msgs::JointStateConstPtr& joint_states, DeflatedJointStates& deflated_elem)
53 {
54  if (joint_states->name.size() != joint_states->position.size()){
55  ROS_ERROR("JointStatesDeflater got invalid joint state message");
56  return;
57  }
58 
59  if (mapping_.size() != joint_names_.size())
60  updateMapping(*joint_states);
61 
62  const unsigned int N = joint_names_.size();
63 
64  deflated_elem.channels_.resize(N);
65 
66  for (unsigned int i=0; i<N; i++)
67  {
68  if ( mapping_[i] >= joint_states->name.size() )
69  updateMapping(*joint_states);
70 
71  if ( joint_states->name[mapping_[i]] != joint_names_[i])
72  updateMapping(*joint_states);
73 
74  deflated_elem.header = joint_states->header;
75  deflated_elem.channels_[i] = joint_states->position[mapping_[i]];
76  deflated_elem.msg_ = joint_states;
77  }
78 }
79 
80 void JointStatesDeflater::prune(const sensor_msgs::JointState& joint_states, sensor_msgs::JointState& pruned_joint_states)
81 {
82  if (joint_states.name.size() != joint_states.position.size())
83  {
84  ROS_ERROR("JointStatesDeflater got invalid joint state message. name's size and position's size don't match.");
85  return;
86  }
87 
88  if (mapping_.size() != joint_names_.size())
89  updateMapping(joint_states);
90 
91  const unsigned int N = joint_names_.size();
92 
93  // Initialize Structures
94  pruned_joint_states.header.stamp = joint_states.header.stamp;
95  pruned_joint_states.name.resize(N);
96  pruned_joint_states.position.resize(N);
97  pruned_joint_states.velocity.clear();
98  pruned_joint_states.effort.clear();
99 
100  for (unsigned int i=0; i<N; i++)
101  {
102  if ( mapping_[i] >= joint_states.name.size() )
103  updateMapping(joint_states);
104 
105  if ( joint_states.name[mapping_[i]] != joint_names_[i])
106  updateMapping(joint_states);
107 
108  pruned_joint_states.name[i] = joint_states.name[mapping_[i]];
109  pruned_joint_states.position[i] = joint_states.position[mapping_[i]];
110 
111  // Currently not copying over velocity or effort. Maybe we should be doing this...
112  }
113 }
114 
115 void JointStatesDeflater::updateMapping(const sensor_msgs::JointState& joint_states)
116 {
117  ROS_DEBUG("Updating the JointStates mapping");
118 
119  const unsigned int N = joint_names_.size();
120 
121  mapping_.resize(N);
122 
123  for (unsigned int i=0; i<N; i++)
124  {
125  bool mapping_found = false;
126  for (unsigned int j=0; j<joint_states.name.size(); j++)
127  {
128  if ( joint_names_[i] == joint_states.name[j])
129  {
130  mapping_[i] = j;
131  mapping_found = true;
132  }
133  }
134  ROS_ERROR_COND(!mapping_found, "Couldn't find mapping for [%s]", joint_names_[i].c_str());
135  }
136 }
#define ROS_ERROR_COND(cond,...)
std_msgs::Header header
std::vector< double > channels_
static const unsigned int N
#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