6#define _USE_MATH_DEFINES
23#define DEG2RAD M_PI/180.0
34 if (!this->parseParams(config)) {
return false;}
36 m_info =
"Fake Laser device for test/debugging";
37 m_device_status = DEVICE_OK_STANDBY;
43 if (config.check(
"help"))
47 yCInfo(
FAKE_LASER_WITH_MOTOR,
"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaserWithMotor --period 0.01 --name /fakeLaser:o --test no_obstacles");
48 yCInfo(
FAKE_LASER_WITH_MOTOR,
"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaserWithMotor --period 0.01 --name /fakeLaser:o --test use_pattern");
49 yCInfo(
FAKE_LASER_WITH_MOTOR,
"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaserWithMotor --period 0.01 --name /fakeLaser:o --test use_constant --const_distance 0.5");
50 yCInfo(
FAKE_LASER_WITH_MOTOR,
"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaserWithMotor --period 0.01 --name /fakeLaser:o --test use_constant --const_distance 0.5 --SENSOR::resolution 0.5 --SKIP::min 0 50 --SKIP::max 10 60");
51 yCInfo(
FAKE_LASER_WITH_MOTOR,
"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaserWithMotor --period 0.01 --name /fakeLaser:o --test use_mapfile --map_file mymap.map");
52 yCInfo(
FAKE_LASER_WITH_MOTOR,
"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaserWithMotor --period 0.01 --name /fakeLaser:o --test use_mapfile --map_file mymap.map --localization_port /fakeLaser/location:i");
53 yCInfo(
FAKE_LASER_WITH_MOTOR,
"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaserWithMotor --period 0.01 --name /fakeLaser:o --test use_mapfile --map_file mymap.map --localization_server /localizationServer");
54 yCInfo(
FAKE_LASER_WITH_MOTOR,
"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaserWithMotor --period 0.01 --name /fakeLaser:o --test use_mapfile --map_file mymap.map --localization_client /fakeLaser/localizationClient --localization_server /localizationServer");
55 yCInfo(
FAKE_LASER_WITH_MOTOR,
"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaserWithMotor --period 0.01 --name /fakeLaser:o --test use_mapfile --map_context context --map_file mymap.map");
56 yCInfo(
FAKE_LASER_WITH_MOTOR,
"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaserWithMotor --period 0.01 --name /fakeLaser:o --test use_mapfile --map_file mymap.map --localization_client /fakeLaser/localizationClient --localization_server /localization2D_nws_yarp --localization_device localization2D_nwc_yarp");
64 else if (
string_test_mode ==
"use_constant") { m_test_mode = USE_CONSTANT_VALUE; }
65 else if (
string_test_mode ==
"use_square_trap") { m_test_mode = USE_SQUARE_TRAP; }
69 if (this->parseConfiguration(config) ==
false)
75 if (m_test_mode == USE_SQUARE_TRAP)
80 m_map.m_map_name =
"test_map_10x10m";
81 m_map.m_resolution = 0.01;
82 m_map.m_origin.setOrigin(-5,-5,0);
83 m_map.setSize_in_meters(10,10);
84 for (
size_t y = 0; y < m_map.m_height; y++)
86 for (
size_t x = 0; x < m_map.m_width; x++)
92 m_originally_loaded_map = m_map;
95 m_map.saveToFile(
"mio1");
97 else if (m_test_mode == USE_MAPFILE)
100 if (!m_MAP_MODE_map_context.empty() && !m_MAP_MODE_map_file.empty())
121 else if (!m_MAP_MODE_map_file.empty())
136 bool ret = m_originally_loaded_map.loadFromFile(
map_file);
142 m_map = m_originally_loaded_map;
154 std::vector<int> axisMap; axisMap.resize(m_njoints, 1.0);
164 if (!alloc(m_njoints))
169 _jointType[0] = JointTypeEnum::VOCAB_JOINTTYPE_PRISMATIC;
170 _jointType[1] = JointTypeEnum::VOCAB_JOINTTYPE_PRISMATIC;
171 _jointType[2] = JointTypeEnum::VOCAB_JOINTTYPE_REVOLUTE;
172 _axisName[0] =
"joint_x";
173 _axisName[1] =
"joint_y";
174 _axisName[2] =
"joint_t";
179 if (!m_rpcPort.open(
"/fakeLaser/rpc:i"))
184 m_rpcPort.setReader(*
this);
240 bool ok = command.
read(connection);
248 if (command.
size() == 1)
253 else if (command.
size() == 2)
265 if (command.
size() == 1)
267 wall_the_robot(1.0, 1.0);
268 wall_the_robot(1.0, 1.05);
271 else if (command.
size() == 2)
277 else if (command.
size() == 3)
296 reply.
addString(
"wall <size> <distance>: creates a wall in front of the robot");
297 reply.
addString(
"trap <size>: traps the robot in a box obstacle");
298 reply.
addString(
"free: removes all generated obstacles");
const yarp::os::LogComponent & FAKE_LASER_WITH_MOTOR()
const yarp::os::LogComponent & FAKE_LASER()
constexpr yarp::conf::vocab32_t VOCAB_OK
constexpr yarp::conf::vocab32_t VOCAB_ERR
constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION
constexpr yarp::conf::vocab32_t VOCAB_CM_VELOCITY
fakeLaserWithMotor : fake sensor device driver for testing purposes and reference for IRangefinder2D ...
void run() override
Loop function.
bool threadInit() override
Initialization method.
bool close() override
Close the DeviceDriver.
void threadRelease() override
Release method.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory.
bool initialize(int k, const int *amap)
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool initialize(int size, const int *amap)
Initialize the internal data and alloc memory, smaller version.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
virtual bool updateLidarData()
This utility method calls in sequence: grabDataFromHW(), updateTimestamp and applyLimitsOnLaserData()...
A simple collection of objects that can be described and transmitted in a portable way.
void addVocab32(yarp::conf::vocab32_t x)
Places a 32 bit vocabulary item in the bottle, at the end of the list.
size_type size() const
Gets the number of elements in the bottle.
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
void clear()
Empties the bottle of any objects it contains.
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
void addString(const char *str)
Places a string in the bottle, at the end of the list.
A mini-server for performing network communication in the background.
An interface for reading from a network connection.
virtual ConnectionWriter * getWriter()=0
Gets a way to reply to the message, if possible.
An interface for writing to a network connection.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
void interrupt() override
Interrupt any current reads or writes attached to the port.
void close() override
Stop port activity.
Helper class for finding config files and other external resources.
bool setDefaultContext(const std::string &contextName)
Sets the context for the current ResourceFinder object.
bool configure(int argc, char *argv[], bool skipFirstArgument=true)
Sets up the ResourceFinder.
std::string findFile(const std::string &name)
Find the full path to a file.
bool setDefaultConfigFile(const std::string &fname)
Provide a default value for the configuration file (can be overridden from command line with the –fro...
A base class for nested structures that can be searched.
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
virtual std::string asString() const
Get string value.
#define yCInfo(component,...)
#define yCError(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
yarp::math::Vec2D< size_t > XYCell
The main, catch-all namespace for YARP.