19#include <yarp/dev/impl/jointData.h>
32 rerun::RecordingStream*
m_rec =
nullptr;
70 bool m_isRunning =
true;
71 std::string m_localPortname =
"/yarprerun:i";
72 std::string m_remotePortname;
73 std::string m_datatype =
"Bottle";
74 std::string m_session_name =
"yarprerun";
90 yInfo() <<
"yarpRerun --data <dataType> [--local <local_portname>] [--remote <remote_portname>] [--session_name <session_name>]";
91 yInfo() <<
"<dataType> : Supported types are: SensorStreamingData, Bottle, RGBImage, DepthImage, Vector, RobotState, Lidar2D, Pointcloud ";
92 yInfo() <<
"<local_portname> : Optional. Sets the name of the input port. By default: /yarprerun:i";
93 yInfo() <<
"<remote_portname> : Optional. If set, it automatically tries to connect the specified port";
94 yInfo() <<
"<session_name> : Optional. Sets the name of the rerun session. By default: yarprerun";
99 if (rf.
check(
"local"))
104 if (rf.
check(
"remote"))
109 if (rf.
check(
"data"))
113 yInfo() <<
"Selected " << m_datatype <<
" data type";
118 yError() <<
"Unable to open port" << m_localPortname;
122 if (rf.
check(
"session_name"))
126 m_rec =
new rerun::RecordingStream(m_session_name);
127 m_rec->spawn().exit_on_failure();
129 if (m_remotePortname !=
"")
134 yError() <<
"Unable to connect to remote port: " << m_remotePortname;
139 yInfo() <<
"Connected to: " << m_remotePortname;
144 yInfo() <<
"Waiting for connection..";
156 if (
prt ==
nullptr) {
return true; }
165 if (m_datatype ==
"SensorStreamingData")
169 else if (m_datatype ==
"Bottle")
173 else if (m_datatype ==
"RGBImage")
177 else if (m_datatype ==
"DepthImage")
181 else if (m_datatype ==
"Vector")
185 else if (m_datatype ==
"RobotState")
189 else if (m_datatype ==
"Lidar2D")
193 else if (m_datatype ==
"Pointcloud")
199 yError() <<
"Unknown datatype " << m_datatype;
213 m_rec->log(
"camera", rerun::Image(
216 rerun::datatypes::ColorModel::RGB
229 std::vector<rerun::Position2D> positions;
230 for (
size_t i=0; i<
las.scans.size(); i++)
232 double step = (
las.angle_max -
las.angle_min) /
double(
las.scans.size());
234 double y =
las.scans[i]*
sin(
angle*3.14159265359/180.0);
235 double x =
las.scans[i]*
cos(
angle*3.14159265359/180.0);
236 positions.push_back(rerun::Position2D(x,y));
238 m_rec->log(
"laserscan",
239 rerun::Points2D(positions));
251 std::vector<rerun::Position3D> positions;
252 for (
size_t x=0; x<pcl.
width(); x++)
254 for (
size_t y=0; y<pcl.
height(); y++)
256 positions.push_back(rerun::Position3D(pcl(x,y).x,
261 m_rec->log(
"pointcloud",
262 rerun::Points3D(positions));
277 for (
size_t i=0; i<
sz; i++)
279 std::string name =
"EncoderArray_" + std::to_string(i);
285 for (
size_t i=0; i<
sz; i++)
287 std::string name =
"LinearVelocitySensors_" + std::to_string(i);
294 for (
size_t i=0; i<
sz; i++)
296 std::string name =
"OrientationSensors_" + std::to_string(i);
303 for (
size_t i=0; i<
sz; i++)
305 std::string name =
"PositionSensors_" + std::to_string(i);
312 for (
size_t i=0; i<
sz; i++)
314 std::string name =
"SixAxisForceTorqueSensors_" + std::to_string(i);
324 for (
size_t i=0; i<
sz; i++)
326 std::string name =
"TemperatureSensors_" + std::to_string(i);
332 for (
size_t i=0; i<
sz; i++)
334 std::string name =
"ThreeAxisGyroscopes_" + std::to_string(i);
341 for (
size_t i=0; i<
sz; i++)
343 std::string name =
"ThreeAxisMagnetometers" + std::to_string(i);
356 if (data==
nullptr)
return;
357 for (
size_t i=0; i< data->
size(); i++)
370 std::string
vsname =
"value";
395 for (
size_t i=0; i< data.
size(); i++)
397 std::string
vsname =
"value_" + std::to_string(i);
407 yarp::dev::impl::jointData data;
411 size_t n = data.controlMode.size();
412 for (
size_t i=0; i<
n; i++)
414 std::string name =
"joint_" + std::to_string(i);
415 m_rec->log(name+
"_Position", rerun::Scalars(data.jointPosition[i]));
416 m_rec->log(name+
"_Velocity", rerun::Scalars(data.jointVelocity[i]));
417 m_rec->log(name+
"_Acceleration", rerun::Scalars(data.jointAcceleration[i]));
418 m_rec->log(name+
"_torque", rerun::Scalars(data.torque[i]));
419 m_rec->log(name+
"_current", rerun::Scalars(data.current[i]));
420 m_rec->log(name+
"_pwmDutycycle", rerun::Scalars(data.pwmDutycycle[i]));
433 m_rec->log(
"camera", rerun::DepthImage(
442int main(
int argc,
char *argv[])
bool process_RGBImage(const Bottle *pbot)
yarp::os::BufferedPort< yarp::os::Bottle > m_port
bool process_RobotState(const Bottle *pbot)
bool process_Bottle(const Bottle *pbot)
bool process_Vector(const Bottle *pbot)
bool process_PointCloud(const Bottle *pbot)
void process_Bottle_nested(const Bottle *pbot, std::vector< size_t > path)
bool updateModule() override
Override this to do whatever your module needs to do.
bool process_DepthImage(const Bottle *pbot)
double getPeriod() override
You can override this to control the approximate periodicity at which updateModule() is called by run...
bool process_SensorStreamingData(const Bottle *pbot)
bool configure(yarp::os::ResourceFinder &rf) override
Configure the module, pass a ResourceFinder object to the module.
rerun::RecordingStream * m_rec
bool interruptModule() override
Try to halt any ongoing operations by threads managed by the module.
bool process_Lidar2D(const Bottle *pbot)
std::vector< SensorMeasurementData > measurements
SensorMeasurementsData ThreeAxisGyroscopes
SensorMeasurementsData OrientationSensors
SensorMeasurementsData PositionSensors
SensorMeasurementsData LinearVelocitySensors
SensorMeasurementsData ThreeAxisMagnetometers
SensorMeasurementsData SixAxisForceTorqueSensors
SensorMeasurementsData TemperatureSensors
SensorMeasurementsData EncoderArrays
A simple collection of objects that can be described and transmitted in a portable way.
size_type size() const
Gets the number of elements in the bottle.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
A mini-server for performing network communication in the background.
bool getEnvelope(PortReader &envelope) override
Get the envelope information (e.g., a timestamp) from the last message received on the port.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void interrupt() override
Interrupt any current reads or writes attached to the port.
T * read(bool shouldWait=true) override
Read an available object from the port.
static bool connect(const std::string &src, const std::string &dest, const std::string &carrier="", bool quiet=true)
Request that an output port connect to an input port.
Utilities for manipulating the YARP network, including initialization and shutdown.
static bool copyPortable(const PortWriter &writer, PortReader &reader)
Copy one portable to another, via writing and reading.
A base-class for standard YARP modules that supports ResourceFinder.
virtual int runModule()
Calls updateModule() until that returns false.
Helper class for finding config files and other external resources.
bool check(const std::string &key) const override
Check if there exists a property of the given name.
bool configure(int argc, char *argv[], bool skipFirstArgument=true)
Sets up the ResourceFinder.
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
An abstraction for a time stamp and/or sequence number.
double getTime() const
Get the time stamp.
virtual bool isString() const
Checks if value is a string.
virtual bool isList() const
Checks if value is a list.
virtual Bottle * asList() const
Get list value.
virtual yarp::conf::float32_t asFloat32() const
Get 32-bit floating point value.
virtual std::string asString() const
Get string value.
size_t width() const
Gets width of image in pixels.
unsigned char * getRawImage() const
Access to the internal image buffer.
size_t height() const
Gets height of image in pixels.
virtual size_t width() const
virtual size_t height() const
size_t size() const
Returns the number of elements in the vector.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
An interface to the operating system, including Port based communication.
The main, catch-all namespace for YARP.
int main(int argc, char *argv[])