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

Integrate and shorten the inherited loopOnce method of SickScanAdapter

parent c15de133
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 1704 additions and 2060 deletions
etc/cmake/Findsick_scan_base.cmake
+ 10
− 1
View file @ 65aa3878
......@@ -20,8 +20,16 @@ set(HEADER_SEARCH_PATHS
${sick_scan_base_DIR}/roswrap/src/cfgsimu/
ENV CPATH
/usr/include/
#/opt/ros/melodic/include/
#/opt/ros/melodic/include/sick_scan/
)
#find_path(sick_scan_base_INCLUDE_DIR_7 NAMES sick_scan_common.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
#if(sick_scan_base_INCLUDE_DIR_7)
# set(sick_scan_base_INCLUDE_DIRS ${sick_scan_base_INCLUDE_DIR_7})
#endif()
#find_library(sick_scan_base_LIBRARIES NAMES libsick_scan_lib.so PATHS /opt/ros/melodic/lib/ NO_DEFAULT_PATH)
find_library(sick_scan_base_LIBRARIES NAMES libsick_scan_generic.so PATHS ${sick_scan_base_DIR}/build NO_DEFAULT_PATH)
message(STATUS "sick_scan_base_LIBRARIES:${sick_scan_base_LIBRARIES}")
......@@ -33,10 +41,11 @@ find_path(sick_scan_base_INCLUDE_DIR_4 NAMES dynamic_reconfigure/config_tools.h
find_path(sick_scan_base_INCLUDE_DIR_5 NAMES sick_scan/SickScanConfig.h PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
find_path(sick_scan_base_INCLUDE_DIR_6 NAMES sick_scan/rosconsole_simu.hpp PATHS ${HEADER_SEARCH_PATHS} NO_DEFAULT_PATH)
if(sick_scan_base_INCLUDE_DIR_0 AND sick_scan_base_INCLUDE_DIR_1 AND sick_scan_base_INCLUDE_DIR_2 AND sick_scan_base_INCLUDE_DIR_3 AND sick_scan_base_INCLUDE_DIR_4 AND sick_scan_base_INCLUDE_DIR_5 AND sick_scan_base_INCLUDE_DIR_6)
set(sick_scan_base_INCLUDE_DIRS ${sick_scan_base_INCLUDE_DIR_0} ${sick_scan_base_INCLUDE_DIR_1} ${sick_scan_base_INCLUDE_DIR_2} ${sick_scan_base_INCLUDE_DIR_3} ${sick_scan_base_INCLUDE_DIR_4} ${sick_scan_base_INCLUDE_DIR_5} ${sick_scan_base_INCLUDE_DIR_6})
endif()
message(STATUS "sick_scan_base_INCLUDE_DIRS:${sick_scan_base_INCLUDE_DIRS}")
find_package_handle_standard_args(sick_scan_base DEFAULT_MSG sick_scan_base_INCLUDE_DIRS sick_scan_base_LIBRARIES)
......
scenarios/SickLaserUnitTest/config/SickLaserUnit.cfg
+ 3
− 30
View file @ 65aa3878
......@@ -167,21 +167,12 @@ ArmarX.SickLaserUnit.angleOffset = 0.0
ArmarX.SickLaserUnit.devices = Device1; Scanner2
# ArmarX.SickLaserUnit.emulateSensor: overwrite the default Settings and don't connect to Scanner
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.SickLaserUnit.emulateSensor = false
# ArmarX.SickLaserUnit.hostname: Hostname of the LaserScanner
# Attributes:
# - Default: 192.168.8.133
# - Case sensitivity: yes
# - Required: no
# ArmarX.SickLaserUnit.hostname = 192.168.8.133
ArmarX.SickLaserUnit.hostname = 192.168.8.133
# ArmarX.SickLaserUnit.laserScannerTopicName: No Description
......@@ -192,14 +183,6 @@ ArmarX.SickLaserUnit.devices = Device1; Scanner2
ArmarX.SickLaserUnit.laserScannerTopicName = "SickTopic"
# ArmarX.SickLaserUnit.newIpAddress: New IP address for the LaserScanner
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.SickLaserUnit.newIpAddress = ""
# ArmarX.SickLaserUnit.port: port to use on the LaserScanner
# Attributes:
# - Default: 2112
......@@ -208,12 +191,11 @@ ArmarX.SickLaserUnit.laserScannerTopicName = "SickTopic"
ArmarX.SickLaserUnit.port = 2112
# ArmarX.SickLaserUnit.protocol: Either use ASCII or Binary protocol
# ArmarX.SickLaserUnit.protocol: No Description
# Attributes:
# - Default: ASCII
# - Case sensitivity: yes
# - Case sensitivity: no
# - Required: no
# - Possible values: {ASCII, Binary}
ArmarX.SickLaserUnit.protocol = ASCII
......@@ -240,15 +222,6 @@ ArmarX.SickLaserUnit.rangeMin = 0.01
ArmarX.SickLaserUnit.scannerType = sick_tim_5xx
# ArmarX.SickLaserUnit.sopasProtocolType: Automatically set to true if the Scanner does not support ASCII communication
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.SickLaserUnit.sopasProtocolType = false
# ArmarX.SickLaserUnit.subscribeDatagram: subscribe to Datagram in communication or not
# Attributes:
# - Default: false
......
source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+ 2
− 2
View file @ 65aa3878
......@@ -45,11 +45,11 @@ armarx_add_component(
SOURCES
SickLaserUnit.cpp
#SickScanAdapter.cpp
SickScanAdapter.cpp
HEADERS
SickLaserUnit.h
#SickScanAdapter.h
SickScanAdapter.h
)
......
source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+ 10
− 86
View file @ 65aa3878
......@@ -32,22 +32,6 @@
namespace armarx
{
std::string protocolToString(ScanProtocol protocol)
{
std::string protocolStr;
switch (protocol)
{
case ScanProtocol::ASCII:
protocolStr = "ASCII";
break;
case ScanProtocol::Binary:
protocolStr = "Binary";
break;
}
return protocolStr;
}
void SickLaserScanDevice::run()
{
while (!task->isStopped())
......@@ -87,37 +71,26 @@ namespace armarx
delete this->scanner; // disconnect scanner
if (this->useTcp)
{
//this->scanner = new SickScanAdapter(this->ip, this->port, this->timelimit, this->parser, this->colaDialectId);
this->scanner = new sick_scan::SickScanCommonTcp(this->ip, this->port, this->timelimit, this->parser, this->colaDialectId);
this->scanner = new SickScanAdapter(this->ip, this->port, this->timelimit, this->parser, 'A');
//this->scanner = new sick_scan::SickScanCommonTcp(this->ip, this->port, this->timelimit, this->parser, 'A');
}
else
{
ARMARX_ERROR_S << "TCP is not switched on. Probably hostname or port not set.\n";
return;
}
if (this->emulSensor)
{
this->scanner->setEmulSensor(true);
}
result = this->scanner->init();
this->isSensorInitialized = true;
ARMARX_INFO_S << "Scanner initialized.";
if (result == sick_scan::ExitSuccess) // OK -> loop again
{
if (this->changeIP)
{
this->runState = RunState::scannerFinalize;
}
else
{
this->runState = RunState::scannerRun; // after initialising switch to run state
}
this->isSensorInitialized = true;
ARMARX_INFO_S << "Scanner initialized.";
this->runState = RunState::scannerRun; // after initialising switch to run state
}
else
{
ARMARX_INFO_S << "Reinitializing scanner.";
this->runState = RunState::scannerInit; // If there was an error, try to restart scanner
}
}
......@@ -138,14 +111,9 @@ namespace armarx
//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(ScanProtocol::ASCII), ScanProtocol::ASCII)
.map(protocolToString(ScanProtocol::Binary), ScanProtocol::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");
......@@ -153,8 +121,6 @@ namespace armarx
def->optional(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
def->optional(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;
}
......@@ -178,11 +144,6 @@ namespace armarx
scanDevice.angleOffset = properties.angleOffset;
scanDevice.timelimit = properties.timelimit;
scanDevice.subscribeDatagram = properties.subscribeDatagram;
if (properties.newIpAddress != "")
{
scanDevice.changeIP = true;
scanDevice.newIpAddress = properties.newIpAddress;
}
scanDevice.scannerType = properties.scannerType;
scanDevice.deviceNumber = properties.deviceNumber;
scanDevice.rangeMin = properties.rangeMin;
......@@ -201,47 +162,10 @@ namespace armarx
ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
return;
}
if (properties.emulSensor)
{
ARMARX_INFO_S << "Found paraemter emulSensor overwriting default settings. Emulation: True";
scanDevice.emulSensor = true;
}
switch (properties.protocol)
{
case ScanProtocol::ASCII:
if (scanDevice.parser->getCurrentParamPtr()->getNumberOfLayers() > 4)
{
ARMARX_WARNING_S << "This scanner type does not support ASCII communication.\n"
<< "Binary communication has been activated.\n"
<< "The parameter \"sopasProtocolType\" has been set to \"true\".";
scanDevice.sopasProtocolType = true;
scanDevice.protocol = ScanProtocol::Binary;
}
else
{
ARMARX_INFO_S << "ASCII protocol activated.";
scanDevice.protocol = ScanProtocol::ASCII;
}
break;
case ScanProtocol::Binary:
ARMARX_INFO_S << "Binary protocol activated.";
scanDevice.protocol = ScanProtocol::Binary;
break;
default:
ARMARX_WARNING_S << "Unknown protocol type. Defaulting to Binary protocol.";
scanDevice.protocol = ScanProtocol::Binary;
}
scanDevice.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
scanDevice.colaDialectId = 'A';
ARMARX_INFO_S << "ASCII protocol activated.";
if (scanDevice.protocol == ScanProtocol::ASCII)
{
scanDevice.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
scanDevice.colaDialectId = 'A';
}
else
{
scanDevice.parser->getCurrentParamPtr()->setUseBinaryProtocol(true);
scanDevice.colaDialectId = 'B';
}
ARMARX_INFO_S << "SickLaserUnit initialisation complete.";
}
......
source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+ 4
− 12
View file @ 65aa3878
......@@ -34,7 +34,7 @@
#include <vector>
//#include "SickScanAdapter.h"
#include "SickScanAdapter.h"
#include <sick_scan/sick_scan_common.h>
#include <sick_scan/sick_scan_common_tcp.h>
#include <sick_scan/sick_generic_laser.h>
......@@ -66,23 +66,18 @@ namespace armarx
double timeIncrement;
//communication parameters
std::string ip;
std::string newIpAddress = "";
std::string port;
int timelimit = 5;
bool subscribeDatagram = false;
ScanProtocol protocol = ScanProtocol::ASCII;
bool sopasProtocolType = false;
bool useTcp = false;
bool changeIP = false;
bool emulSensor = false;
char colaDialectId = 'B';
char colaDialectId = 'A';
//data and task pointers
std::vector<long> scanData;
RunState runState = RunState::scannerFinalize;
RunningTask<SickLaserScanDevice>::pointer_type task;
sick_scan::SickScanConfig cfg;
//SickScanAdapter* scanner;
sick_scan::SickScanCommonTcp* scanner;
SickScanAdapter* scanner;
//sick_scan::SickScanCommonTcp* scanner;
sick_scan::SickGenericParser* parser;
int result = sick_scan::ExitError;
bool isSensorInitialized = false;
......@@ -156,12 +151,9 @@ namespace armarx
{
//communication parameters
std::string hostname = "192.168.8.133";
std::string newIpAddress = "";
std::string port = "2112";
int timelimit = 5;
bool subscribeDatagram = false;
ScanProtocol protocol = ScanProtocol::ASCII;
bool sopasProtocolType = false;
//scanner parameters
std::string scannerType = "sick_tim_5xx";
int deviceNumber = 0;
......
source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+ 1669
− 1929
View file @ 65aa3878
This diff is collapsed.
source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+ 6
− 0
View file @ 65aa3878
......@@ -35,6 +35,8 @@
#ifndef SICK_TIM3XX_COMMON_TCP_H
#define SICK_TIM3XX_COMMON_TCP_H
#include <ArmarXCore/core/Component.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
......@@ -70,6 +72,9 @@ namespace armarx
public:
int loopOnce();
int parse_datagram(char* datagram, size_t datagram_length, sick_scan::SickScanConfig& config,
sensor_msgs::LaserScan& msg, int& numEchos, int& echoMask);
static void disconnectFunctionS(void* obj);
SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit, sick_scan::SickGenericParser* parser,
......@@ -161,6 +166,7 @@ namespace armarx
boost::system::error_code ec_;
size_t bytes_transfered_;
sick_scan::SickGenericParser* parser_ptr;
std::string hostname_;
std::string port_;
int timelimit_;
......
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