Skip to content
Snippets Groups Projects
Commit 6e11930b authored by Johann Mantel's avatar Johann Mantel
Browse files

implement parameters as ArmarX properties

parent 92ce1a77
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+ 241
− 235
View file @ 6e11930b
......@@ -28,216 +28,222 @@
// #include <SimoxUtility/color/Color.h>
namespace armarx {
armarx::PropertyDefinitionsPtr SickLaserUnit::createPropertyDefinitions() {
armarx::PropertyDefinitionsPtr def =
new ComponentPropertyDefinitions(getConfigIdentifier());
// Publish to a topic (passing the TopicListenerPrx).
// def->topic(myTopicListener);
// Subscribe to a topic (passing the topic name).
// def->topic<PlatformUnitListener>("MyTopic");
// Use (and depend on) another component (passing the ComponentInterfacePrx).
// def->component(myComponentProxy)
// Add a required property.
def->required(properties.laserScannerTopicName, "laserScannerTopicName",
"Name of the published Topic");
// Add an optionalproperty.
def->optional(properties.updatePeriod, "updatePeriod", "Update every ? ms");
def->optional(properties.angleOffset, "angleOffset",
"Offset to the LaserScanner Angle");
def->optional(properties.devices, "devices", "List of Device Names");
return def;
}
void SickLaserUnit::onInitComponent() {
// Topics and properties defined above are automagically registered.
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
std::string topicName =
getProperty<std::string>("laserScannerTopicName").getValue();
offeringTopic(topicName);
ARMARX_INFO << "Going to report on topic " << topicName;
int updatePeriod = getProperty<int>("updatePeriod").getValue();
float angleOffset = getProperty<float>("angleOffset").getValue();
std::string deviceStrings = getProperty<std::string>("devices").getValue();
std::vector<std::string> splitDeviceStrings = Split(deviceStrings, ";");
ARMARX_INFO_S << "SickLaserUnit sagt Hallo Welt!";
std::string scannerName;
if (false == nhPriv.getParam("scanner_type", scannerName)) {
ROS_ERROR("cannot find parameter "
"scanner_type"
" in the param set. Please specify scanner_type.");
ROS_ERROR("Try to set %s as fallback.\n", nodeName.c_str());
scannerName = nodeName;
}
bool useTCP = false;
std::string hostname;
if (nhPriv.getParam("hostname", hostname)) {
useTCP = true;
}
bool changeIP = false;
std::string sNewIp;
if (nhPriv.getParam("new_IP_address", sNewIp)) {
changeIP = true;
}
std::string port;
nhPriv.param<std::string>("port", port, "2112");
int timelimit;
nhPriv.param("timelimit", timelimit, 5);
bool subscribe_datagram;
int device_number;
nhPriv.param("subscribe_datagram", subscribe_datagram, false);
nhPriv.param("device_number", device_number, 0);
sick_scan::SickGenericParser *parser =
new sick_scan::SickGenericParser(scannerName);
double param;
char colaDialectId = 'A'; // A or B (Ascii or Binary)
if (nhPriv.getParam("range_min", param)) {
parser->set_range_min(param);
}
if (nhPriv.getParam("range_max", param)) {
parser->set_range_max(param);
}
if (nhPriv.getParam("time_increment", param)) {
parser->set_time_increment(param);
}
/*
* Check, if parameter for protocol type is set
*/
bool use_binary_protocol = true;
if (true == nhPriv.getParam("emul_sensor", emulSensor)) {
ROS_INFO("Found emul_sensor overwriting default settings. Emulation: %s\n",
emulSensor ? "True" : "False");
}
if (true == nhPriv.getParam("use_binary_protocol", use_binary_protocol)) {
ROS_INFO("Found sopas_protocol_type param overwriting default protocol:");
if (use_binary_protocol == true) {
ROS_INFO("Binary protocol activated");
} else {
if (parser->getCurrentParamPtr()->getNumberOfLayers() > 4) {
nhPriv.setParam("sopas_protocol_type", true);
use_binary_protocol = true;
ROS_WARN(
"This scanner type does not support ASCII communication.\n"
"Binary communication has been activated.\n"
"The parameter \"sopas_protocol_type\" has been set to \"True\".");
} else {
ROS_INFO("ASCII protocol activated");
}
namespace armarx
{
std::string protocolToString(SickLaserUnit::Protocol protocol)
{
std::string protocolStr;
switch (protocol)
{
case SickLaserUnit::Protocol::ASCII:
protocolStr = "ASCII";
break;
case SickLaserUnit::Protocol::Binary:
protocolStr = "Binary";
break;
}
return modeStr;
}
parser->getCurrentParamPtr()->setUseBinaryProtocol(use_binary_protocol);
}
if (parser->getCurrentParamPtr()->getUseBinaryProtocol()) {
colaDialectId = 'B';
} else {
colaDialectId = 'A';
}
int result = sick_scan::ExitError;
sick_scan::SickScanConfig cfg;
}
void SickLaserUnit::onConnectComponent() {
ROS_INFO("Start initialising scanner [Ip: %s] [Port: %s]", hostname.c_str(),
port.c_str());
// attempt to connect/reconnect
delete s; // disconnect scanner
if (useTCP) {
s = new sick_scan::SickScanCommonTcp(hostname, port, timelimit, parser,
colaDialectId);
} else {
ROS_ERROR("TCP is not switched on. Probably hostname or port not set. Use "
"roslaunch to start node.");
exit(-1);
}
if (emulSensor) {
s->setEmulSensor(true);
}
result = s->init();
isInitialized = true;
signal(SIGINT,
SIG_DFL); // change back to standard signal handler after initialising
if (result == sick_scan::ExitSuccess) // OK -> loop again
{
if (changeIP) {
runState = scanner_finalize;
armarx::PropertyDefinitionsPtr SickLaserUnit::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr def =
new ComponentPropertyDefinitions(getConfigIdentifier());
// Publish to a topic (passing the TopicListenerPrx).
// def->topic(myTopicListener);
// Subscribe to a topic (passing the topic name).
// def->topic<PlatformUnitListener>("MyTopic");
// Use (and depend on) another component (passing the ComponentInterfacePrx).
// def->component(myComponentProxy)
//communication parameters
def->optional(properties.hostname, "hostname", "Hostname of the LaserScanner");
def->optional(properties.newIpAddress, "newIpAddress", "New IP address for the LaserScanner");
def->optional(properties.port, "port", "port to use on the LaserScanner");
def->optional(properties.timelimit, "timelimit", "timelimit for communication");
def->optional(properties.subscribeDatagram, "subscribeDatagram", "subscribe to Datagram in communication or not");
def->optional(properties.protocol, "protocol", "Either use ASCII or Binary protocol")
.map(protocolToString(Protocol::ASCII), Protocol::ASCII)
.map(protocolToString(Protocol::Binary), Protocol::Binary);
def->optional(properties.sopasProtocolType, "sopasProtocolType", "Automatically set to true if the Scanner does not support ASCII communication");
//Scanner parameters
def->required(properties.scannerType, "scannerType", "Name of the LaserScanner");
def->optional(properties.deviceNumber, "deviceNumber", "number of the LaserScanner Device");
def->required(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
def->required(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
def->optional(properties.timeIncrement, "timeIncrement", "timeIncrement??");
//Additional configuration
def->optional(properties.emulSensor, "emulateSensor", "overwrite the default Settings and don't connect to Scanner");
return def;
}
runState = scanner_run; // after initialising switch to run state
} else {
runState = scanner_init; // If there was an error, try to restart scanner
}
// Do things after connecting to topics and components.
/* (Requies the armarx::DebugObserverComponentPluginUser.)
// Use the debug observer to log data over time.
// The data can be viewed in the ObserverView and the LivePlotter.
// (Before starting any threads, we don't need to lock mutexes.)
{
setDebugObserverDatafield("numBoxes", properties.numBoxes);
setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
sendDebugObserverBatch();
}
*/
/* (Requires the armarx::ArVizComponentPluginUser.)
// Draw boxes in ArViz.
// (Before starting any threads, we don't need to lock mutexes.)
drawBoxes(properties, arviz);
*/
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
// Setup the remote GUI.
{
createRemoteGuiTab();
RemoteGui_startRunningTask();
}
*/
}
void SickLaserUnit::run() {
while (!task->isStopped()) {
if (result == sick_scan::ExitSuccess) // OK -> loop again
void SickLaserUnit::onInitComponent()
{
ros::spinOnce();
result = s->loopOnce();
} else {
runState = scanner_finalize; // interrupt
// Topics and properties defined above are automagically registered.
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
ARMARX_INFO_S << "SickLaserUnit sagt Hallo Welt!";
bool useTCP = false;
if (properties.hostname != "")
{
useTCP = true;
}
bool changeIP = false;
if (sNewIp != "")
{
changeIP = true;
}
//scanner Parameters
sick_scan::SickGenericParser* parser = new sick_scan::SickGenericParser(properties.scannerType);
parser->set_range_min(properties.rangeMin);
parser->set_range_max(properties.rangeMax);
parser->set_time_increment(properties.timeIncrement);
char colaDialectId = 'A'; // A or B (Ascii or Binary)
if (properties.emulSensor)
{
ARMARX_INFO("Found paraemter emulSensor overwriting default settings. Emulation: True\n");
}
switch (properties.protocol)
{
case Protocol::ASCII:
if (parser->getCurrentParamPtr()->getNumberOfLayers() > 4)
{
ARMARX_WARNING(
"This scanner type does not support ASCII communication.\n"
"Binary communication has been activated.\n"
"The parameter \"sopasProtocolType\" has been set to \"true\".");
properties.sopasProtocolType = true;
properties.protocol = Protocol::Binary;
}
else
{
ARMARX_INFO("ASCII protocol activated");
}
break;
case Protocol::Binary:
ARMARX_INFO("Binary protocol activated");
break;
default:
ARMARX_WARNING("Unknown protocol type. Defaulting to Binary protocol.\n");
properties.protocol = Protocol::Binary;
}
if (properties.protocol == Protocol::ASCII)
{
parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
colaDialectId = 'A';
}
else
{
parser->getCurrentParamPtr()->setUseBinaryProtocol(true);
colaDialectId = 'B';
}
}
}
}
void SickLaserUnit::onDisconnectComponent() {}
void SickLaserUnit::onConnectComponent()
{
ARMARX_INFO("Start initialising scanner [Ip: %s] [Port: %s]", hostname.c_str(),
port.c_str());
// attempt to connect/reconnect
delete s; // disconnect scanner
if (useTCP)
{
s = new sick_scan::SickScanCommonTcp(hostname, port, timelimit, parser,
colaDialectId);
}
else
{
ROS_ERROR("TCP is not switched on. Probably hostname or port not set. Use "
"roslaunch to start node.");
exit(-1);
}
void SickLaserUnit::onExitComponent() {}
if (emulSensor)
{
s->setEmulSensor(true);
}
result = s->init();
std::string SickLaserUnit::getDefaultName() const { return "SickLaserUnit"; }
isInitialized = true;
signal(SIGINT,
SIG_DFL); // change back to standard signal handler after initialising
if (result == sick_scan::ExitSuccess) // OK -> loop again
{
if (changeIP)
{
runState = scanner_finalize;
}
runState = scanner_run; // after initialising switch to run state
}
else
{
runState = scanner_init; // If there was an error, try to restart scanner
}
// Do things after connecting to topics and components.
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
void SickLaserUnit::createRemoteGuiTab()
{
/* (Requies the armarx::DebugObserverComponentPluginUser.)
// Use the debug observer to log data over time.
// The data can be viewed in the ObserverView and the LivePlotter.
// (Before starting any threads, we don't need to lock mutexes.)
{
setDebugObserverDatafield("numBoxes", properties.numBoxes);
setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
sendDebugObserverBatch();
}
*/
/* (Requires the armarx::ArVizComponentPluginUser.)
// Draw boxes in ArViz.
// (Before starting any threads, we don't need to lock mutexes.)
drawBoxes(properties, arviz);
*/
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
// Setup the remote GUI.
{
createRemoteGuiTab();
RemoteGui_startRunningTask();
}
*/
}
void SickLaserUnit::run()
{
while (!task->isStopped())
{
if (result == sick_scan::ExitSuccess) // OK -> loop again
{
ros::spinOnce();
result = s->loopOnce();
}
else
{
runState = scanner_finalize; // interrupt
}
}
}
void SickLaserUnit::onDisconnectComponent() {}
void SickLaserUnit::onExitComponent() {}
std::string SickLaserUnit::getDefaultName() const
{
return "SickLaserUnit";
}
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
void SickLaserUnit::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
// Setup the widgets.
......@@ -254,62 +260,62 @@ void SickLaserUnit::createRemoteGuiTab()
GridLayout grid;
int row = 0;
{
grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
++row;
grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
++row;
grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
++row;
grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
++row;
grid.add(tab.drawBoxes, {row, 0}, {2, 1});
++row;
grid.add(tab.drawBoxes, {row, 0}, {2, 1});
++row;
}
VBoxLayout root = {grid, VSpacer()};
RemoteGui_createTab(getName(), root, &tab);
}
}
void SickLaserUnit::RemoteGui_update()
{
void SickLaserUnit::RemoteGui_update()
{
if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
{
std::scoped_lock lock(propertiesMutex);
properties.boxLayerName = tab.boxLayerName.getValue();
properties.numBoxes = tab.numBoxes.getValue();
std::scoped_lock lock(propertiesMutex);
properties.boxLayerName = tab.boxLayerName.getValue();
properties.numBoxes = tab.numBoxes.getValue();
{
setDebugObserverDatafield("numBoxes", properties.numBoxes);
setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
sendDebugObserverBatch();
}
{
setDebugObserverDatafield("numBoxes", properties.numBoxes);
setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
sendDebugObserverBatch();
}
}
if (tab.drawBoxes.wasClicked())
{
// Lock shared variables in methods running in seperate threads
// and pass them to functions. This way, the called functions do
// not need to think about locking.
std::scoped_lock lock(propertiesMutex, arvizMutex);
drawBoxes(properties, arviz);
// Lock shared variables in methods running in seperate threads
// and pass them to functions. This way, the called functions do
// not need to think about locking.
std::scoped_lock lock(propertiesMutex, arvizMutex);
drawBoxes(properties, arviz);
}
}
*/
}
*/
/* (Requires the armarx::ArVizComponentPluginUser.)
void SickLaserUnit::drawBoxes(const SickLaserUnit::Properties& p, viz::Client&
arviz)
{
/* (Requires the armarx::ArVizComponentPluginUser.)
void SickLaserUnit::drawBoxes(const SickLaserUnit::Properties& p, viz::Client&
arviz)
{
// Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
// See the ArVizExample in RobotAPI for more examples.
viz::Layer layer = arviz.layer(p.boxLayerName);
for (int i = 0; i < p.numBoxes; ++i)
{
layer.add(viz::Box("box_" + std::to_string(i))
.position(Eigen::Vector3f(i * 100, 0, 0))
.size(20).color(simox::Color::blue()));
layer.add(viz::Box("box_" + std::to_string(i))
.position(Eigen::Vector3f(i * 100, 0, 0))
.size(20).color(simox::Color::blue()));
}
arviz.commit(layer);
}
*/
}
*/
} // namespace armarx
source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+ 32
− 25
View file @ 6e11930b
......@@ -22,19 +22,19 @@
#pragma once
// #include <mutex>
#include <ArmarXCore/core/Component.h>
// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
// #include
// <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
// #include
// <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
# include <vector>
#include <vector>
namespace armarx
{
......@@ -50,20 +50,21 @@ namespace armarx
*
* Detailed description of class SickLaserUnit.
*/
class SickLaserUnit :
virtual public armarx::Component
class SickLaserUnit : virtual public armarx::Component
// , virtual public armarx::DebugObserverComponentPluginUser
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
// , virtual public armarx::ArVizComponentPluginUser
{
public:
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
enum class Protocol
{
ASCII,
Binary
};
protected:
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
......@@ -79,7 +80,6 @@ namespace armarx
/// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
/* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
/// This function should be called once in onConnect() or when you
/// need to re-create the Remote GUI tab.
......@@ -91,9 +91,7 @@ namespace armarx
void RemoteGui_update() override;
*/
private:
// Private methods go here.
// Forward declare `Properties` if you used it before its defined.
......@@ -104,27 +102,39 @@ namespace armarx
void drawBoxes(const Properties& p, viz::Client& arviz);
*/
private:
// Private member variables go here.
/// Properties shown in the Scenario GUI.
struct Properties
{
std::string laserScannerTopicName;
int updatePeriod = 1;
float angleOffset = 0.0;
std::string devices = "Device1; Scanner2";
//communication parameters
std::string hostname = "";
std::string newIpAddress = "";
std::string port = "2112";
int timelimit = 5;
bool subscribeDatagram = false;
Protocol protocol = Protocol::Binary;
bool sopasProtocolType = false;
//scanner parameters
std::string scannerType;
int deviceNumber = 0;
double rangeMin;
double rangeMax;
double timeIncrement;
//additional parameters
bool emulSensor = false;
};
Properties properties;
char colaDialectId = 'B';
int result = sick_scan::ExitError;
sick_scan::SickScanConfig cfg;
/* Use a mutex if you access variables from different threads
* (e.g. ice functions and RemoteGui_update()).
std::mutex propertiesMutex;
*/
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
/// Tab shown in the Remote GUI.
struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
......@@ -137,13 +147,10 @@ namespace armarx
RemoteGuiTab tab;
*/
/* (Requires the armarx::ArVizComponentPluginUser.)
* When used from different threads, an ArViz client needs to be synchronized.
/// Protects the arviz client inherited from the ArViz plugin.
std::mutex arvizMutex;
*/
};
}
} // namespace armarx
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment