Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

T265 with fisheye camera #20

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

T265 with fisheye camera #20

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions app/devices/realsense2Tracking/realsense2Tracking.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<robot name="t265-camera" build="2" portprefix="test" xmlns:xi="http://www.w3.org/2001/XInclude">
<devices>
<xi:include href="./sensors/realsense.xml" />
<xi:include href="./wrappers/grabber_yarp.xml" />
<xi:include href="./wrappers/pose_yarp.xml" />
<!--<xi:include href="./wrappers/pose_ros.xml" />-->
</devices>
</robot>
8 changes: 8 additions & 0 deletions app/devices/realsense2Tracking/sensors/realsense.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<device xmlns:xi="http://www.w3.org/2001/XInclude" name="realsense2dev" type="realsense2Tracking">
<group name="SETTINGS">
<param name="enableFisheyeStream"> 1 </param>
</group>
</device>
14 changes: 14 additions & 0 deletions app/devices/realsense2Tracking/wrappers/grabber_yarp.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<device xmlns:xi="http://www.w3.org/2001/XInclude" name="grabberDual_YARP" type="grabberDual">
<param name="period"> 33 </param>
<param name="name"> /camera:o </param>
<param name="capabilities"> RAW </param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="subdevicergbd"> realsense2dev </elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach" />
</device>
15 changes: 15 additions & 0 deletions app/devices/realsense2Tracking/wrappers/pose_ros.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<device xmlns:xi="http://www.w3.org/2001/XInclude" name="pose_ROS" type="PoseStampedRosPublisher">
<param name="period"> 0.01 </param>
<param name="name"> /tracking </param>
<param name="topic"> /ros_tracking </param>
<param name="node_name"> /tracking_node </param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="subdevicergbd"> realsense2dev </elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach" />
</device>
13 changes: 13 additions & 0 deletions app/devices/realsense2Tracking/wrappers/pose_yarp.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<device xmlns:xi="http://www.w3.org/2001/XInclude" name="pose_YARP" type="multipleanalogsensorsserver">
<param name="period"> 10 </param>
<param name="name"> /tracking </param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="subdevicergbd"> realsense2dev </elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach" />
</device>
1 change: 1 addition & 0 deletions app/devices/realsense2Tracking/yarprobotinterface.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
config ./realsense2Tracking.xml
1 change: 1 addition & 0 deletions src/devices/realsense2/realsense2Driver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <yarp/sig/Matrix.h>
#include <yarp/os/Stamp.h>
#include <yarp/dev/IRGBDSensor.h>
#include <yarp/dev/FrameGrabberInterfaces.h >
#include <yarp/dev/RGBDSensorParamParser.h>
#include <librealsense2/rs.hpp>

Expand Down
184 changes: 96 additions & 88 deletions src/devices/realsense2/realsense2Tracking.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@

/**********************************************************************************************************/
// This software module is experimental.
// It is provided with uncomplete documentation and it may be modified/renamed/removed without any notice.
// It is provided with incomplete documentation and it may be modified/renamed/removed without any notice.
/**********************************************************************************************************/

using namespace yarp::dev;
Expand Down Expand Up @@ -147,10 +147,66 @@ static void settingErrorMsg(const string& error, bool& ret)
}
#endif

realsense2Tracking::realsense2Tracking()
realsense2Tracking::realsense2Tracking() : PeriodicThread (0.010)
{
}

bool realsense2Tracking::threadInit()
{
bool b = pipelineStartup();
if (b == false)
{
yCError(REALSENSE2TRACKING) << "Pipeline initialization failed";
return false;
}
return true;
}

void realsense2Tracking::threadRelease()
{
pipelineShutdown();
}

void realsense2Tracking::run()
{
m_yarp_timestamp = yarp::os::Time::now();

rs2::frameset dataframe;
try
{
dataframe = m_pipeline.wait_for_frames();
}
catch (const rs2::error & e)
{
yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:" << "(" << e.what() << ")";
m_lastError = e.what();
return;
}
auto fg = dataframe.first_or_default(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F);
rs2::motion_frame gyro = fg.as<rs2::motion_frame>();

auto fa = dataframe.first_or_default(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);
rs2::motion_frame accel = fa.as<rs2::motion_frame>();

auto fp = dataframe.first_or_default(RS2_STREAM_POSE);
rs2::pose_frame pose = fp.as<rs2::pose_frame>();

auto fr = dataframe.first_or_default(RS2_STREAM_FISHEYE);

std::lock_guard<std::mutex> guard(m_mutex);
m_last_gyro = gyro.get_motion_data();
m_last_accel = accel.get_motion_data();
m_last_pose = pose.get_pose_data();
m_fisheye_data = fr.get_data();
m_fisheye_size = fr.get_data_size();

if (m_timestamp_type == yarp_timestamp) { m_timestamp = m_yarp_timestamp; }
else if (m_timestamp_type == rs_timestamp) { m_timestamp = m_rs_timestamp = gyro.get_timestamp(); }
else m_timestamp = 0;

return;
}

bool realsense2Tracking::pipelineStartup()
{
try {
Expand Down Expand Up @@ -189,7 +245,7 @@ bool realsense2Tracking::pipelineRestart()
bool realsense2Tracking::open(Searchable& config)
{
yCWarning(REALSENSE2TRACKING) << "This software module is experimental.";
yCWarning(REALSENSE2TRACKING) << "It is provided with uncomplete documentation and it may be modified/renamed/removed without any notice.";
yCWarning(REALSENSE2TRACKING) << "It is provided with incomplete documentation and it may be modified/renamed/removed without any notice.";

string sensor_is = "t265";
m_timestamp_type = timestamp_enumtype::yarp_timestamp;
Expand All @@ -205,24 +261,20 @@ bool realsense2Tracking::open(Searchable& config)
}
}

bool b= true;

yCInfo(REALSENSE2TRACKING) << "Starting the pipeline";
m_cfg.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);
m_cfg.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F);
m_cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
b &= pipelineStartup();
if (b==false)
{
yCError(REALSENSE2TRACKING) << "Pipeline initialization failed";
return false;
}
m_cfg.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8);
m_cfg.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8);

this->start();
return true;
}

bool realsense2Tracking::close()
{
pipelineShutdown();
askToStop();
return true;
}

Expand Down Expand Up @@ -260,23 +312,6 @@ bool realsense2Tracking::getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::s
}

std::lock_guard<std::mutex> guard(m_mutex);
rs2::frameset dataframe;
try
{
dataframe = m_pipeline.wait_for_frames();
}
catch (const rs2::error& e)
{
yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
m_lastError = e.what();
return false;
}
auto fg = dataframe.first_or_default(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F);
rs2::motion_frame gyro = fg.as<rs2::motion_frame>();
if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
else if (m_timestamp_type == rs_timestamp) { timestamp = gyro.get_timestamp(); }
else timestamp=0;
m_last_gyro = gyro.get_motion_data();
out.resize(3);
out[0] = m_last_gyro.x;
out[1] = m_last_gyro.y;
Expand Down Expand Up @@ -317,23 +352,6 @@ bool realsense2Tracking::getThreeAxisLinearAccelerometerMeasure(size_t sens_inde
if (sens_index != 0) { return false; }

std::lock_guard<std::mutex> guard(m_mutex);
rs2::frameset dataframe;
try
{
dataframe = m_pipeline.wait_for_frames();
}
catch (const rs2::error& e)
{
yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
m_lastError = e.what();
return false;
}
auto fa = dataframe.first_or_default(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);
rs2::motion_frame accel = fa.as<rs2::motion_frame>();
m_last_accel = accel.get_motion_data();
if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
else if (m_timestamp_type == rs_timestamp) { timestamp = accel.get_timestamp(); }
else timestamp = 0;
out.resize(3);
out[0] = m_last_accel.x;
out[1] = m_last_accel.y;
Expand Down Expand Up @@ -375,23 +393,6 @@ bool realsense2Tracking::getOrientationSensorMeasureAsRollPitchYaw(size_t sens_i
if (sens_index != 0) { return false; }

std::lock_guard<std::mutex> guard(m_mutex);
rs2::frameset dataframe;
try
{
dataframe = m_pipeline.wait_for_frames();
}
catch (const rs2::error& e)
{
yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
m_lastError = e.what();
return false;
}
auto fa = dataframe.first_or_default(RS2_STREAM_POSE);
rs2::pose_frame pose = fa.as<rs2::pose_frame>();
m_last_pose = pose.get_pose_data();
if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
else if (m_timestamp_type == rs_timestamp) { timestamp = pose.get_timestamp(); }
else timestamp = 0;
yarp::math::Quaternion q(m_last_pose.rotation.x, m_last_pose.rotation.y, m_last_pose.rotation.z, m_last_pose.rotation.w);
yarp::sig::Matrix mat = q.toRotationMatrix3x3();
yarp::sig::Vector rpy_temp = yarp::math::dcm2rpy(mat);
Expand All @@ -404,6 +405,34 @@ bool realsense2Tracking::getOrientationSensorMeasureAsRollPitchYaw(size_t sens_i

//-------------------------------------------------------------------------------------------------------

bool realsense2Tracking::getImage(yarp::sig::ImageOf<yarp::sig::PixelMono>& image)
{
image.resize(848,800);
std::lock_guard<std::mutex> guard(m_mutex);
if (m_fisheye_data==nullptr) return false;
unsigned char* pi = (unsigned char*)m_fisheye_data;
unsigned char* po = image.getRawImage();
for (size_t i=0; i< m_fisheye_width * m_fisheye_height; i++)
{
*po=*pi;
po++;
pi++;
}
return true;
}

int realsense2Tracking::height() const
{
return m_fisheye_height;
}

int realsense2Tracking::width() const
{
return m_fisheye_width;
}

//-------------------------------------------------------------------------------------------------------

/* IPositionSensors methods */
size_t realsense2Tracking::getNrOfPositionSensors() const
{
Expand Down Expand Up @@ -436,40 +465,22 @@ bool realsense2Tracking::getPositionSensorMeasure(size_t sens_index, yarp::sig::
if (sens_index != 0) { return false; }

std::lock_guard<std::mutex> guard(m_mutex);
rs2::frameset dataframe;
try
{
dataframe = m_pipeline.wait_for_frames();
}
catch (const rs2::error& e)
{
yCError(REALSENSE2TRACKING) << "m_pipeline.wait_for_frames() failed with error:"<< "(" << e.what() << ")";
m_lastError = e.what();
return false;
}
auto fa = dataframe.first_or_default(RS2_STREAM_POSE);
rs2::pose_frame pose = fa.as<rs2::pose_frame>();
m_last_pose = pose.get_pose_data();
if (m_timestamp_type == yarp_timestamp) { timestamp = yarp::os::Time::now(); }
else if (m_timestamp_type == rs_timestamp) { timestamp = pose.get_timestamp(); }
else timestamp = 0;
xyz.resize(3);
xyz[0] = m_last_pose.translation.x;
xyz[1] = m_last_pose.translation.y;
xyz[2] = m_last_pose.translation.z;
return true;
}

//-------------------------------------------------------------------------------------------------------
// IAnalogSensor methods. Beware! this is just a temporary test.
// This methods will be removed soon: realsesne2Tracking should NOT derive from IAnalogSensor!
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you add a reference to the YARP release notes that deprecated IAnalogSensor so we can propagate the info to non-IIT devs? Perhaps @drdanz knows where they can be found.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Regarding AnalogServer:
as mentioned in the last SW update meeting, with the upcoming yarp release, deprecated devices (such as AnalogServer) will display a warning in the startup phase even if not marked as deprecated (please note: marking a device as deprecated means that, by default, it will not start at all). We are also discussing moving these devices to a separate folder.
Regarding IAnalogSensor:
Besides the deprecation, as already mentioned, this interface should be simply not used on this device, for the same reason it is not used on a control board device for getting encoder data, even if technically feasible.
I am going to open a new PR to show which is the correct way to achieve the result you want.


int realsense2Tracking::read(yarp::sig::Vector& out)
{
// Publishes the data in the analog port as:
// <positionX positionY positionZ QuaternionW QuaternionX QuaternionY QuaternionZ>
std::lock_guard<std::mutex> guard(m_mutex);
rs2::frameset dataframe = m_pipeline.wait_for_frames();
auto fa = dataframe.first_or_default(RS2_STREAM_POSE);
rs2::pose_frame pose = fa.as<rs2::pose_frame>();
m_last_pose = pose.get_pose_data();

out.resize(7);
out[0] = m_last_pose.translation.x;
out[1] = m_last_pose.translation.y;
Expand Down Expand Up @@ -512,9 +523,6 @@ int realsense2Tracking::calibrateChannel(int ch, double value)
return 0;
}




//-------------------------------------------------------------------------------------------------------
#if 0
/* IPoseSensors methods */
Expand Down
Loading