From 637effc27d107c52dcc3a4fb36edc1ac003ccb3c Mon Sep 17 00:00:00 2001 From: ndehio Date: Wed, 2 Sep 2015 18:29:52 +0200 Subject: [PATCH] Added JointAccelerationsConverter Signed-off-by: ndehio --- cpp/src/CMakeLists.txt | 3 + .../converters/rci/JointAccelerationsConverter.cpp | 110 +++++++++++++++++++++ .../converters/rci/JointAccelerationsConverter.h | 66 +++++++++++++ 3 files changed, 179 insertions(+) create mode 100644 cpp/src/rst/converters/rci/JointAccelerationsConverter.cpp create mode 100644 cpp/src/rst/converters/rci/JointAccelerationsConverter.h diff --git a/cpp/src/CMakeLists.txt b/cpp/src/CMakeLists.txt index c4c7a74..ca512f5 100644 --- a/cpp/src/CMakeLists.txt +++ b/cpp/src/CMakeLists.txt @@ -65,6 +65,7 @@ IF(RCI_FOUND) "rst/converters/rci/JointImpedanceConverter.cpp" "rst/converters/rci/JointTorquesConverter.cpp" "rst/converters/rci/JointVelocitiesConverter.cpp" + "rst/converters/rci/JointAccelerationsConverter.cpp" "rst/converters/rci/LengthConverter.cpp" "rst/converters/rci/PhaseConverter.cpp" "rst/converters/rci/PoseConverter.cpp" @@ -77,6 +78,7 @@ IF(RCI_FOUND) "rst/converters/rci/JointImpedanceConverter.h" "rst/converters/rci/JointTorquesConverter.h" "rst/converters/rci/JointVelocitiesConverter.h" + "rst/converters/rci/JointAccelerationsConverter.h" "rst/converters/rci/LengthConverter.h" "rst/converters/rci/PhaseConverter.h" "rst/converters/rci/PoseConverter.h" @@ -227,6 +229,7 @@ IF(RCI_FOUND) "rst::converters::rci::JointImpedanceConverter" "rst::converters::rci::JointTorquesConverter" "rst::converters::rci::JointVelocitiesConverter" + "rst::converters::rci::JointAccelerationsConverter" "rst::converters::rci::LengthConverter" "rst::converters::rci::PhaseConverter" "rst::converters::rci::PoseConverter" diff --git a/cpp/src/rst/converters/rci/JointAccelerationsConverter.cpp b/cpp/src/rst/converters/rci/JointAccelerationsConverter.cpp new file mode 100644 index 0000000..5b25c40 --- /dev/null +++ b/cpp/src/rst/converters/rci/JointAccelerationsConverter.cpp @@ -0,0 +1,110 @@ +/* ============================================================ + * + * This file is part of the rst-converters project. + * + * Copyright (C) 2015 by Niels Dehio + * + * 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 "JointAccelerationsConverter.h" + +#include +#include + +#include + +#include + +using namespace std; +using namespace nemo; +using namespace rci; + +namespace rst { +namespace converters { +namespace rci { + +JointAccelerationsConverter::JointAccelerationsConverter() : + rsb::converter::Converter( + rsc::runtime::typeName(), + RSB_TYPE_TAG( ::rci::JointAccelerations)) { + converter = boost::shared_ptr >( + new rsb::converter::ProtocolBufferConverter< + rst::kinematics::JointAccelerations>); +} + +JointAccelerationsConverter::~JointAccelerationsConverter() { +} + +string JointAccelerationsConverter::getWireSchema() const { + return converter->getWireSchema(); +} + +string JointAccelerationsConverter::serialize(const rsb::AnnotatedData &data, + string &wire) { + + // Cast to original domain type + boost::shared_ptr< ::rci::JointAccelerations> domain = + boost::static_pointer_cast< ::rci::JointAccelerations>(data.second); + + // Fill protocol buffer object + rst::kinematics::JointAccelerations 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_accelerations(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 JointAccelerationsConverter::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->accelerations_size())); + for (int i = 0; i < proto->accelerations_size(); ++i) { + values[i] = proto->accelerations(i); + } + + // Instantiate domain object + boost::shared_ptr< ::rci::JointAccelerations> domain( + new ::rci::JointAccelerations(values)); + + return rsb::AnnotatedData(getDataType(), domain); + +} + +} +} +} + diff --git a/cpp/src/rst/converters/rci/JointAccelerationsConverter.h b/cpp/src/rst/converters/rci/JointAccelerationsConverter.h new file mode 100644 index 0000000..a529edc --- /dev/null +++ b/cpp/src/rst/converters/rci/JointAccelerationsConverter.h @@ -0,0 +1,66 @@ +/* ============================================================ + * + * This file is part of the rst-converters project. + * + * Copyright (C) 2015 by Niels Dehio + * + * 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::JointAccelerations to JointAccelerations type. + * + * @author ndehio + */ +class RST_CONVERTERS_EXPORT JointAccelerationsConverter: public rsb::converter::Converter { +public: + + JointAccelerationsConverter(); + virtual ~JointAccelerationsConverter(); + + 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