23#include <librealsense2/rsutil.h>
24#include <librealsense2/rs.hpp>
50 return {
x * t,
y * t,
z * t };
55 return {
x - t,
y - t,
z - t };
65 void add(
float t1,
float t2,
float t3)
80 ss <<
"Device information: " << std::endl;
84 ss <<
" " << std::left << std::setw(20) <<
info_type <<
" : ";
89 ss <<
"N/A" << std::endl;
110 }
catch (
const rs2::error&
e) {
134 }
catch (
const rs2::error&
e) {
154bool realsense2Tracking::pipelineStartup()
158 }
catch (
const rs2::error&
e) {
160 <<
"(" <<
e.what() <<
")";
167bool realsense2Tracking::pipelineShutdown()
171 }
catch (
const rs2::error&
e) {
173 <<
"(" <<
e.what() <<
")";
180bool realsense2Tracking::pipelineRestart()
183 if (!pipelineShutdown())
186 return pipelineStartup();
192 yCWarning(
REALSENSE2TRACKING) <<
"It is provided with uncomplete documentation and it may be modified/renamed/removed without any notice.";
196 if (config.
check(
"timestamp"))
198 string temp = config.
find(
"timestamp").asString();
214 b &= pipelineStartup();
269 catch (
const rs2::error&
e)
276 rs2::motion_frame
gyro =
fg.as<rs2::motion_frame>();
326 catch (
const rs2::error&
e)
333 rs2::motion_frame
accel =
fa.as<rs2::motion_frame>();
382 catch (
const rs2::error&
e)
389 rs2::pose_frame
pose =
fa.as<rs2::pose_frame>();
443 catch (
const rs2::error&
e)
450 rs2::pose_frame
pose =
fa.as<rs2::pose_frame>();
504 catch (
const rs2::error&
e)
511 rs2::pose_frame
pose =
fa.as<rs2::pose_frame>();
560 catch (
const rs2::error&
e)
567 rs2::pose_frame
pose =
fa.as<rs2::pose_frame>();
584size_t realsense2Tracking::getNrOfPoseSensors()
const
595bool realsense2Tracking::getPoseSensorName(
size_t sens_index, std::string& name)
const
602bool realsense2Tracking::getPoseSensorFrameName(
size_t sens_index, std::string& frameName)
const
617 catch (
const rs2::error&
e)
624 rs2::pose_frame
pose =
fa.as<rs2::pose_frame>();
yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getLinearVelocitySensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
std::string m_orientationFrameName
timestamp_enumtype m_timestamp_type
bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
size_t getNrOfPositionSensors() const override
Get the number of position sensors exposed by this device.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool getPositionSensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
const std::string m_accel_sensor_tag
size_t getNrOfThreeAxisLinearAccelerometers() const override
Get the number of three axis linear accelerometers exposed by this device.
size_t getNrOfOrientationSensors() const override
Get the number of orientation sensors exposed by this device.
bool getThreeAxisAngularAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
std::string m_gyroFrameName
yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
const std::string m_pose_sensor_tag
bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
Get the last reading of the specified sensor.
std::string m_inertial_sensor_name_prefix
bool getLinearVelocitySensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
const std::string m_gyro_sensor_tag
bool close() override
Close the DeviceDriver.
const std::string m_orientation_sensor_tag
bool getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector &xyz, double ×tamp) const override
Get the last reading of the position sensor as x y z.
bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
size_t getNrOfThreeAxisAngularAccelerometers() const override
Get the number of three axis angular accelerometers exposed by this device.
bool getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
bool getThreeAxisAngularAccelerometerName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool getPositionSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
rs2::pipeline_profile m_profile
bool getOrientationSensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
yarp::dev::MAS_status getLinearVelocitySensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
size_t getNrOfLinearVelocitySensors() const override
Get the number of linear velocity sensors exposed by this device.
std::string m_poseFrameName
std::string m_accelFrameName
bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double ×tamp) const override
Get the last reading of the orientation sensor as roll pitch yaw.
bool getLinearVelocitySensorMeasure(size_t sens_index, yarp::sig::Vector &xyz, double ×tamp) const override
Get the last reading of the linear velocity sensor as x y z.
yarp::dev::MAS_status getThreeAxisAngularAccelerometerStatus(size_t sens_index) const override
Get the status of the specified sensor.
yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override
Get the status of the specified sensor.
bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
Get the last reading of the gyroscope.
bool getThreeAxisAngularAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
Get the last reading of the specified sensor.
yarp::dev::MAS_status getPositionSensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
size_t getNrOfThreeAxisGyroscopes() const override
Get the number of three axis gyroscopes exposed by this sensor.
yarp::sig::Matrix toRotationMatrix3x3() const
Converts a quaternion to a rotation matrix.
A mini-server for performing network communication in the background.
A base class for nested structures that can be searched.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
void resize(size_t size) override
Resize the vector.
#define yCError(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
MAS_status
Status of a given analog sensor exposed by a multiple analog sensors interface.
@ MAS_UNKNOWN
The sensor is in an unknown state.
@ MAS_OK
The sensor is working correctly.
yarp::sig::Vector dcm2rpy(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to roll-pitch-yaw angles (defined in Math....
double now()
Return the current time in seconds, relative to an arbitrary starting point.
An interface to the operating system, including Port based communication.
static bool setOption(rs2_option option, const rs2::sensor *sensor, float value)
static bool getOption(rs2_option option, const rs2::sensor *sensor, float &value)
static std::string get_device_information(const rs2::device &dev)
static void settingErrorMsg(const string &error, bool &ret)
float3 operator-(float t)
void add(float t1, float t2, float t3)
float3 operator*(float t)