Skip to content
Snippets Groups Projects
Commit 6546a3e9 authored by andreeatulbure's avatar andreeatulbure
Browse files

added statechart

parent 396cfcdd
No related branches found
No related tags found
No related merge requests found
Showing
with 536 additions and 91 deletions
<?xml version="1.0" encoding="utf-8"?>
<scenario name="SensorPackage" lastChange="2017-02-27.11:57:43" creation="2017-02-27.11:57:30" globalConfigName="./config/global.cfg" package="RobotAPI">
<application name="SensorPackageUnitApp" instance="" package="RobotAPI"/>
</scenario>
# ==================================================================
# SensorPackageUnitApp properties
# ==================================================================
# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
# Attributes:
# - Default: Default value not mapped.
# - Case sensitivity: no
# - Required: no
# ArmarX.AdditionalPackages = Default value not mapped.
# ArmarX.ApplicationName: Application name
# Attributes:
# - Default: ""
# - Case sensitivity: no
# - Required: no
# ArmarX.ApplicationName = ""
# ArmarX.CachePath: Path for cache files
# Attributes:
# - Default: ${HOME}/.armarx/mongo/.cache
# - Case sensitivity: no
# - Required: no
# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
# ArmarX.Config: Comma-separated list of configuration files
# Attributes:
# - Default: ""
# - Case sensitivity: no
# - Required: no
# ArmarX.Config = ""
# ArmarX.DataPath: Semicolon-separated search list for data files
# Attributes:
# - Default: ""
# - Case sensitivity: no
# - Required: no
# ArmarX.DataPath = ""
# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
# Attributes:
# - Default: Default value not mapped.
# - Case sensitivity: no
# - Required: no
# ArmarX.DefaultPackages = Default value not mapped.
# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
# Attributes:
# - Default: ./config/dependencies.cfg
# - Case sensitivity: no
# - Required: no
# ArmarX.DependenciesConfig = ./config/dependencies.cfg
# ArmarX.DisableLogging: Turn logging off in whole application
# Attributes:
# - Default: 0
# - Case sensitivity: no
# - Required: no
# ArmarX.DisableLogging = 0
# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
# Attributes:
# - Default: 0
# - Case sensitivity: no
# - Required: no
# ArmarX.EnableProfiling = 0
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
# - Default: 1
# - Case sensitivity: no
# - Required: no
# ArmarX.RedirectStdout = 1
# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
# Attributes:
# - Default: 3000
# - Case sensitivity: no
# - Required: no
# ArmarX.RemoteHandlesDeletionTimeout = 3000
# ArmarX.SensorPackageUnit.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: 0
# - Case sensitivity: no
# - Required: no
# ArmarX.SensorPackageUnit.EnableProfiling = 0
# ArmarX.SensorPackageUnit.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
# - Default: Undefined
# - Case sensitivity: no
# - Required: no
# ArmarX.SensorPackageUnit.MinimumLoggingLevel = Undefined
# ArmarX.SensorPackageUnit.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: no
# - Required: no
# ArmarX.SensorPackageUnit.ObjectName = ""
# ArmarX.SensorPackageUnit.SerialInterfaceDevice: The serial device the arduino is connected to.
# Attributes:
# - Default: /dev/ttyACM0
# - Case sensitivity: no
# - Required: no
# ArmarX.SensorPackageUnit.SerialInterfaceDevice = /dev/ttyACM0
# ArmarX.SensorPackageUnit.TopicName: Name of the topic on which the sensor values are provided
# Attributes:
# - Default: SensorPackageUnit
# - Case sensitivity: no
# - Required: no
# ArmarX.SensorPackageUnit.TopicName = SensorPackageUnit
# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
# Attributes:
# - Default: 0
# - Case sensitivity: no
# - Required: no
# ArmarX.StartDebuggerOnCrash = 0
# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
# Attributes:
# - Default: ""
# - Case sensitivity: no
# - Required: no
# ArmarX.TopicSuffix = ""
# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
# - Default: 0
# - Case sensitivity: no
# - Required: no
# ArmarX.UseTimeServer = 0
# ArmarX.Verbosity: Global logging level for whole application
# Attributes:
# - Default: Info
# - Case sensitivity: no
# - Required: no
# ArmarX.Verbosity = Info
# Ice.Config: Custom Property
# Attributes:
# - Default: ::NOT_DEFINED::
# - Case sensitivity: no
# - Required: no
# Ice.Config = ::NOT_DEFINED::
# ==================================================================
# Global Config from Scenario SensorPackage
# ==================================================================
......@@ -7,35 +7,13 @@
using namespace armarx;
SensorPackageUnit::SensorPackageUnit()
: recording(false)
{
}
void SensorPackageUnit::startRecording(const std::string& recordingName, const Ice::Current& c)
{
ARMARX_INFO << "startRecording";
std::string sensorDataRecordingFileName = recordingName + "_data.txt";
outputFile.open(sensorDataRecordingFileName);
recording = true;
}
void SensorPackageUnit::stopRecording(const Ice::Current& c)
{
recording = false;
ARMARX_INFO << "stopRecording";
ScopedLock lock(fileMutex);
outputFile.close();
}
bool SensorPackageUnit::isComponentOnline(const Ice::Current& c)
{
return true;
}
void SensorPackageUnit::onInitComponent()
{
offeringTopic("SensorPackageUnit");
recording = false;
//open serial port
std::string portname = getProperty<std::string>("SerialInterfaceDevice").getValue();
......@@ -103,18 +81,20 @@ PropertyDefinitionsPtr SensorPackageUnit::createPropertyDefinitions()
void SensorPackageUnit::run()
{
std::string line;
while (readTask->isRunning())
{
std::string line;
getline(arduino, line, '\n');
if (recording)
//ARMARX_IMPORTANT << line;
getValues(line.c_str());
/*if (recording)
{
ScopedLock lock(fileMutex);
// get sensor data from serial
getValues(line.c_str());
outputFile << IceUtil::Time::now().toDateTime() << " " << "Id: " << sensorData[0] << " Pressure: " << sensorData[1] << " IMU: " << sensorData[2] << " " << sensorData[3] << " " << sensorData[4] << " " << sensorData[5] << "\n";
ARMARX_INFO << "Id: " << sensorData[0] << " Pressure: " << sensorData[1] << " IMU: " << sensorData[2] << " " << sensorData[3] << " " << sensorData[4] << " " << sensorData[5] << "\n";
//outputFile << IceUtil::Time::now().toDateTime() << " " << line << "\n";
......@@ -123,60 +103,24 @@ void SensorPackageUnit::run()
outputFile.flush();
//listenerIMU->reportForceSensorValues(sensorData[0], sensorData[1], sensorData[2], sensorData[3], sensorData[4], sensorData[5]);
}
}*/
}
}
// get imu values from incoming string
void SensorPackageUnit::getValues(std::string line)
{
std::string erasedLine = line.erase(0, 6);
char* erasedL = new char[erasedLine.length() + 1];
strcpy(erasedL, erasedLine.c_str());
// split string at blank space
const char* cutter = " ";
char* cutPieces = strtok(erasedL, cutter);
while (cutPieces != NULL)
{
for (int i = 0; i < sizeArray; i++)
{
sensorData[i] = atof(cutPieces);
cutPieces = strtok(NULL, cutter);
}
}
delete [] erasedL;
}
/*void SensorPackageUnit::angleDiff()
{
double diff;
for (int i = 0; i < 3 ; i++)
{
diff = std::fabs(std::fabs(imuValEuler[i]) - std::fabs(imuValEuler[i + 3]));
imuDiff[i] = std::min(diff, 360 - diff);
}
imuDiff[0] = 210 - imuDiff[0];
}
void ArduinoIMUUnit::quaternionToEuler(float w, float x, float y, float z)
SensorPackageUnit::SensorData SensorPackageUnit::getValues(std::string line)
{
//calculation roll in radians
float ro = atan2((2 * (w * x + y * z)), (1 - (2 * ((x * x) + (y * y)))));
//transformation to degree
roll = (ro * 180) / PI;
//calculation pitch in radians
float pit = asin(2 * (w * y - z * y));
//transformation to degree
pitch = (pit * 180) / PI;
//calculation yaw in radians
float yw = atan2((2 * (w * z + x * y)), (1 - (2 * ((y * y) + (z * z)))));
//transformation to degree
yaw = (yw * 180) / PI;
SensorData data;
std::vector<std::string> splitValues;
boost::split(splitValues, line, boost::is_any_of(" "));
data.id = stoi(splitValues.at(0));
data.pressure = std::stof(splitValues.at(1));
data.qw = std::stof(splitValues.at(2));
data.qx = std::stof(splitValues.at(3));
data.qy = std::stof(splitValues.at(4));
data.qz = std::stof(splitValues.at(5));
return data;
}
*/
......
......@@ -10,8 +10,6 @@
#include <fstream>
#include <stdio.h>
#include <boost/date_time/posix_time/posix_time.hpp>
//#include <MMMCapture/interface/units/MMMCaptureSensor.h>
//#include <ArmarXCore/interface/observers/ObserverInterface.h>
namespace armarx
......@@ -47,16 +45,19 @@ namespace armarx
{
public:
SensorPackageUnit();
virtual void startRecording(const std::string& recordingName, const Ice::Current& c);
virtual void stopRecording(const Ice::Current& c);
bool isComponentOnline(const Ice::Current& c);
virtual std::string getDefaultName() const
{
return "SensorPackageUnit";
}
struct SensorData
{
int id;
float pressure;
float qw, qx, qy, qz;
};
protected:
virtual void onInitComponent();
virtual void onConnectComponent();
......@@ -65,18 +66,14 @@ namespace armarx
private:
std::fstream arduino;
std::ofstream outputFile;
Mutex fileMutex;
//std::ofstream outputFile;
//Mutex fileMutex;
RunningTask<SensorPackageUnit>::pointer_type readTask;
SensorPackageUnitListenerPrx listenerSensorPackage;
void periodicExec();
bool recording;
//void periodicExec();
void run();
void getValues(std::string s);
static const int sizeArray = 6;
float sensorData[sizeArray];
SensorData getValues(std::string s);
int fd;
};
......
......@@ -49,9 +49,7 @@ module armarx
interface SensorPackageUnitListener
{
void reportSensorValues(float id, float pressure, float posw, float posx, float posy, float posz);
//void reportForceSensorValues(float pos1x, float pos1y, float pos1z, float quaternion1,
// float pos2x, float pos2y, float pos2z, float quaternion2);
void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz);
};
......
......@@ -2,4 +2,5 @@
add_subdirectory(operations)
add_subdirectory(WeissHapticGroup)
add_subdirectory(StatechartProfilesTestGroup)
\ No newline at end of file
add_subdirectory(StatechartProfilesTestGroup)
add_subdirectory(SensorPackageGroup)
\ No newline at end of file
armarx_component_set_name("SensorPackageGroup")
#find_package(MyLib QUIET)
#armarx_build_if(MyLib_FOUND "MyLib not available")
#
# all include_directories must be guarded by if(Xyz_FOUND)
# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
#if(MyLib_FOUND)
# include_directories(${MyLib_INCLUDE_DIRS})
#endif()
#find_package(Eigen3 QUIET)
#find_package(Simox QUIET)
#
#armarx_build_if(Eigen3_FOUND "Eigen3 not available")
#armarx_build_if(Simox_FOUND "Simox-VirtualRobot not available")
#
#if (Eigen3_FOUND AND Simox_FOUND)
# include_directories(
# ${Eigen3_INCLUDE_DIR}
# ${Simox_INCLUDE_DIRS}
# )
#endif()
set(COMPONENT_LIBS
# RobotAPIInterfaces RobotAPICore
ArmarXCoreInterfaces ArmarXCore ArmarXCoreStatechart ArmarXCoreObservers)
# Sources
set(SOURCES
SensorPackageGroupRemoteStateOfferer.cpp
./SensorPackageTest.cpp
#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
)
set(HEADERS
SensorPackageGroupRemoteStateOfferer.h
SensorPackageGroup.scgxml
./SensorPackageTest.h
#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
./SensorPackageTest.xml
#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.xml
)
armarx_add_component("${SOURCES}" "${HEADERS}")
<?xml version="1.0" encoding="utf-8"?>
<StatechartGroup name="SensorPackageGroup" package="RobotAPI" generateContext="true">
<Proxies/>
<Configurations/>
<State filename="SensorPackageTest.xml" visibility="public"/>
</StatechartGroup>
/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotAPI::SensorPackageGroup::SensorPackageGroupRemoteStateOfferer
* @author andreeatulbure ( andreea_tulbure at yahoo dot de )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#include "SensorPackageGroupRemoteStateOfferer.h"
using namespace armarx;
using namespace SensorPackageGroup;
// DO NOT EDIT NEXT LINE
SensorPackageGroupRemoteStateOfferer::SubClassRegistry SensorPackageGroupRemoteStateOfferer::Registry(SensorPackageGroupRemoteStateOfferer::GetName(), &SensorPackageGroupRemoteStateOfferer::CreateInstance);
SensorPackageGroupRemoteStateOfferer::SensorPackageGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
XMLRemoteStateOfferer < SensorPackageGroupStatechartContext > (reader)
{
}
void SensorPackageGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
{
}
void SensorPackageGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
{
}
void SensorPackageGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
{
}
// DO NOT EDIT NEXT FUNCTION
std::string SensorPackageGroupRemoteStateOfferer::GetName()
{
return "SensorPackageGroupRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
XMLStateOffererFactoryBasePtr SensorPackageGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new SensorPackageGroupRemoteStateOfferer(reader));
}
/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotAPI::SensorPackageGroup
* @author andreeatulbure ( andreea_tulbure at yahoo dot de )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#ifndef _ARMARX_XMLUSERCODE_RobotAPI_SensorPackageGroup_REMOTESTATEOFFERER_H
#define _ARMARX_XMLUSERCODE_RobotAPI_SensorPackageGroup_REMOTESTATEOFFERER_H
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
#include "SensorPackageGroupStatechartContext.generated.h"
namespace armarx
{
namespace SensorPackageGroup
{
class SensorPackageGroupRemoteStateOfferer :
virtual public XMLRemoteStateOfferer < SensorPackageGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
SensorPackageGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
// inherited from RemoteStateOfferer
void onInitXMLRemoteStateOfferer();
void onConnectXMLRemoteStateOfferer();
void onExitXMLRemoteStateOfferer();
// static functions for AbstractFactory Method
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
};
}
}
#endif
/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotAPI::SensorPackageGroup
* @author andreeatulbure ( andreea_tulbure at yahoo dot de )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#include "SensorPackageTest.h"
using namespace armarx;
using namespace SensorPackageGroup;
// DO NOT EDIT NEXT LINE
SensorPackageTest::SubClassRegistry SensorPackageTest::Registry(SensorPackageTest::GetName(), &SensorPackageTest::CreateInstance);
void SensorPackageTest::onEnter()
{
//SensorPackageGroupStatechartContext* context = getContext<SensorPackageGroupStatechartContext>();
//HapticUnitObserverInterfacePrx hapticObserver = context->getHapticObserver();
//ChannelRegistry channels = hapticObserver->getAvailableChannels(false);
//std::map<std::string, DatafieldRefPtr> tactileDatafields_MaximumValueMap;
//local.setTactileDatafields_MaximumValue(tactileDatafields_MaximumValueMap);
}
//void SensorPackageTest::run()
//{
// // put your user code for the execution-phase here
// // runs in seperate thread, thus can do complex operations
// // should check constantly whether isRunningTaskStopped() returns true
//
//// uncomment this if you need a continous run function. Make sure to use sleep or use blocking wait to reduce cpu load.
// while (!isRunningTaskStopped()) // stop run function if returning true
// {
// // do your calculations
// }
//}
//void SensorPackageTest::onBreak()
//{
// // put your user code for the breaking point here
// // execution time should be short (<100ms)
//}
void SensorPackageTest::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
}
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr SensorPackageTest::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new SensorPackageTest(stateData));
}
/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotAPI::SensorPackageGroup
* @author andreeatulbure ( andreea_tulbure at yahoo dot de )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#ifndef _ARMARX_XMLUSERCODE_RobotAPI_SensorPackageGroup_SensorPackageTest_H
#define _ARMARX_XMLUSERCODE_RobotAPI_SensorPackageGroup_SensorPackageTest_H
#include <RobotAPI/statecharts/SensorPackageGroup/SensorPackageTest.generated.h>
namespace armarx
{
namespace SensorPackageGroup
{
class SensorPackageTest :
public SensorPackageTestGeneratedBase <SensorPackageTest>
{
public:
SensorPackageTest(const XMLStateConstructorParams& stateData):
XMLStateTemplate <SensorPackageTest> (stateData), SensorPackageTestGeneratedBase <SensorPackageTest> (stateData)
{
}
// inherited from StateBase
void onEnter();
// void run();
// void onBreak();
void onExit();
// static functions for AbstractFactory Method
static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
static SubClassRegistry Registry;
// DO NOT INSERT ANY CLASS MEMBERS,
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
}
}
#endif
<?xml version="1.0" encoding="utf-8"?>
<State version="1.2" name="SensorPackageTest" uuid="83D59864-EC09-4DA1-8773-7FF6D0E74A6A" width="800" height="600" type="Normal State">
<InputParameters/>
<OutputParameters/>
<LocalParameters/>
<Substates/>
<Events>
<Event name="Failure">
<Description>Event for statechart-internal failures or optionally user-code failures</Description>
</Event>
</Events>
<Transitions/>
</State>
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment