From 3b64b749a951b05b5255ce711ed3059d4c963129 Mon Sep 17 00:00:00 2001 From: ndehio Date: Mon, 1 Jun 2015 11:39:32 +0200 Subject: [PATCH] added rci converter for JointVelocities Signed-off-by: ndehio --- cpp/src/CMakeLists.txt | 3 + .../converters/rci/JointVelocitiesConverter.cpp | 109 +++++++++++++++++++++ .../rst/converters/rci/JointVelocitiesConverter.h | 65 ++++++++++++ 3 files changed, 177 insertions(+) create mode 100644 cpp/src/rst/converters/rci/JointVelocitiesConverter.cpp create mode 100644 cpp/src/rst/converters/rci/JointVelocitiesConverter.h diff --git a/cpp/src/CMakeLists.txt b/cpp/src/CMakeLists.txt index 2464f33..c4c7a74 100644 --- a/cpp/src/CMakeLists.txt +++ b/cpp/src/CMakeLists.txt @@ -64,6 +64,7 @@ IF(RCI_FOUND) LIST(APPEND SOURCES "rst/converters/rci/JointAnglesConverter.cpp" "rst/converters/rci/JointImpedanceConverter.cpp" "rst/converters/rci/JointTorquesConverter.cpp" + "rst/converters/rci/JointVelocitiesConverter.cpp" "rst/converters/rci/LengthConverter.cpp" "rst/converters/rci/PhaseConverter.cpp" "rst/converters/rci/PoseConverter.cpp" @@ -75,6 +76,7 @@ IF(RCI_FOUND) LIST(APPEND HEADERS "rst/converters/rci/JointAnglesConverter.h" "rst/converters/rci/JointImpedanceConverter.h" "rst/converters/rci/JointTorquesConverter.h" + "rst/converters/rci/JointVelocitiesConverter.h" "rst/converters/rci/LengthConverter.h" "rst/converters/rci/PhaseConverter.h" "rst/converters/rci/PoseConverter.h" @@ -224,6 +226,7 @@ IF(RCI_FOUND) LIST(APPEND RCI_CONVERTERS "rst::converters::rci::JointAnglesConverter" "rst::converters::rci::JointImpedanceConverter" "rst::converters::rci::JointTorquesConverter" + "rst::converters::rci::JointVelocitiesConverter" "rst::converters::rci::LengthConverter" "rst::converters::rci::PhaseConverter" "rst::converters::rci::PoseConverter" diff --git a/cpp/src/rst/converters/rci/JointVelocitiesConverter.cpp b/cpp/src/rst/converters/rci/JointVelocitiesConverter.cpp new file mode 100644 index 0000000..4832807 --- /dev/null +++ b/cpp/src/rst/converters/rci/JointVelocitiesConverter.cpp @@ -0,0 +1,109 @@ +/* ============================================================ + * + * This file is part of the rst-converters project. + * + * Copyright (C) 2010 by Sebastian Wrede + * + * This file may be licensed under the terms of the + * GNU Lesser General Public License Version 3 (the ``LGPL''), + * or (at your option) any later version. + * + * Software distributed under the License is distributed + * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either + * express or implied. See the LGPL for the specific language + * governing rights and limitations. + * + * You should have received a copy of the LGPL along with this + * program. If not, go to http://www.gnu.org/licenses/lgpl.html + * or write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * + * The development of this software was supported by: + * CoR-Lab, Research Institute for Cognition and Robotics + * Bielefeld University + * + * ============================================================ */ + +#include "JointVelocitiesConverter.h" + +#include +#include + +#include + +#include + +using namespace std; +using namespace nemo; +using namespace rci; + +namespace rst { +namespace converters { +namespace rci { + +JointVelocitiesConverter::JointVelocitiesConverter() : + rsb::converter::Converter( + rsc::runtime::typeName(), + RSB_TYPE_TAG( ::rci::JointVelocities)) { + converter = boost::shared_ptr >( + new rsb::converter::ProtocolBufferConverter< + rst::kinematics::JointVelocities>); +} + +JointVelocitiesConverter::~JointVelocitiesConverter() { +} + +string JointVelocitiesConverter::getWireSchema() const { + return converter->getWireSchema(); +} + +string JointVelocitiesConverter::serialize(const rsb::AnnotatedData &data, + string &wire) { + + // Cast to original domain type + boost::shared_ptr< ::rci::JointVelocities> domain = boost::static_pointer_cast< + ::rci::JointVelocities>(data.second); + + // Fill protocol buffer object + rst::kinematics::JointVelocities proto; + + // 1. reserve() + // 2. std::copy(begin, begin+n) // oder memcpy + + // TODO Check if this can be done more efficiently (see protobuf C++ docs for repeated numeric fields) + for (unsigned int i = 0; i < domain->getDimension(); ++i) { + proto.add_velocities(domain->asDouble(i)); + } + + // Use embedded ProtoBuf converter for serialization to wire + return converter->serialize( + make_pair(rsc::runtime::typeName(), + boost::shared_ptr(&proto, rsc::misc::NullDeleter())), + wire); +} + +rsb::AnnotatedData JointVelocitiesConverter::deserialize( + const std::string &wireType, const std::string &wire) { + + // Deserialize and cast to specific ProtoBuf type + boost::shared_ptr proto = + boost::static_pointer_cast( + converter->deserialize(wireType, wire).second); + + // Read domain data from ProtoBuf + RealVector values = RealVector(dim(proto->velocities_size())); + for (int i = 0; i < proto->velocities_size(); ++i) { + values[i] = proto->velocities(i); + } + + // Instantiate domain object + boost::shared_ptr< ::rci::JointVelocities> domain( + new ::rci::JointVelocities(values)); + + return rsb::AnnotatedData(getDataType(), domain); + +} + +} +} +} diff --git a/cpp/src/rst/converters/rci/JointVelocitiesConverter.h b/cpp/src/rst/converters/rci/JointVelocitiesConverter.h new file mode 100644 index 0000000..63fafd8 --- /dev/null +++ b/cpp/src/rst/converters/rci/JointVelocitiesConverter.h @@ -0,0 +1,65 @@ +/* ============================================================ + * + * This file is part of the rst-converters project. + * + * Copyright (C) 2010 by Sebastian Wrede + * + * This file may be licensed under the terms of the + * GNU Lesser General Public License Version 3 (the ``LGPL''), + * or (at your option) any later version. + * + * Software distributed under the License is distributed + * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either + * express or implied. See the LGPL for the specific language + * governing rights and limitations. + * + * You should have received a copy of the LGPL along with this + * program. If not, go to http://www.gnu.org/licenses/lgpl.html + * or write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + * + * The development of this software was supported by: + * CoR-Lab, Research Institute for Cognition and Robotics + * Bielefeld University + * + * ============================================================ */ + +#pragma once + +#include +#include + +#include + +#include "rst/rstconvertersexports.h" + +namespace rst { +namespace converters { +namespace rci { + +/** + * Converter for rci::JointVelocities to JointVelocities type. + * + * @author swrede + */ +class RST_CONVERTERS_EXPORT JointVelocitiesConverter: public rsb::converter::Converter { +public: + + JointVelocitiesConverter(); + virtual ~JointVelocitiesConverter(); + + std::string getWireSchema() const; + + std::string serialize(const rsb::AnnotatedData &data, std::string &wire); + rsb::AnnotatedData deserialize(const std::string &wireType, + const std::string &wire); + +private: + + ::boost::shared_ptr > converter; + +}; + +} +} +} -- 1.9.1