Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22891] pkg/trunk
|
From: <vij...@us...> - 2009-08-25 21:12:34
|
Revision: 22891
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22891&view=rev
Author: vijaypradeep
Date: 2009-08-25 21:12:26 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
JointStatesDeflater passes unit tests
Modified Paths:
--------------
pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt
pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml
pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/deflated.h
pkg/trunk/stacks/calibration/settlerlib/manifest.xml
Added Paths:
-----------
pkg/trunk/stacks/calibration/joint_states_settler/include/
pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/
pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h
pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h
pkg/trunk/stacks/calibration/joint_states_settler/src/
pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp
pkg/trunk/stacks/calibration/joint_states_settler/test/
pkg/trunk/stacks/calibration/joint_states_settler/test/CMakeLists.txt
pkg/trunk/stacks/calibration/joint_states_settler/test/joint_states_deflater_unittest.cpp
Removed Paths:
-------------
pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h
pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/src/joint_states_deflater.cpp
pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/test/joint_states_deflater_unittest.cpp
Deleted: pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h 2009-08-25 21:12:26 UTC (rev 22891)
@@ -1,67 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef CALIBRATION_MESSAGE_FILTERS_JOINT_STATES_DEFLATER_H_
-#define CALIBRATION_MESSAGE_FILTERS_JOINT_STATES_DEFLATER_H_
-
-#include "calibration_message_filters/deflated.h"
-#include "mechanism_msgs/JointStates.h"
-
-namespace calibration_message_filters
-{
-
-
-class JointStatesDeflater
-{
-public:
- typedef DeflatedMsg<mechanism_msgs::JointStates> DeflatedJointStates;
-
- JointStatesDeflater();
-
- void setDeflationJointNames(std::vector<std::string> joint_names);
- void deflate(const mechanism_msgs::JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem);
-
-private:
- std::vector<unsigned int> mapping_;
- std::vector<std::string> joint_names_;
-
- void updateMapping(const mechanism_msgs::JointStates& joint_states);
-
-};
-
-}
-
-
-
-#endif
Deleted: pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/src/joint_states_deflater.cpp
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/src/joint_states_deflater.cpp 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/src/joint_states_deflater.cpp 2009-08-25 21:12:26 UTC (rev 22891)
@@ -1,96 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#include "calibration_message_filters/joint_states_deflater.h"
-
-using namespace std;
-using namespace calibration_message_filters;
-using namespace mechanism_msgs;
-
-JointStatesDeflater::JointStatesDeflater()
-{
- mapping_.clear();
-}
-
-void JointStatesDeflater::setDeflationJointNames(std::vector<std::string> joint_names)
-{
- joint_names_ = joint_names;
- mapping_.resize(joint_names_.size());
-}
-
-void JointStatesDeflater::deflate(const JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem)
-{
- if (mapping_.size() != joint_names_.size())
- updateMapping(*joint_states);
-
- const unsigned int N = joint_names_.size();
-
- deflated_elem.deflated_.resize(N);
-
- for (unsigned int i=0; i<N; i++)
- {
- if ( mapping_[i] >= joint_states->joints.size() )
- updateMapping(*joint_states);
-
- if ( joint_states->joints[mapping_[i]].name != joint_names_[i])
- updateMapping(*joint_states);
-
- deflated_elem.header = joint_states->header;
- deflated_elem.deflated_[i] = joint_states->joints[mapping_[i]].position;
- deflated_elem.msg = joint_states;
- }
-}
-
-void JointStatesDeflater::updateMapping(const JointStates& joint_states)
-{
- ROS_DEBUG("Updating the JointStates mapping");
-
- const unsigned int N = joint_names_.size();
-
- mapping_.resize(N);
-
- for (unsigned int i=0; i<N; i++)
- {
- bool mapping_found = false;
- for (unsigned int j=0; j<joint_states.joints.size(); j++)
- {
- if ( joint_names_[i] == joint_states.joints[j].name)
- {
- mapping_[i] = j;
- mapping_found = true;
- }
- }
- ROS_ERROR_COND(!mapping_found, "Couldn't find mapping for [%s]", joint_names_[i].c_str());
- }
-}
Deleted: pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/test/joint_states_deflater_unittest.cpp
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/test/joint_states_deflater_unittest.cpp 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/test/joint_states_deflater_unittest.cpp 2009-08-25 21:12:26 UTC (rev 22891)
@@ -1,119 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-
-#include <gtest/gtest.h>
-
-#include "calibration_message_filters/joint_states_deflater.h"
-
-using namespace std;
-using namespace calibration_message_filters;
-using namespace mechanism_msgs;
-
-
-static const double eps = 1e-10;
-
-TEST(JointStatesDeflator, easy1)
-{
- JointStatesDeflater deflater;
-
- vector<string> joint_names;
- joint_names.resize(2);
- joint_names[0] = "A";
- joint_names[1] = "C";
- deflater.setDeflationJointNames(joint_names);
-
- JointStatesPtr joint_states;
- DeflatedMsg<JointStates> deflated_msg;
-
- // ***********************************************
-
- joint_states.reset(new JointStates);
- joint_states->joints.resize(3);
- joint_states->joints[0].name = "A";
- joint_states->joints[1].name = "B";
- joint_states->joints[2].name = "C";
- joint_states->joints[0].position = 1.0;
- joint_states->joints[1].position = 2.0;
- joint_states->joints[2].position = 3.0;
-
- deflater.deflate(joint_states, deflated_msg);
-
- EXPECT_EQ(deflated_msg.deflated_.size(), (unsigned int) 2);
- EXPECT_NEAR(deflated_msg.deflated_[0], 1.0, eps);
- EXPECT_NEAR(deflated_msg.deflated_[1], 3.0, eps);
-
- // ***********************************************
-
- joint_states.reset(new JointStates);
- joint_states->joints.resize(3);
- joint_states->joints[0].name = "C";
- joint_states->joints[1].name = "A";
- joint_states->joints[2].name = "B";
- joint_states->joints[0].position = 4.0;
- joint_states->joints[1].position = 5.0;
- joint_states->joints[2].position = 6.0;
-
- deflater.deflate(joint_states, deflated_msg);
-
- EXPECT_EQ(deflated_msg.deflated_.size(), (unsigned int) 2);
- EXPECT_NEAR(deflated_msg.deflated_[0], 5.0, eps);
- EXPECT_NEAR(deflated_msg.deflated_[1], 4.0, eps);
-
- // ***********************************************
-
- joint_states.reset(new JointStates);
- joint_states->joints.resize(4);
- joint_states->joints[0].name = "D";
- joint_states->joints[1].name = "C";
- joint_states->joints[2].name = "B";
- joint_states->joints[3].name = "A";
- joint_states->joints[0].position = 7.0;
- joint_states->joints[1].position = 8.0;
- joint_states->joints[2].position = 9.0;
- joint_states->joints[3].position = 10.0;
-
- deflater.deflate(joint_states, deflated_msg);
-
- EXPECT_EQ(deflated_msg.deflated_.size(), (unsigned int) 2);
- EXPECT_NEAR(deflated_msg.deflated_[0], 10.0, eps);
- EXPECT_NEAR(deflated_msg.deflated_[1], 8.0, eps);
-
-}
-
-
-int main(int argc, char **argv){
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
Modified: pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt 2009-08-25 21:12:26 UTC (rev 22891)
@@ -11,20 +11,18 @@
rosbuild_init()
-#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
+# rosbuild_genmsg()
+# rosbuild_gensrv()
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
+rosbuild_add_library(${PROJECT_NAME} src/joint_states_deflater.cpp)
+
+# target_link_libraries(${PROJECT_NAME} another_library)
+# rosbuild_add_boost_directories()
+# rosbuild_link_boost(${PROJECT_NAME} thread)
+# rosbuild_add_executable(example examples/example.cpp)
+# target_link_libraries(example ${PROJECT_NAME})
+
+add_subdirectory(test EXCLUDE_FROM_ALL)
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h (from rev 22890, pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h)
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h 2009-08-25 21:12:26 UTC (rev 22891)
@@ -0,0 +1,52 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef JOINT_STATES_SETTLER_DEFLATED_JOINT_STATES_H_
+#define JOINT_STATES_SETTLER_DEFLATED_JOINT_STATES_H_
+
+#include <mechanism_msgs/JointStates.h>
+#include <settlerlib/deflated.h>
+
+namespace joint_states_settler
+{
+
+class DeflatedJointStates : public settlerlib::Deflated
+{
+public:
+ mechanism_msgs::JointStatesConstPtr msg_;
+};
+
+}
+
+#endif
Copied: pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h (from rev 22890, pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h)
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h 2009-08-25 21:12:26 UTC (rev 22891)
@@ -0,0 +1,87 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef JOINT_STATES_SETTLER_JOINT_STATES_DEFLATER_H_
+#define JOINT_STATES_SETTLER_JOINT_STATES_DEFLATER_H_
+
+#include <mechanism_msgs/JointStates.h>
+#include <settlerlib/deflated.h>
+#include "deflated_joint_states.h"
+
+namespace joint_states_settler
+{
+
+/**
+ * \brief Given a set a joint names, efficiently extracts a subset of joint positions
+ *
+ * This class is generally more efficient than other methods, because it caches the the mapping
+ * from the incoming joint_states message to the deflated vector. It also updates the mapping
+ * whenever the ordering in joint_states changes
+ **/
+class JointStatesDeflater
+{
+public:
+
+ JointStatesDeflater();
+
+ /**
+ * \brief Specify which joints to extract
+ * \param joint_names Ordered list of joint names to extract
+ */
+ void setDeflationJointNames(std::vector<std::string> joint_names);
+
+ /**
+ * \brief Perform the deflation on a joint_states message
+ * \param joint_states Incoming JointStates message
+ * \param Ouput datatype. Stores the deflated data, along with the original joint states message
+ */
+ void deflate(const mechanism_msgs::JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem);
+
+private:
+ std::vector<unsigned int> mapping_;
+ std::vector<std::string> joint_names_;
+
+ /**
+ * \brief Given a stereotypical JointStates message, computes the mapping
+ * from JointStates to the deflated data
+ */
+ void updateMapping(const mechanism_msgs::JointStates& joint_states);
+
+};
+
+}
+
+
+
+#endif
Modified: pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml 2009-08-25 21:12:26 UTC (rev 22891)
@@ -11,6 +11,7 @@
<depend package="roscpp"/>
<depend package="actionlib"/>
<depend package="mechanism_msgs"/>
+ <depend package="settlerlib"/>
</package>
Copied: pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp (from rev 22890, pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/src/joint_states_deflater.cpp)
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp 2009-08-25 21:12:26 UTC (rev 22891)
@@ -0,0 +1,95 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "joint_states_settler/joint_states_deflater.h"
+
+using namespace std;
+using namespace joint_states_settler;
+
+JointStatesDeflater::JointStatesDeflater()
+{
+ mapping_.clear();
+}
+
+void JointStatesDeflater::setDeflationJointNames(std::vector<std::string> joint_names)
+{
+ joint_names_ = joint_names;
+ mapping_.resize(joint_names_.size());
+}
+
+void JointStatesDeflater::deflate(const mechanism_msgs::JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem)
+{
+ if (mapping_.size() != joint_names_.size())
+ updateMapping(*joint_states);
+
+ const unsigned int N = joint_names_.size();
+
+ deflated_elem.channels_.resize(N);
+
+ for (unsigned int i=0; i<N; i++)
+ {
+ if ( mapping_[i] >= joint_states->joints.size() )
+ updateMapping(*joint_states);
+
+ if ( joint_states->joints[mapping_[i]].name != joint_names_[i])
+ updateMapping(*joint_states);
+
+ deflated_elem.header = joint_states->header;
+ deflated_elem.channels_[i] = joint_states->joints[mapping_[i]].position;
+ deflated_elem.msg_ = joint_states;
+ }
+}
+
+void JointStatesDeflater::updateMapping(const mechanism_msgs::JointStates& joint_states)
+{
+ ROS_DEBUG("Updating the JointStates mapping");
+
+ const unsigned int N = joint_names_.size();
+
+ mapping_.resize(N);
+
+ for (unsigned int i=0; i<N; i++)
+ {
+ bool mapping_found = false;
+ for (unsigned int j=0; j<joint_states.joints.size(); j++)
+ {
+ if ( joint_names_[i] == joint_states.joints[j].name)
+ {
+ mapping_[i] = j;
+ mapping_found = true;
+ }
+ }
+ ROS_ERROR_COND(!mapping_found, "Couldn't find mapping for [%s]", joint_names_[i].c_str());
+ }
+}
Added: pkg/trunk/stacks/calibration/joint_states_settler/test/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/test/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/test/CMakeLists.txt 2009-08-25 21:12:26 UTC (rev 22891)
@@ -0,0 +1,6 @@
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
+
+# ********** Tests **********
+rospack_add_gtest(test/joint_states_deflater_unittest joint_states_deflater_unittest.cpp)
+target_link_libraries(test/joint_states_deflater_unittest joint_states_settler)
+
Copied: pkg/trunk/stacks/calibration/joint_states_settler/test/joint_states_deflater_unittest.cpp (from rev 22890, pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/test/joint_states_deflater_unittest.cpp)
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/test/joint_states_deflater_unittest.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/test/joint_states_deflater_unittest.cpp 2009-08-25 21:12:26 UTC (rev 22891)
@@ -0,0 +1,115 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <gtest/gtest.h>
+#include "joint_states_settler/joint_states_deflater.h"
+
+using namespace std;
+using namespace joint_states_settler;
+using namespace mechanism_msgs;
+
+static const double eps = 1e-10;
+
+TEST(JointStatesDeflator, easy1)
+{
+ JointStatesDeflater deflater;
+
+ vector<string> joint_names;
+ joint_names.resize(2);
+ joint_names[0] = "A";
+ joint_names[1] = "C";
+ deflater.setDeflationJointNames(joint_names);
+
+ JointStatesPtr joint_states;
+ DeflatedJointStates deflated_msg;
+
+ // ***********************************************
+
+ joint_states.reset(new JointStates);
+ joint_states->joints.resize(3);
+ joint_states->joints[0].name = "A";
+ joint_states->joints[1].name = "B";
+ joint_states->joints[2].name = "C";
+ joint_states->joints[0].position = 1.0;
+ joint_states->joints[1].position = 2.0;
+ joint_states->joints[2].position = 3.0;
+
+ deflater.deflate(joint_states, deflated_msg);
+
+ EXPECT_EQ(deflated_msg.channels_.size(), (unsigned int) 2);
+ EXPECT_NEAR(deflated_msg.channels_[0], 1.0, eps);
+ EXPECT_NEAR(deflated_msg.channels_[1], 3.0, eps);
+
+ // ***********************************************
+
+ joint_states.reset(new JointStates);
+ joint_states->joints.resize(3);
+ joint_states->joints[0].name = "C";
+ joint_states->joints[1].name = "A";
+ joint_states->joints[2].name = "B";
+ joint_states->joints[0].position = 4.0;
+ joint_states->joints[1].position = 5.0;
+ joint_states->joints[2].position = 6.0;
+
+ deflater.deflate(joint_states, deflated_msg);
+
+ EXPECT_EQ(deflated_msg.channels_.size(), (unsigned int) 2);
+ EXPECT_NEAR(deflated_msg.channels_[0], 5.0, eps);
+ EXPECT_NEAR(deflated_msg.channels_[1], 4.0, eps);
+
+ // ***********************************************
+
+ joint_states.reset(new JointStates);
+ joint_states->joints.resize(4);
+ joint_states->joints[0].name = "D";
+ joint_states->joints[1].name = "C";
+ joint_states->joints[2].name = "B";
+ joint_states->joints[3].name = "A";
+ joint_states->joints[0].position = 7.0;
+ joint_states->joints[1].position = 8.0;
+ joint_states->joints[2].position = 9.0;
+ joint_states->joints[3].position = 10.0;
+
+ deflater.deflate(joint_states, deflated_msg);
+
+ EXPECT_EQ(deflated_msg.channels_.size(), (unsigned int) 2);
+ EXPECT_NEAR(deflated_msg.channels_[0], 10.0, eps);
+ EXPECT_NEAR(deflated_msg.channels_[1], 8.0, eps);
+}
+
+
+int main(int argc, char **argv){
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
Modified: pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/deflated.h
===================================================================
--- pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/deflated.h 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/deflated.h 2009-08-25 21:12:26 UTC (rev 22891)
@@ -45,7 +45,7 @@
{
public:
roslib::Header header; // Need header to put in cache
- std::vector<double> channel_;
+ std::vector<double> channels_;
};
}
Modified: pkg/trunk/stacks/calibration/settlerlib/manifest.xml
===================================================================
--- pkg/trunk/stacks/calibration/settlerlib/manifest.xml 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/stacks/calibration/settlerlib/manifest.xml 2009-08-25 21:12:26 UTC (rev 22891)
@@ -9,6 +9,11 @@
<url>http://pr.willowgarage.com/wiki/settlerlib</url>
<depend package="roscpp"/>
+ <export>
+ <cpp cflags="-I${prefix}/include" />
+ </export>
+
+
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
