YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FakeLaserWithMotor.h
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#ifndef FAKE_LASER_WITH_MOTOR_H
8#define FAKE_LASER_WITH_MOTOR_H
9
10
23#include <yarp/dev/MapGrid2D.h>
24#include <yarp/dev/PolyDriver.h>
25#include <yarp/sig/Vector.h>
26
27#include <mutex>
28#include <random>
29#include <string>
30
32
59 // public yarp::dev::StubImplTorqueControlRaw,
68{
69protected:
71
73
76
77 //motor stuff
78 size_t m_njoints=3;
79
80 //this is the position of the localized robot in the map
81 double m_robot_loc_x=0;
82 double m_robot_loc_y=0;
83 double m_robot_loc_t=0;
84
85 std::random_device* m_rd;
86 std::mt19937* m_gen;
87 std::uniform_real_distribution<>* m_dis;
88
90
91public:
92 FakeLaserWithMotor(double period = 0.02) : PeriodicThread(period),
100 {
101 //default parameters
102 m_min_distance = 0.1; //m
103 m_max_distance = 8.0; //m
104 m_min_angle = 0; //degrees
105 m_max_angle = 360; //degrees
106 m_resolution = 1.0; //degrees
107 m_scan_rate = period; //s
108
109 //noise generator
110 m_rd = new std::random_device;
111 m_gen = new std::mt19937((*m_rd)());
112 m_dis = new std::uniform_real_distribution<>(0, 0.01);
113 }
114
116 {
117 delete m_rd;
118 delete m_gen;
119 delete m_dis;
120 m_rd = 0;
121 m_gen = 0;
122 m_dis = 0;
123 }
124
125 bool open(yarp::os::Searchable& config) override;
126 bool close() override;
127 bool threadInit() override;
128 void threadRelease() override;
129 void run() override;
130
131private:
132 void drawStraightLine(yarp::dev::Nav2D::XYCell src, yarp::dev::Nav2D::XYCell dst);
133 void wall_the_robot(double siz = 1.0, double dist = 1.0);
134 void obst_the_robot(double siz = 1.0, double dist = 1.0);
135 void trap_the_robot(double siz = 1.0);
136 void free_the_robot();
137
138 double checkStraightLine(yarp::dev::Nav2D::XYCell src, yarp::dev::Nav2D::XYCell dst);
139 bool LiangBarsky_clip(int edgeLeft, int edgeRight, int edgeTop, int edgeBottom,
141 yarp::dev::Nav2D::XYCell& src_clipped, yarp::dev::Nav2D::XYCell& dst_clipped);
142
143public:
144 //IRangefinder2D interface
145 yarp::dev::ReturnValue setDistanceRange (double min, double max) override;
146 yarp::dev::ReturnValue setScanLimits (double min, double max) override;
148 yarp::dev::ReturnValue setScanRate (double rate) override;
149
150public:
151 // internal stuff
152 std::string* _axisName = nullptr; // axis name
153 yarp::dev::JointTypeEnum* _jointType = nullptr; // axis type
154 int* _controlModes = nullptr;
155 double* _encoders = nullptr; // encoders
156 double* _posCtrl_references = nullptr; // used for position control.
157 double* _ref_speeds = nullptr; // used for position control.
158 double* _command_speeds = nullptr; // used for velocity control.
159 double* _ref_accs = nullptr; // used for velocity control.
160 std::vector<yarp::dev::SelectableControlModeEnum>* _availableControlModes = nullptr;
161
162 bool alloc(int njoints);
163 bool dealloc();
164
165public:
166 //motor interfaces
169 yarp::dev::ReturnValue setEncoderRaw(int j, double val) override;
170 yarp::dev::ReturnValue setEncodersRaw(const double* vals) override;
171 yarp::dev::ReturnValue getEncoderRaw(int j, double* v) override;
172 yarp::dev::ReturnValue getEncodersRaw(double* encs) override;
173 yarp::dev::ReturnValue getEncoderSpeedRaw(int j, double* sp) override;
174 yarp::dev::ReturnValue getEncoderSpeedsRaw(double* spds) override;
175 yarp::dev::ReturnValue getEncoderAccelerationRaw(int j, double* spds) override;
177
178 yarp::dev::ReturnValue getEncodersTimedRaw(double* encs, double* stamps) override;
179 yarp::dev::ReturnValue getEncoderTimedRaw(int j, double* encs, double* stamp) override;
180
181 // POSITION CONTROL INTERFACE RAW
182 yarp::dev::ReturnValue getAxes(size_t& ax) override;
183 yarp::dev::ReturnValue positionMoveRaw(int j, double ref) override;
184 yarp::dev::ReturnValue positionMoveRaw(const double* refs) override;
185 yarp::dev::ReturnValue relativeMoveRaw(int j, double delta) override;
186 yarp::dev::ReturnValue relativeMoveRaw(const double* deltas) override;
187 yarp::dev::ReturnValue checkMotionDoneRaw(bool& flag) override;
188 yarp::dev::ReturnValue checkMotionDoneRaw(int j, bool& flag) override;
189 yarp::dev::ReturnValue setTrajSpeedRaw(int j, double sp) override;
190 yarp::dev::ReturnValue setTrajSpeedsRaw(const double* spds) override;
191 yarp::dev::ReturnValue setTrajAccelerationRaw(int j, double acc) override;
192 yarp::dev::ReturnValue setTrajAccelerationsRaw(const double* accs) override;
193 yarp::dev::ReturnValue getTrajSpeedRaw(int j, double* ref) override;
194 yarp::dev::ReturnValue getTrajSpeedsRaw(double* spds) override;
195 yarp::dev::ReturnValue getTrajAccelerationRaw(int j, double* acc) override;
196 yarp::dev::ReturnValue getTrajAccelerationsRaw(double* accs) override;
197 yarp::dev::ReturnValue stopRaw(int j) override;
199
200 yarp::dev::ReturnValue positionMoveRaw(const int n_joint, const int* joints, const double* refs) override;
201 yarp::dev::ReturnValue relativeMoveRaw(const int n_joint, const int* joints, const double* deltas) override;
202 yarp::dev::ReturnValue checkMotionDoneRaw(const std::vector<int>& joints, bool& flags) override;
203 yarp::dev::ReturnValue setTrajSpeedsRaw(const int n_joint, const int* joints, const double* spds) override;
204 yarp::dev::ReturnValue setTrajAccelerationsRaw(const int n_joint, const int* joints, const double* accs) override;
205 yarp::dev::ReturnValue getTrajSpeedsRaw(const int n_joint, const int* joints, double* spds) override;
206 yarp::dev::ReturnValue getTrajAccelerationsRaw(const int n_joint, const int* joints, double* accs) override;
207 yarp::dev::ReturnValue stopRaw(const int n_joint, const int* joints) override;
208 yarp::dev::ReturnValue getTargetPositionRaw(const int joint, double* ref) override;
209 yarp::dev::ReturnValue getTargetPositionsRaw(double* refs) override;
210 yarp::dev::ReturnValue getTargetPositionsRaw(const int n_joint, const int* joints, double* refs) override;
211
212 //IControlMode
213 yarp::dev::ReturnValue getAvailableControlModesRaw(int j, std::vector<yarp::dev::SelectableControlModeEnum>& avail) override;
215 yarp::dev::ReturnValue getControlModesRaw(std::vector<yarp::dev::ControlModeEnum>& mode) override;
216 yarp::dev::ReturnValue getControlModesRaw(const std::vector<int>& j, std::vector<yarp::dev::ControlModeEnum>& mode) override;
218 yarp::dev::ReturnValue setControlModesRaw(const std::vector<int>& j, const std::vector<yarp::dev::SelectableControlModeEnum>& mode) override;
219 yarp::dev::ReturnValue setControlModesRaw(const std::vector<yarp::dev::SelectableControlModeEnum>& mode) override;
220
221 //IVelocityControl
222 yarp::dev::ReturnValue velocityMoveRaw(int j, double sp) override;
223 yarp::dev::ReturnValue velocityMoveRaw(const double* sp) override;
224 yarp::dev::ReturnValue velocityMoveRaw(const int n_joint, const int* joints, const double* spds) override;
225 yarp::dev::ReturnValue getTargetVelocityRaw(const int joint, double* ref) override;
226 yarp::dev::ReturnValue getTargetVelocitiesRaw(double* refs) override;
227 yarp::dev::ReturnValue getTargetVelocitiesRaw(const int n_joint, const int* joints, double* refs) override;
228
229 yarp::dev::ReturnValue getAxisNameRaw(int axis, std::string& name) override;
231public:
232 //Lidar2DDeviceBase
233 bool acquireDataFromHW() override final;
234
235public:
236 bool read(yarp::os::ConnectionReader& connection) override;
237};
238
239#endif
define control board standard interfaces
contains the definition of a map type
contains the definition of a Vector type
This class is the parameters parser for class FakeLaserWithMotor.
fakeLaserWithMotor : fake sensor device driver for testing purposes and reference for IRangefinder2D ...
yarp::dev::ReturnValue getTrajSpeedRaw(int j, double *ref) override
Get reference speed for a joint.
yarp::dev::ReturnValue setTrajAccelerationRaw(int j, double acc) override
Set reference acceleration for a joint.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue getEncoderSpeedRaw(int j, double *sp) override
Read the instantaneous speed of an axis.
void run() override
Loop function.
yarp::dev::ReturnValue positionMoveRaw(int j, double ref) override
Set new reference point for a single axis.
yarp::dev::ReturnValue setDistanceRange(double min, double max) override
set the device detection range.
yarp::dev::ReturnValue setTrajAccelerationsRaw(const double *accs) override
Set reference acceleration on all joints.
yarp::dev::ReturnValue setTrajSpeedRaw(int j, double sp) override
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
yarp::dev::ReturnValue getTrajAccelerationRaw(int j, double *acc) override
Get reference acceleration for a joint.
yarp::dev::ReturnValue getTargetVelocitiesRaw(double *refs) override
Get the last reference speed set by velocityMove for all joints.
yarp::dev::Nav2D::MapGrid2D m_originally_loaded_map
yarp::dev::ReturnValue getControlModeRaw(int j, yarp::dev::ControlModeEnum &mode) override
bool threadInit() override
Initialization method.
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue checkMotionDoneRaw(bool &flag) override
Check if the current trajectory all joints is terminated.
yarp::dev::ReturnValue getEncoderTimedRaw(int j, double *encs, double *stamp) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getTrajSpeedsRaw(double *spds) override
Get reference speed of all joints.
yarp::dev::ReturnValue relativeMoveRaw(int j, double delta) override
Set relative position.
yarp::dev::ReturnValue setControlModeRaw(int j, yarp::dev::SelectableControlModeEnum mode) override
yarp::dev::ReturnValue getAxisNameRaw(int axis, std::string &name) override
FakeLaserWithMotor(double period=0.02)
yarp::dev::ReturnValue getTargetPositionRaw(const int joint, double *ref) override
Get the last position reference for the specified axis.
yarp::dev::ReturnValue getTrajAccelerationsRaw(double *accs) override
Get reference acceleration of all joints.
yarp::dev::ReturnValue getEncodersRaw(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue getControlModesRaw(std::vector< yarp::dev::ControlModeEnum > &mode) override
yarp::dev::ReturnValue getEncodersTimedRaw(double *encs, double *stamps) override
Read the instantaneous acceleration of all axes.
yarp::dev::ReturnValue getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
yarp::dev::ReturnValue velocityMoveRaw(int j, double sp) override
Start motion at a given speed, single joint.
void threadRelease() override
Release method.
yarp::dev::ReturnValue setScanRate(double rate) override
set the scan rate (scans per seconds)
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
yarp::dev::ReturnValue getTargetPositionsRaw(double *refs) override
Get the last position reference for all axes.
yarp::dev::ReturnValue setEncoderRaw(int j, double val) override
Set the value of the encoder for a given joint.
yarp::dev::ReturnValue setControlModesRaw(const std::vector< int > &j, const std::vector< yarp::dev::SelectableControlModeEnum > &mode) override
yarp::dev::ReturnValue getTargetVelocityRaw(const int joint, double *ref) override
Get the last reference speed set by velocityMove for single joint.
std::random_device * m_rd
yarp::dev::ReturnValue resetEncodersRaw() override
Reset encoders.
yarp::dev::JointTypeEnum * _jointType
yarp::dev::ReturnValue getAvailableControlModesRaw(int j, std::vector< yarp::dev::SelectableControlModeEnum > &avail) override
yarp::dev::ReturnValue setTrajSpeedsRaw(const double *spds) override
Set reference speed on all joints.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
yarp::dev::ReturnValue setHorizontalResolution(double step) override
get the angular step between two measurements (if available)
std::vector< yarp::dev::SelectableControlModeEnum > * _availableControlModes
bool acquireDataFromHW() override final
This method should be implemented by the user, and contain the logic to grab data from the hardware.
yarp::os::Port m_rpcPort
yarp::dev::ReturnValue setEncodersRaw(const double *vals) override
Set the value of all encoders.
yarp::dev::ReturnValue getEncoderSpeedsRaw(double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue setScanLimits(double min, double max) override
set the scan angular range.
yarp::dev::ReturnValue resetEncoderRaw(int j) override
Reset encoder, single joint.
std::uniform_real_distribution * m_dis
yarp::dev::ReturnValue getEncoderAccelerationRaw(int j, double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue stopRaw() override
Stop motion, multiple joints.
yarp::dev::Nav2D::MapGrid2D m_map
yarp::dev::ReturnValue getEncoderRaw(int j, double *v) override
Read the value of an encoder.
yarp::dev::ReturnValue getEncoderAccelerationsRaw(double *accs) override
Read the instantaneous acceleration of all axes.
Interface implemented by all device drivers.
Interface for getting information about specific axes, if available.
Definition IAxisInfo.h:73
Interface for setting control mode in control board.
Control board, extend encoder raw interface adding timestamps.
Interface for a generic control board device implementing position control in encoder coordinates.
Interface for control boards implementing velocity control in encoder coordinates.
ImplementAxisInfo(yarp::dev::IAxisInfoRaw *y)
ImplementEncodersTimed(yarp::dev::IEncodersTimedRaw *y)
Interface settings the way the robot interacts with the environment: basic interaction types are Stif...
ImplementInteractionMode(yarp::dev::IInteractionModeRaw *Class_p)
Constructor.
Default implementation of the IPositionControl interface.
ImplementPositionControl(yarp::dev::IPositionControlRaw *y)
Constructor.
ImplementVelocityControl(yarp::dev::IVelocityControlRaw *y)
Constructor.
The Lidar2DDeviceBase class.
Stub implementation of IInteractionMode interface.
An abstraction for a periodic thread.
PeriodicThread(double period, ShouldUseSystemClock useSystemClock=ShouldUseSystemClock::No, PeriodicThreadClock clockAccuracy=PeriodicThreadClock::Relative)
Constructor.
void step()
Call this to "step" the thread rather than starting it.
Interface implemented by all objects that can read themselves from the network, such as Bottle object...
Definition PortReader.h:24
A mini-server for network communication.
Definition Port.h:46
A base class for nested structures that can be searched.
Definition Searchable.h:31
SelectableControlModeEnum
The main, catch-all namespace for YARP.
Definition dirs.h:16