...
 
Commits (2)
Subproject commit 0d8dc2961e6caee5f236fcab2918efd37d247953
# ---> Eclipse
*.pydevproject
.metadata
.gradle
bin/
tmp/
*.tmp
*.bak
*.swp
*~.nib
local.properties
.settings/
.loadpath
# Eclipse Core
.project
# External tool builders
.externalToolBuilders/
# Locally stored "Eclipse launch configurations"
*.launch
# CDT-specific
.cproject
# JDT-specific (Eclipse Java Development Tools)
.classpath
# Java annotation processor (APT)
.factorypath
# PDT-specific
.buildpath
# sbteclipse plugin
.target
# TeXlipse plugin
.texlipse
# ---> C++
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
cmake_minimum_required(VERSION 2.8.3)
project(emros_common)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
utility_ofc
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS roscpp
)
###########
## Build ##
###########
## Specify additional locations of header files
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(${PROJECT_NAME} INTERFACE
)
#############
## Install ##
#############
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
\ No newline at end of file
MIT License
Copyright (c) <year> <copyright holders>
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
/*
* wheel_drive_config.h
*
* Created on: May 14, 2018
* Author: sponfeldner_c
*/
//#pragma once
#ifndef EMROS_EMROSCOMMON_INC_WHEEL_DRIVE_CONFIG_H_
#define EMROS_EMROSCOMMON_INC_WHEEL_DRIVE_CONFIG_H_
//////////////////////KINEMATICS/////////////////////////////////////////////
#define DEF_2WD
#endif /* EMROS_EMROSCOMMON_INC_WHEEL_DRIVE_CONFIG_H_ */
/*
* emros_commands.h
*
* Created on: Apr 12, 2016
* Author: Sydney Speckle
* Mail: sydney.speckle@nanotec.de
*/
#pragma once
#ifdef MTKERNEL
#include "../../../utility_ofc/include/utility_ofc/ethprotocol.h"
#include "../../../basecontroller/inc/basecontroller_types.h"
#else
#include "utility_ofc/ethprotocol.h"
#endif
#include "velocities.h"
#include <cstdint>
#include <map>
//#include <string>
//typedefs
namespace EMROSCmdType
{
const Ethprotocol::Id BROADCAST_ID = 0x80000000;
namespace Service
{
const Ethprotocol::Id DEVICE_ID = 0x00000001;
const Ethprotocol::Cmd SYNC_TIME = 0x0110;
const Ethprotocol::Cmd ENABLE_SYNC = 0x0111;
const Ethprotocol::Cmd DISABLE_SYNC = 0x0112;
const Ethprotocol::Cmd ENABLE_NAVIGATION = 0x0115;
const Ethprotocol::Cmd DISABLE_NAVIGATION = 0x0116;
const Ethprotocol::Cmd ENABLE_REMOTE_CONTROL = 0x0117;
const Ethprotocol::Cmd DISABLE_REMOTE_CONTROL = 0x0118;
}
namespace ServiceBridge
{
const Ethprotocol::Id DEVICE_ID = 0x00000002;
const Ethprotocol::Cmd SDO_RX = 0x0101;
const Ethprotocol::Cmd SDO_TX = 0x0102;
}
namespace Basecontroller
{
enum class ModeOfOperation
{
FOUR_WD, TWO_WD, UNDEFINED
};
const Ethprotocol::Id DEVICE_ID = 0x00000003;
const Ethprotocol::Cmd INIT = 0x0101;
const Ethprotocol::Cmd TARGET_VELOCITIES = 0x0110;
const Ethprotocol::Cmd ACTUAL_VELOCITIES = 0x0120;
const Ethprotocol::Cmd ENABLE_CSV_MODE = 0x0102;
const Ethprotocol::Cmd DISABLE_CSV_MODE = 0x0103;
const Ethprotocol::Cmd GET_VELOCITIES = 0x0201;
const Ethprotocol::Cmd GET_MOTOR_STATUS = 0x0202;
using TargetVel = int32_t;
using ActualVel = int32_t;
#ifdef MTKERNEL
using TargetWheelVelocities4WD = WheelVelocities_4WD_ext<TargetVel>;
using ActualWheelVelocities4WD = WheelVelocities_4WD_ext<ActualVel>;
using TargetWheelVelocities2WD = WheelVelocities_2WD_ext<TargetVel>;
using ActualWheelVelocities2WD = WheelVelocities_2WD_ext<ActualVel>;
using TargetVelocitiesInterface = ExtendedVelocities<TargetVel>;
using ActualVelocitiesInterface = ExtendedVelocities<ActualVel>;
#else
using TargetWheelVelocities4WD = WheelVelocities_4WD<TargetVel>;
using ActualWheelVelocities4WD = WheelVelocities_4WD<ActualVel>;
using TargetWheelVelocities2WD = WheelVelocities_2WD<TargetVel>;
using ActualWheelVelocities2WD = WheelVelocities_2WD<ActualVel>;
using TargetVelocitiesInterface = Velocities<TargetVel>;
using ActualVelocitiesInterface = Velocities<ActualVel>;
#endif
}
namespace Security
{
const Ethprotocol::Id DEVICE_ID = 0x99999999;
const Ethprotocol::Cmd CMD_VEL_DOWNLINK_TIMEOUT = 0x0110;
const Ethprotocol::Cmd HEARTBEAT = 0x0001;
const Ethprotocol::Cmd DOWNLINK_HEARTBEAT = 0x0002;
const Ethprotocol::Cmd DOWNLINK_LOST = 0x0003;
}
namespace PowerManagement
{
const Ethprotocol::Id DEVICE_ID = 0x99999998;
const Ethprotocol::Cmd SYSTEM_SHUTDOWN_REQUEST = 0x0010;
const Ethprotocol::Cmd SYSTEM_READY_FOR_POWEROFF = 0x0011;
const Ethprotocol::Cmd PC_BOOTUP = 0x0012;
const Ethprotocol::Cmd EM5_BOOTUP = 0x0013;
const Ethprotocol::Cmd LOW_POWER = 0x0001;
const Ethprotocol::Cmd CHARGING = 0x0002;
const Ethprotocol::Cmd MOBILE_OPERATION = 0x0003;
const Ethprotocol::Cmd STATIONARY_OPERATION = 0x0004;
const Ethprotocol::Cmd ACTUAL_SUPPLY_CURRENT = 0x0021;
const Ethprotocol::Cmd ACTUAL_BATTERY_CURRENT = 0x0022;
const Ethprotocol::Cmd ACTUAL_SUPPLY_VOLTAGE = 0x0023;
const Ethprotocol::Cmd ACTUAL_BATTERY_VOLTAGE = 0x0024;
const Ethprotocol::Cmd SYSTEM_READY = 0x0014;
const Ethprotocol::Cmd PC_POWEROFF = 0x0015;
}
namespace Logger
{
//TODO: this part should go to logger class
const uint8_t MAX_LENGTH = Ethprotocol::MAX_PAYLOAD_LENGTH;
struct Msg
{
char data[MAX_LENGTH];
uint8_t size;
};
const Ethprotocol::Id DEVICE_ID = 0x00000004;
const Ethprotocol::Cmd PRINT_MSG = 0x0101;
}
namespace SdoBridge
{
const Ethprotocol::Id DEVICE_ID = 0x00000005;
const Ethprotocol::Cmd READ_SDO = 1;
const Ethprotocol::Cmd WRITE_SDO = 2;
const Ethprotocol::Cmd BUSSY = 3;
const Ethprotocol::Cmd TIMEOUT = 4;
const Ethprotocol::Cmd ERROR = 5;
}
}
/*
* volocities.h
*
* Created on: Feb 1, 2017
* Author: speckle_s
*/
#ifndef VELOCITIES_H_
#define VELOCITIES_H_
#include <cstring>
#include <cstdint>
#include <array>
#ifdef MTKERNEL
#include "../../../utility_ofc/include/utility_ofc/error.h"
#else
#include "error.h"
#endif
//////////////////////INTERFACE/////////////////////////////////////////////
template<typename VEL_T>
class Velocities
{
public:
Velocities()
{
}
virtual ~Velocities()
{
}
virtual void clearData()=0;
virtual Error set(const uint8_t* raw_data, size_t size)=0;
virtual Error set(const VEL_T* data_by_type, size_t size)=0;
virtual Error get(uint8_t* raw_data, size_t size)=0;
virtual Error get(VEL_T* data_by_type, size_t size)=0;
virtual size_t getVelTypeSize()=0;
virtual size_t getRawSize()=0;
virtual size_t getNumberOfMotors()=0;
virtual VEL_T* dataByType()=0;
virtual bool changed()=0;
};
///////////////////ABSOLUTE CLASSES///////////////////////////////////////////////
template<typename VEL_T, typename STRUCT_T>
class WheelVelocities : virtual public Velocities<VEL_T>
{
public:
enum
{
VEL_TYPE_SIZE = sizeof(VEL_T), RAW_SIZE = sizeof(STRUCT_T), NUMBER_OF_MOTORS = sizeof(STRUCT_T) / sizeof(VEL_T)
};
~WheelVelocities()
{
}
WheelVelocities& operator=(WheelVelocities& s)
{
if (&s == this)
{
return (*this);
}
s.get(mData.raw, RAW_SIZE);
return (*this);
}
void clearData(void) override
{
std::memset(this->mData.raw.begin(), 0, RAW_SIZE);
}
Error set(const uint8_t* raw_data, size_t size) override
{
if (size == this->RAW_SIZE)
{
if (std::memcmp(mData.raw.begin(), raw_data, size) != 0)
{
std::memcpy(mData.raw.begin(), raw_data, size);
mUpdate = true;
}
return (Error::NO_ERROR);
}
else
{
//TargetWheelVelocities_update_dataSizeMismatch
return (Error::ERROR);
}
}
Error set(const VEL_T* data_by_type, size_t size) override
{
if (size == this->RAW_SIZE)
{
if (std::memcmp(mData.by_index.begin(), data_by_type, size) != 0)
{
std::memcpy(mData.by_index.begin(), data_by_type, size);
mUpdate = true;
}
return (Error::NO_ERROR);
}
else
{
//TargetWheelVelocities_update_dataSizeMismatch
return (Error::ERROR);
}
}
Error get(uint8_t* raw_data, size_t size) override
{
if (this->RAW_SIZE <= size)
{
std::memcpy(raw_data, mData.raw.begin(), RAW_SIZE);
return (Error::NO_ERROR);
}
else
{ //TargetWheelVelocities_copy_dataSizeMismatch
return (Error::ERROR);
}
}
Error get(VEL_T* data_by_type, size_t size) override
{
if (this->RAW_SIZE <= size)
{
std::memcpy(data_by_type, mData.by_index.begin(), RAW_SIZE);
return (Error::NO_ERROR);
}
else
{ //TargetWheelVelocities_copy_dataSizeMismatch
return (Error::ERROR);
}
}
STRUCT_T& data()
{
return (mData.by_name);
}
VEL_T* dataByType() override
{
return (mData.by_index.begin());
}
size_t getVelTypeSize() override
{
return (VEL_TYPE_SIZE);
}
size_t getRawSize() override
{
return (RAW_SIZE);
}
size_t getNumberOfMotors() override
{
return (NUMBER_OF_MOTORS);
}
bool changed() override
{
bool tmp = mUpdate;
mUpdate = false;
return (tmp);
}
private:
union
{
STRUCT_T by_name;
std::array<VEL_T, NUMBER_OF_MOTORS> by_index;
std::array<VEL_T, RAW_SIZE> raw;
} mData;
bool mUpdate;
protected:
WheelVelocities() :
mUpdate(), mData()
{
}
};
template<typename VEL_T>
struct struct_2WD
{
VEL_T left;
VEL_T right;
}__attribute__((packed));
template<typename VEL_T>
struct struct_4WD
{
VEL_T front_left_wheel;
VEL_T front_right_wheel;
VEL_T rear_left_wheel;
VEL_T rear_right_wheel;
}__attribute__((packed));
template<typename VEL_T>
class WheelVelocities_2WD : public WheelVelocities<VEL_T, struct_2WD<VEL_T> >
{
};
template<typename VEL_T>
class WheelVelocities_4WD : public WheelVelocities<VEL_T, struct_4WD<VEL_T> >
{
};
#endif /* VELOCITIES_H_ */
/*
* wheel_drive_config.h
*
* Created on: May 14, 2018
* Author: sponfeldner_c
*/
//#pragma once
#ifndef EMROS_emros_common_INC_WHEEL_DRIVE_CONFIG_H_
#define EMROS_emros_common_INC_WHEEL_DRIVE_CONFIG_H_
//////////////////////KINEMATICS/////////////////////////////////////////////
#define DEF_2WD
#endif /* EMROS_emros_common_INC_WHEEL_DRIVE_CONFIG_H_ */
<?xml version="1.0"?>
<package format="2">
<name>emros_common</name>
<version>0.0.0</version>
<description>The emros_common package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="speckle_s@todo.todo">speckle_s</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/emros_common</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>utility_ofc</build_depend>
<build_export_depend>roscpp</build_export_depend>
<exec_depend>roscpp</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
Subproject commit 5a28276a1b95ea300844aec52e0b536f2444dc72
# ---> Eclipse
*.pydevproject
.metadata
.gradle
bin/
tmp/
*.tmp
*.bak
*.swp
*~.nib
local.properties
.settings/
.loadpath
# Eclipse Core
.project
# External tool builders
.externalToolBuilders/
# Locally stored "Eclipse launch configurations"
*.launch
# CDT-specific
.cproject
# JDT-specific (Eclipse Java Development Tools)
.classpath
# Java annotation processor (APT)
.factorypath
# PDT-specific
.buildpath
# sbteclipse plugin
.target
# TeXlipse plugin
.texlipse
# ---> C++
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
# ---> C
# Object files
*.o
*.ko
*.obj
*.elf
# Precompiled Headers
*.gch
*.pch
# Libraries
*.lib
*.a
*.la
*.lo
# Shared objects (inc. Windows DLLs)
*.dll
*.so
*.so.*
*.dylib
# Executables
*.exe
*.out
*.app
*.i*86
*.x86_64
*.hex
# Debug files
*.dSYM/
# ---> CMake
CMakeCache.txt
CMakeFiles
CMakeScripts
Makefile
cmake_install.cmake
install_manifest.txt
cmake_minimum_required(VERSION 2.8.3)
project(utility_ofc)
## Compile as C++11
add_compile_options(-std=c++11)
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS roscpp
)
###########
## Build ##
###########
## Specify additional locations of header files
include_directories(
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(${PROJECT_NAME} #STATIC
src/crc16.cpp
src/ethprotocol.cpp
)
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
)
#############
## Install ##
#############
## Mark executables and/or libraries for installation
install(TARGETS ${PROJECT_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
MIT License
Copyright (c) <year> <copyright holders>
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
This diff is collapsed.
/*
* CRC16 Checksum Implementation and Demo
*
* Copyright (c) 2014 Roman Bendt
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
* of the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
* INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/*
* implements a CRC16 checksum using
* base polynom: 0xA001
* initial value: 0xFFFF
*/
#ifndef CRC16_H
#define CRC16_H
#ifdef MTKERNEL
#include "../../../../nanotec/inc/nanoj.h"
#endif
#include <cstdint>
#include <array>
#include <algorithm>
class CRC16
{
public:
CRC16() = default;
~CRC16() = default;
/*
* calculates a crc16 from the given, arbitrary-lenght data.
* returns crc16 checksum to transmit with the data.
*/
//uint16_t calculate_checksum_crc16(const uint8_t* data, uint8_t size);
template<std::size_t N>
static uint16_t calculate_checksum_crc16(const std::array<uint8_t, N>& data, size_t size)
{
uint16_t _crc = 0xFFFF;
if (size <= data.size())
{
std::for_each(data.begin(), data.begin() + size, [&](uint8_t item)
{
_crc = crc16_update(_crc, item);
});
}
return (_crc);
}
/*
* verifys a given crc16 checksum against the given data.
* returns true if data is error free, false if data is corrupted.
*/
//bool verify_checksum_crc16( uint8_t* data, uint8_t size, uint16_t checksum);
template<std::size_t N>
static bool verify_checksum_crc16(const std::array<uint8_t, N>& data, size_t size, uint16_t checksum)
{
uint16_t _crc = 0xFFFF;
if (size <= data.size())
{
std::for_each(data.begin(), data.begin() + size, [&](uint8_t item)
{
_crc = crc16_update(_crc, item);
});
}
_crc = crc16_update(_crc, (uint8_t)(checksum & 0xFF));
_crc = crc16_update(_crc, (uint8_t)((checksum >> 8) & 0xFF));
return (_crc == 0);
}
private:
static uint16_t crc16_update(uint16_t crc, uint8_t data);
};
#endif
/*
* datacontainer.h
*
* Created on: Feb 6, 2017
* Author: speckle_s
*/
#ifndef EMROS_COMMON_INC_DATACONTAINER_H_
#define EMROS_COMMON_INC_DATACONTAINER_H_
#include <array>
#include <cstring>
#include "error.h"
template<typename STRUCT_T>
class Container
{
public:
enum
{
MAX_DATA_LENGTH = sizeof(STRUCT_T),
};
Container() :
mData()
{
}
~Container()
{
}
void clearData(void)
{
//std::memset(this->mData.raw, 0, MAX_DATA_LENGTH);
mData.raw.fill(0);
}
const STRUCT_T& getData() const
{
return (mData.data);
}
STRUCT_T& setData()
{
return (mData.data);
}
// Container& operator=(Container& s)
// {
// if (&s == this)
// {
// return (*this);
// }
// if (this->MAX_DATA_LENGTH == s.MAX_DATA_LENGTH)
// {
// //std::memcpy(mData.raw,s.getRaw(),MAX_DATA_LENGTH);
// mData.raw = s.getRaw();
// //std::copy(s.getRaw().begin(),s.getRaw().end(),mData.raw.begin());
//
// }
// return (*this);
// }
Error get(uint8_t *data, size_t len)
{
if (len >= MAX_DATA_LENGTH)
{
std::memcpy(data, mData.raw.data(), MAX_DATA_LENGTH);
}
else
{
//OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__ , "dataSizeMismatch");
return (Error::ERROR);
}
return (Error::NO_ERROR);
}
template<std::size_t N>
Error get(std::array<uint8_t, N> &data)
{
if (data.size() <= mData.raw.size())
{
std::copy(mData.raw.begin(), mData.raw.end(), data.begin());
}
else
{
//OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__ ," dataSizeMismatch");
return (Error::ERROR);
}
}
Error set(uint8_t *data, size_t len)
{
if (len <= MAX_DATA_LENGTH)
{
std::memcpy(mData.raw.data(), data, len);
}
else
{
//OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__ ," dataSizeMismatch");
return (Error::ERROR);
}
return (Error::NO_ERROR);
}
template<std::size_t N>
Error set(const std::array<uint8_t, N> &data)
{
if (data.size() <= mData.raw.size())
{
std::copy(data.begin(), data.end(), mData.raw.begin());
}
else
{
//OFCLoggerRTClient::instance()->InsertError(__PRETTY_FUNCTION__ ," dataSizeMismatch");
return (Error::ERROR);
}
return (Error::NO_ERROR);
}
const std::array<uint8_t, MAX_DATA_LENGTH>& getRaw () const
{
return (mData.raw);
}
std::array<uint8_t, MAX_DATA_LENGTH>& setRaw()
{
return (mData.raw);
}
// const uint8_t* getRaw()
// {
// return mData.raw.data();
// }
private:
union
{
STRUCT_T data;
std::array<uint8_t, MAX_DATA_LENGTH> raw;
} mData;
};
template <typename Type>
union union_t
{
Type data;
std::array<uint8_t, sizeof(Type)> raw;
};
#endif /* EMROS_COMMON_INC_DATACONTAINER_H_ */
/*
* error.h
*
* Created on: Feb 28, 2018
* Author: speckle_s
*/
#ifndef CAN_GATEWAY_CORE_INC_ERROR_H_
#define CAN_GATEWAY_CORE_INC_ERROR_H_
enum class Error{
ERROR,NO_ERROR
};
#endif /* CAN_GATEWAY_CORE_INC_ERROR_H_ */
/*
* ethprotocol.hpp
*
* Created on: Feb 12, 2016
* Author: Sydney Speckle
* Mail: sydney.speckle@nanotec.de
*/
#ifndef ETHPROTOCOL_HPP_
#define ETHPROTOCOL_HPP_
#include <cstring>
#include "crc16.h"
#include "datacontainer.h"
#include <cstdint>
#include <array>
#include <algorithm>
#ifdef MTKERNEL
#include "../../../../nanotec/inc/nanoj.h"
#include "../../../NanoOFC/logger/inc/ofc_client_RT.h"
#else
#include "ros/ros.h"
#include "error.h"
#endif
class Ethprotocol
{
public:
using Stamp = uint32_t;
using Cmd = uint32_t;
using Id = uint32_t;
using Seq = uint8_t;
using PayLen = uint8_t;
using Crc = uint16_t;
using Len = uint8_t;
enum
{
MAX_CMD_LENGTH = (sizeof(Len) + sizeof(Stamp) + sizeof(Cmd) + sizeof(Id) + sizeof(Seq) + sizeof(PayLen)
+ sizeof(Crc)), MAX_PAYLOAD_LENGTH = 100, MAX_FRAME_LENGTH = (MAX_PAYLOAD_LENGTH + MAX_CMD_LENGTH),
MAX_NUMBER_OF_FRAMES = 5, MAX_BUFFER_LENGTH = (MAX_FRAME_LENGTH * MAX_NUMBER_OF_FRAMES), OVERHEAD_LEN = sizeof(Len)
+ sizeof(Stamp) + sizeof(Cmd) + sizeof(Id) + sizeof(Seq) + sizeof(PayLen), CRC_LEN = sizeof(Crc)
};
private:
struct Dataframe
{
Len size; //don't change this. It would cause massive faults without changing parts of the source code !!!!!!
Seq sequence;
Stamp time;
Id id;
Cmd cmd;
PayLen payload_length;
std::array<uint8_t, MAX_PAYLOAD_LENGTH> payload;
}__attribute__((packed));
static Seq GetRxSequence()
{
return (mMSG_rx_cnt++);
}
static Seq GetTxSequence()
{
return (mMSG_tx_cnt++);
}
static Seq mMSG_rx_cnt;
static Seq mMSG_tx_cnt;
static Seq mbuffer_cnt;
public:
//TODO: nachfolgende klassen evtl. structs da dumme Datenobjekte
struct StreamBuffer
{
std::array<uint8_t, MAX_BUFFER_LENGTH> data;
uint8_t length;
};
struct Stream
{
std::array<uint8_t, MAX_FRAME_LENGTH> data;
size_t length;
};
using EthDataframe = Container<Dataframe>;
class DataframeRxBuffer
{
public:
DataframeRxBuffer() :
pos(0)
{
}
void clearData()
{
for (int i = 0; i < MAX_NUMBER_OF_FRAMES; i++)
{
dataframe[i].clearData();
}
}
std::array<EthDataframe, MAX_NUMBER_OF_FRAMES> dataframe;
size_t pos;
};
Ethprotocol()
{
}
~Ethprotocol()
{
}
static void ProcessTxData(EthDataframe& data_in, Stream& stream_out);
static Error ProcessRxData(const Stream& stream_in, EthDataframe& data_out);
static Error ProccessRxStreamBuffer(const StreamBuffer& buffer, DataframeRxBuffer& parsed_msgs);
};
#endif /* ETHPROTOCOL_HPP_ */
/*
* leaky_bucket_timeout.hpp
*
* Created on: Jun 1, 2016
* Author: Sydney Speckle
* Mail: sydney.speckle@nanotec.de
*
* this template provides a timeout based leaky-bucket function.
* It supports multi-threading and none multi-threading systems. It is also designed platform independent.
* You can use it with the EM5 as well as with any linux-based system.
* For multi-threading systems is a interface for boost mutex provided.
* You can define the weighting of a success and a fail (timeout) event.
* It is also possible to define the threshold for the return of an error.
*/
#ifndef ROS_PACKAGES_SEPP_SEPP_COMMON_CODE_INCLUDE_LEAKY_BUCKET_TIMEOUT_HPP_
#define ROS_PACKAGES_SEPP_SEPP_COMMON_CODE_INCLUDE_LEAKY_BUCKET_TIMEOUT_HPP_
#include "timeout.h"
#ifdef MTKERNEL
#include "../../../../nanotec/inc/nanoj.h"
#endif
#ifndef MTKERNEL
#if __cplusplus <= 201103L
#include <stdint.h>
#else
#include <cstdint>
#endif
#endif
class mutexDummy
{
public:
typedef bool mutex_type;
mutexDummy(mutex_type )
{
}
};
template<typename TIME, typename DURATION, typename MUTEX, typename GET_TIME>
class LeakyBucketTimeout
{
public:
LeakyBucketTimeout(DURATION duration, int32_t error_threshold, int32_t error_emphasis, int32_t success_emphasis,
GET_TIME get_time);
void setEvent();
bool checkError();
void update();
void reset();
private:
Timeout<TIME, DURATION, GET_TIME> timeout;
typename MUTEX::mutex_type mMutex;
int32_t mError_threshold;
int32_t mError_emphasis;
int32_t mSuccess_emphasis;
int32_t mCounter;
};
template<typename TIME, typename DURATION, typename MUTEX, typename GET_TIME>
LeakyBucketTimeout<TIME, DURATION, MUTEX, GET_TIME>::LeakyBucketTimeout(DURATION duration, int32_t error_threshold,
int32_t error_emphasis,
int32_t success_emphasis, GET_TIME get_time) :
timeout(duration, get_time), mError_threshold(error_threshold), mError_emphasis(error_emphasis), mSuccess_emphasis(
success_emphasis), mCounter(0)
{
timeout.resetTime();
}
template<typename TIME, typename DURATION, typename MUTEX, typename GET_TIME>
void LeakyBucketTimeout<TIME, DURATION, MUTEX, GET_TIME>::setEvent()
{
MUTEX scoped_lock(mMutex);
mCounter -= mSuccess_emphasis;
if (mCounter < 0)
{
mCounter = 0;
}
timeout.resetTime();
}
template<typename TIME, typename DURATION, typename MUTEX, typename GET_TIME>
bool LeakyBucketTimeout<TIME, DURATION, MUTEX, GET_TIME>::checkError()
{
MUTEX scoped_lock(mMutex);
if (mCounter > mError_threshold)
{
return true;
}
else
{
return false;
}
}
template<typename TIME, typename DURATION, typename MUTEX, typename GET_TIME>
void LeakyBucketTimeout<TIME, DURATION, MUTEX, GET_TIME>::update()
{
MUTEX scoped_lock(mMutex);
if (timeout.timeoutReached())
{
if (mCounter <= mError_threshold)
{
mCounter += mError_emphasis;
}
timeout.resetTime();
}
}
template<typename TIME, typename DURATION, typename MUTEX, typename GET_TIME>
void LeakyBucketTimeout<TIME, DURATION, MUTEX, GET_TIME>::reset()
{
mCounter = 0;
}
#endif /* ROS_PACKAGES_SEPP_SEPP_COMMON_CODE_INCLUDE_LEAKY_BUCKET_TIMEOUT_HPP_ */
//http://www.lonecpluspluscoder.com/2015/08/13/an-elegant-way-to-extract-keys-from-a-c-map/
#pragma once
template<typename TK, typename TV>
std::vector<TK> extract_keys(std::map<TK, TV> const& input_map) {
std::vector<TK> retval;
for (auto const& element : input_map) {
retval.push_back(element.first);
}
return (retval);
}
template<typename TK, typename TV>
std::vector<TV> extract_values(std::map<TK, TV> const& input_map) {
std::vector<TV> retval;
for (auto const& element : input_map) {
retval.push_back(element.second);
}
return (retval);
}
/*
* singleton.h
*
* Created on: Feb 24, 2017
* Author: speckle_s
*
* https://de.wikibooks.org/wiki/C%2B%2B-Programmierung:_Entwurfsmuster:_Singleton
*/
#ifndef EMROS_EMROS_INC_SINGLETON_H_
#define EMROS_EMROS_INC_SINGLETON_H_
template<typename C>
class Singleton
{
public:
static C* instance()
{
if (!_instance)
_instance = new C();
return (_instance);
}
virtual ~Singleton()
{
delete _instance;
_instance = nullptr;
}
private:
static C* _instance;
protected:
Singleton()
{
}
};
template<typename C>
C* Singleton<C>::_instance = nullptr;
#endif /* EMROS_EMROS_INC_SINGLETON_H_ */
/*
* timeout.hpp
*
* Created on: Jun 1, 2016
* Author: Sydney Speckle
* Mail: sydney.speckle@nanotec.de
* this software provides a timeaut template.
* It is also designed platform independent.
* You can use it with the EM5 as well as with any linux-based system. It supports different data-types for time-stamps.
* The type you plan to us has to support standard math functions. For more information refer the source code.
* For example you can use a uint32_t type which is used by the EM5 and you can also use the ROS Time und Duration classes.
*/
#ifndef ROS_PACKAGES_SEPP_SEPP_COMMON_CODE_INCLUDE_TIMEOUT_HPP_
#define ROS_PACKAGES_SEPP_SEPP_COMMON_CODE_INCLUDE_TIMEOUT_HPP_
template<typename TIME, typename DURATION,typename GET_TIME> class Timeout
{
public:
Timeout(DURATION duration,GET_TIME get_time_F);
~Timeout(){}
bool timeoutReached();
void resetTime();
private:
DURATION mTarget_duration;
TIME mStart_time;
GET_TIME mGet_time;
};
template<typename TIME, typename DURATION,typename GET_TIME> Timeout<TIME,DURATION,GET_TIME>::Timeout(DURATION duration,GET_TIME get_time_F) :
mTarget_duration(duration),mGet_time(get_time_F)
{
}
template<typename TIME, typename DURATION,typename GET_TIME> void Timeout<TIME,DURATION,GET_TIME>::resetTime()
{
mStart_time = mGet_time();
}
template<typename TIME, typename DURATION,typename GET_TIME> bool Timeout<TIME,DURATION,GET_TIME>::timeoutReached()
{
if (mGet_time() - mStart_time >= mTarget_duration)
{
return true;
}
else
{
return false;
}
}
#endif /* ROS_PACKAGES_SEPP_SEPP_COMMON_CODE_INCLUDE_TIMEOUT_HPP_ */
/*
* tuple_templates.hpp
*
* Created on: Feb 22, 2018
* Author: speckle_s
*/
#ifndef TUPLE_TEMPLATES_HPP_
#define TUPLE_TEMPLATES_HPP_
#include <tuple>
template <std::size_t...Is>
struct index_sequence {};
template <std::size_t N, std::size_t...Is>
struct make_sequence : make_sequence<N - 1, N - 1, Is...> {};
template <std::size_t...Is>
struct make_sequence<0, Is...> : index_sequence<Is...> {};
template<typename F, typename T, std::size_t... Is>
void for_each_in_tuple(T&& tuple, F func, index_sequence<Is...>) {
using expander = int[];
static_cast<void>(expander {0, ((void)func(std::get<Is>(tuple)), 0)...});
}
template<typename F, typename T, std::size_t... Is, std::size_t BUFFER_SIZE>
void for_each_in_tuple(T&& tuple, F func ,std::array<char,BUFFER_SIZE>& data_out, index_sequence<Is...>) {
using expander = int[];
static_cast<void>(expander {0, ((void)func(std::get<Is>(tuple),data_out), 0)...});
}
template<typename F, typename... Ts>
void for_each_in_tuple(std::tuple<Ts...> const& tuple, F func) {
for_each_in_tuple(tuple, func, make_sequence<sizeof...(Ts)>());
}
template<typename F, typename... Ts, std::size_t BUFFER_SIZE>
void for_each_in_tuple(std::tuple<Ts...> const& tuple, F func, std::array<char, BUFFER_SIZE>& data_out) {
for_each_in_tuple(tuple, func, data_out, make_sequence<sizeof...(Ts)>());
}
#endif /* TUPLE_TEMPLATES_HPP_ */
<?xml version="1.0"?>
<package format="2">
<name>utility_ofc</name>
<version>0.0.0</version>
<description>The utility_ofc package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="speckle_s@todo.todo">speckle_s</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/utility_ofc</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_export_depend>roscpp</build_export_depend>
<exec_depend>roscpp</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
/*
* CRC16 Checksum Implementation and Demo
*
* Copyright (c) 2014 Roman Bendt
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
* of the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
* INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
* PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
* *********************************************************************************
* 2016 Modified for system use; pre c++11 Standard and no vectors.
*/
#include "../include/utility_ofc/crc16.h"
/*
* helper function for calculating crc checksum. digests the next byte to include in the crc.
*/
uint16_t CRC16::crc16_update(uint16_t crc, uint8_t data)
{
uint8_t i = 0;
crc = (uint16_t)(crc ^ data);
for (i = 0; i < 8; ++i)
{
if (crc & 1)
crc = (uint16_t)((crc >> 1) ^ 0xA001);
else
crc = (uint16_t)(crc >> 1);
}
return (crc);
}
//uint16_t CRC16::calculate_checksum_crc16(const uint8_t* data, uint8_t size)
//{
// uint16_t _crc = 0xFFFF;
// uint8_t i;
//
// for (i = 0; i < size; i++)
// {
// _crc = crc16_update(_crc, data[i]);
// }
// return _crc;
//}
//bool CRC16::verify_checksum_crc16(uint8_t* data, uint8_t size, uint16_t checksum)
//{
// uint16_t _crc = 0xFFFF;
// for (uint8_t i = 0; i < size; i++)
// {
// _crc = crc16_update(_crc, data[i]);
// }
// _crc = crc16_update(_crc, (uint8_t)(checksum & 0xFF));
// _crc = crc16_update(_crc, (uint8_t)((checksum >> 8) & 0xFF));