|
From: <rdi...@us...> - 2008-12-27 05:27:00
|
Revision: 8610
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8610&view=rev
Author: rdiankov
Date: 2008-12-27 05:26:57 +0000 (Sat, 27 Dec 2008)
Log Message:
-----------
connected tf to object tracking in openrave
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h
pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
pkg/trunk/vision/checkerboard_detector/msg/Object6DPose.msg
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-27 04:15:32 UTC (rev 8609)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-27 05:26:57 UTC (rev 8610)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 588
+SVN_REVISION = -r 589
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2008-12-27 04:15:32 UTC (rev 8609)
+++ pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2008-12-27 05:26:57 UTC (rev 8610)
@@ -8,5 +8,5 @@
<node pkg="phase_space" type="phase_space_node"/>
<!-- launch octave scripts to start the openrave client -->
- <node pkg="ormanipulation" type="runperception.m" output="screen"/>
+<!-- <node pkg="ormanipulation" type="runperception.m" output="screen"/> -->
</launch>
Modified: pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml 2008-12-27 04:15:32 UTC (rev 8609)
+++ pkg/trunk/openrave_planning/ormanipulation/startchecker.ros.xml 2008-12-27 05:26:57 UTC (rev 8610)
@@ -1,22 +1,12 @@
<launch>
- <machine name="cameras" address="ahc" default="false" user="rdiankov"/>
+ <machine name="localhost_cameras" address="localhost" default="false"/>
- <node machine="cameras" name="dcam" pkg="dcam" type="dcam" respawn="false">
- <!-- video_mode should be one of the following:
- 640x480_videre_rectified: Provides rectified images from the hw
- Provides: left mono image
- 640x480_videre_disparity: Disparity and rectification done on chip.
- Provides: Disparity and left mono image
- 640x480_videre_disparity_raw: Disparity done on chip (debayer and rectification in software).
- Provides: disparity and left color image.
- 640x480_videre_none: No stereo on chip (all processing done in software).
- Provides: all 3 images available
- -->
+<!-- <node machine="cameras" name="dcam" pkg="dcam" type="dcam" respawn="false">
<param name="video_mode" type="str" value="640x480_videre_rectified"/>
<param name="do_rectify" type="bool" value="False"/>
<param name="do_stereo" type="bool" value="False"/>
<param name="do_calc_points" type="bool" value="False"/>
- </node>
+ </node> -->
<group ns="checkerdetector" clear_params="true">
<param name="display" type="int" value="1"/>
<param name="rect0_size_x" type="double" value="0.02133"/>
@@ -24,10 +14,10 @@
<param name="grid0_size_x" type="int" value="4"/>
<param name="grid0_size_y" type="int" value="3"/>
<param name="type0" type="string" value="data/ricebox.kinbody.xml"/>
- <node machine="cameras" pkg="checkerboard_detector" type="checkerboard_detector" respawn="true" output="screen">
+ <node pkg="checkerboard_detector" type="checkerboard_detector" respawn="true" output="screen">
<remap from="CamInfo" to="/dcam/left/cam_info"/>
<remap from="Image" to="/dcam/left/image_rect"/>
- <env name="DISPLAY" value=":0.0"/>
+<!-- <env name="DISPLAY" value=":0.0"/> -->
</node>
</group>
</launch>
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2008-12-27 04:15:32 UTC (rev 8609)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2008-12-27 05:26:57 UTC (rev 8610)
@@ -12,4 +12,5 @@
<depend package="boost"/>
<depend package="checkerboard_detector"/>
<depend package="robot_msgs"/>
+ <depend package="tf"/>
</package>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2008-12-27 04:15:32 UTC (rev 8609)
+++ pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2008-12-27 05:26:57 UTC (rev 8610)
@@ -26,8 +26,10 @@
#ifndef OBJECTTRANSFORM_SENSOR_SYSTEM
#define OBJECTTRANSFORM_SENSOR_SYSTEM
+#include <tf/transform_listener.h>
+#include <checkerboard_detector/ObjectDetection.h>
+
#include "rossensorsystem.h"
-#include "checkerboard_detector/ObjectDetection.h"
// used to update objects through a mocap system
class ObjectTransformXMLID
@@ -40,22 +42,84 @@
{
public:
ObjectTransformSystem(EnvironmentBase* penv)
- : ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>(penv), nNextId(1)
+ : ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>(penv), _probot(NULL), nNextId(1)
{
}
virtual bool Init(istream& sinput)
{
+ _probot = NULL;
_topic = "ObjectDetection";
- bool bSuccess = ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>::Init(sinput);
- if( !bSuccess )
- return false;
_fThreshSqr = 0.05*0.05f;
- sinput >> _fThreshSqr;
- return true;
+
+ string cmd;
+ while(!sinput.eof()) {
+ sinput >> cmd;
+ if( !sinput )
+ break;
+
+ if( stricmp(cmd.c_str(), "topic") == 0 )
+ sinput >> _topic;
+ else if( stricmp(cmd.c_str(), "thresh") == 0 )
+ sinput >> _fThreshSqr;
+ else if( stricmp(cmd.c_str(), "robot") == 0 ) {
+ int id;
+ sinput >> id;
+ KinBody* pbody = GetEnv()->GetBodyFromNetworkId(id);
+ if( pbody->IsRobot() )
+ _probot = (RobotBase*)pbody;
+
+ if( _probot == NULL )
+ RAVELOG_WARNA("failed to find robot with id %d\n", id);
+ }
+ else break;
+
+ if( !sinput ) {
+ RAVELOG_ERRORA("failed\n");
+ return false;
+ }
+ }
+
+ startsubscriptions();
+ return _bSubscribed;
}
private:
+ virtual void startsubscriptions()
+ {
+ ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>::startsubscriptions();
+
+ if( _bSubscribed ) {
+ ros::node* pnode = check_roscpp_nocreate();
+ if( pnode != NULL ) {
+ double tf_cache_time_secs;
+ pnode->param("~tf_cache_time_secs", tf_cache_time_secs, 10.0);
+ if (tf_cache_time_secs < 0)
+ RAVELOG_ERRORA("ROSSensorSystem: Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs);
+ unsigned long long tf_cache_time = tf_cache_time_secs*1000000000ULL;
+ _tf.reset(new tf::TransformListener(*pnode, true, tf_cache_time));
+ RAVELOG_INFOA("ROSSensorSystem: TF Cache Time: %f Seconds", tf_cache_time_secs);
+
+ // **** Set TF Extrapolation Limit ****
+ double tf_extrap_limit_secs ;
+ pnode->param("~tf_extrap_limit", tf_extrap_limit_secs, 0.00);
+ if (tf_extrap_limit_secs < 0.0)
+ RAVELOG_ERRORA("ROSSensorSystem: parameter tf_extrap_limit<0 (%f)", tf_extrap_limit_secs);
+
+ ros::Duration extrap_limit;
+ extrap_limit.fromSec(tf_extrap_limit_secs);
+ _tf->setExtrapolationLimit(extrap_limit);
+ RAVELOG_INFOA("ROSSensorSystem: tf extrapolation Limit: %f Seconds", tf_extrap_limit_secs);
+ }
+ }
+ }
+
+ virtual void stopsubscriptions()
+ {
+ ROSSensorSystem<checkerboard_detector::ObjectDetection, ObjectTransformXMLID>::stopsubscriptions();
+ _tf.reset();
+ }
+
void newdatacb()
{
list< SNAPSHOT > listbodies;
@@ -64,13 +128,39 @@
{
boost::mutex::scoped_lock lock(_mutex);
TYPEOF(_mapbodies) mapbodies = _mapbodies;
+ std_msgs::PoseStamped posestamped, poseout;
+ Transform trobot;
+ if( _probot != NULL && _topicmsg.objects.size() > 0 ) {
+ GetEnv()->LockPhysics(true);
+ trobot = _probot->GetTransform();
+ GetEnv()->LockPhysics(false);
+ }
+
FOREACHC(itobj, _topicmsg.objects) {
boost::shared_ptr<BODY> b;
- Transform tnew = GetTransform(itobj->pose);
+
+ Transform tnew;
+ // if on robot, have to find the global transformation
+ if( _probot != NULL ) {
+ posestamped.pose = itobj->pose;
+ posestamped.header = _topicmsg.header;
+
+ try {
+ _tf->transformPose(_stdwcstombs(_probot->GetLinks().front()->GetName()), posestamped, poseout);
+ tnew = trobot * _probot->GetTransform() * GetTransform(poseout.pose);
+ }
+ catch(tf::TransformException& ex) {
+ RAVELOG_WARNA("failed to get tf frames %S for object %s\n",posestamped.header.frame_id.c_str(), _probot->GetLinks().front()->GetName(), b->_initdata->sid.c_str());
+ tnew = GetTransform(itobj->pose);
+ }
+ }
+ else
+ tnew = GetTransform(itobj->pose);
+
FOREACH(it, mapbodies) {
- if( it->second->_initdata.sid == itobj->type ) {
+ if( it->second->_initdata->sid == itobj->type ) {
// same type matched, need to check proximity
Transform tbody = it->second->GetOffsetLink()->GetParent()->GetTransform();
@@ -132,11 +222,19 @@
}
}
- Transform GetTransform(const std_msgs::Transform& pose)
+ Transform GetTransform(const std_msgs::Pose& pose)
{
- return Transform(Vector(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z), Vector(pose.translation.x, pose.translation.y, pose.translation.z));
+ return Transform(Vector(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z), Vector(pose.position.x, pose.position.y, pose.position.z));
}
+ Transform GetTransform(const btTransform& bt)
+ {
+ btQuaternion q = bt.getRotation();
+ btVector3 o = bt.getOrigin();
+ return Transform(Vector(q.w(),q.x(),q.y(),q.z()),Vector(o.x(),o.y(),o.z()));
+ }
+ boost::shared_ptr<tf::TransformListener> _tf;
+ RobotBase* _probot; ///< system is attached to this robot
dReal _fThreshSqr;
int nNextId;
};
Modified: pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h 2008-12-27 04:15:32 UTC (rev 8609)
+++ pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h 2008-12-27 05:26:57 UTC (rev 8610)
@@ -66,7 +66,7 @@
Transform tnew = GetTransform(psbody.pose);
FOREACH(it, _mapbodies) {
- if( it->second->_initdata.id == psbody.id ) {
+ if( it->second->_initdata->id == psbody.id ) {
b = it->second;
break;
}
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2008-12-27 04:15:32 UTC (rev 8609)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rossensorsystem.h 2008-12-27 05:26:57 UTC (rev 8610)
@@ -43,10 +43,22 @@
virtual bool Init(istream& sinput)
{
- if( !SimpleSensorSystem<XMLID>::Init(sinput) )
- return false;
+ string cmd;
+ while(!sinput.eof()) {
+ sinput >> cmd;
+ if( !sinput )
+ break;
- sinput >> _topic;
+ if( stricmp(cmd.c_str(), "topic") == 0 )
+ sinput >> _topic;
+ else break;
+
+ if( !sinput ) {
+ RAVELOG_ERRORA("failed\n");
+ return false;
+ }
+ }
+
startsubscriptions();
return _bSubscribed;
}
Modified: pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h 2008-12-27 04:15:32 UTC (rev 8609)
+++ pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h 2008-12-27 05:26:57 UTC (rev 8610)
@@ -32,16 +32,20 @@
template <typename XMLID>
class SimpleSensorSystem : public SensorSystemBase
{
- public:
+public:
class MocapData : public XMLReadable
{
public:
MocapData() {}
virtual const char* GetXMLId() { return XMLID::GetXMLId(); }
-
+
+ virtual void copy(const MocapData* pdata) {
+ assert( pdata != NULL );
+ *this = *pdata;
+ }
+
string sid; ///< global id for the system id
int id;
- string tfframe; ///< if not empty, the tf frame this object is in
std::wstring strOffsetLink; ///< the link where the markers are attached (if any)
Transform transOffset,transPreOffset; // final offset = transOffset * transReturnedFromVision * transPreOffset
};
@@ -49,11 +53,10 @@
class BODY : public BODYBASE
{
public:
- BODY(KinBody* pbody, const MocapData& initdata) : BODYBASE()
+ BODY(KinBody* pbody) : BODYBASE()
{
assert( pbody != NULL );
- _initdata = initdata;
- pOffsetLink = pbody->GetLink(_initdata.strOffsetLink.c_str());
+ pOffsetLink = pbody->GetLink(_initdata->strOffsetLink.c_str());
if( pOffsetLink == NULL )
pOffsetLink = pbody->GetLinks().front();
@@ -67,21 +70,21 @@
virtual void* GetInitData(int* psize) { if( psize ) *psize = sizeof(_initdata); return &_initdata; }
ros::Time lastupdated;
- MocapData _initdata;
+ boost::shared_ptr<MocapData> _initdata;
private:
friend class SimpleSensorSystem<XMLID>;
};
- class MocapXMLReader : public BaseXMLReader
+ class SimpleXMLReader : public BaseXMLReader
{
public:
- MocapXMLReader(MocapData* pMocap, const char **atts) {
+ SimpleXMLReader(MocapData* pMocap, const char **atts) {
_pMocap = pMocap;
if( _pMocap == NULL )
_pMocap = new MocapData();
}
- virtual ~MocapXMLReader() { delete _pMocap; }
+ virtual ~SimpleXMLReader() { delete _pMocap; }
void* Release() { MocapData* temp = _pMocap; _pMocap = NULL; return temp; }
@@ -100,8 +103,6 @@
ss >> _pMocap->id;
else if( stricmp((const char*)name, "sid") == 0 )
ss >> _pMocap->sid;
- else if( stricmp((const char*)name, "tfframe") == 0 )
- ss >> _pMocap->tfframe;
else if( stricmp((const char*)name, "translation") == 0 )
ss >> _pMocap->transOffset.trans.x >> _pMocap->transOffset.trans.y >> _pMocap->transOffset.trans.z;
else if( stricmp((const char*)name, "rotationmat") == 0 ) {
@@ -134,7 +135,7 @@
RAVELOG_ERRORA("unknown field %s\n", name);
if( !ss )
- RAVELOG_ERRORA("MocapXMLReader error parsing %s\n", name);
+ RAVELOG_ERRORA("SimpleXMLReader error parsing %s\n", name);
return false;
}
@@ -162,7 +163,7 @@
static BaseXMLReader* CreateMocapReader(KinBody* parent, const char **atts)
{
- return new MocapXMLReader(NULL, atts);
+ return new SimpleXMLReader(NULL, atts);
}
SimpleSensorSystem(EnvironmentBase* penv) : SensorSystemBase(penv), _expirationtime(2,0)
@@ -221,7 +222,8 @@
return NULL;
}
- BODY* b = new BODY(pbody, *pdata);
+ BODY* b = CreateBODY(pbody);
+ b->_initdata->copy((const MocapData*)pdata);
_mapbodies[pbody->GetNetworkId()].reset(b);
RAVELOG_DEBUGA("system adding body %S, total: %d\n", pbody->GetName(), _mapbodies.size());
return b;
@@ -309,6 +311,12 @@
}
protected:
+ virtual BODY* CreateBODY(KinBody* pbody)
+ {
+ BODY* b = new BODY(pbody);
+ b->_initdata.reset(new MocapData());
+ return b;
+ }
typedef pair<boost::shared_ptr<BODY>, Transform > SNAPSHOT;
virtual void UpdateBodies(list<SNAPSHOT>& listbodies)
@@ -326,8 +334,8 @@
// transform with respect to offset link
TransformMatrix tlink = it->first->GetOffsetLink()->GetTransform();
TransformMatrix tbase = it->first->GetOffsetLink()->GetParent()->GetTransform();
- TransformMatrix toffset = tbase * tlink.inverse() * it->first->_initdata.transOffset;
- TransformMatrix tfinal = toffset * it->second*it->first->_initdata.transPreOffset;
+ TransformMatrix toffset = tbase * tlink.inverse() * it->first->_initdata->transOffset;
+ TransformMatrix tfinal = toffset * it->second*it->first->_initdata->transPreOffset;
it->first->GetOffsetLink()->GetParent()->SetTransform(tfinal);
it->first->lastupdated = curtime;
@@ -371,7 +379,7 @@
#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()
BOOST_TYPEOF_REGISTER_TEMPLATE(SimpleSensorSystem::MocapData, 1)
BOOST_TYPEOF_REGISTER_TEMPLATE(SimpleSensorSystem::BODY, 1)
-BOOST_TYPEOF_REGISTER_TEMPLATE(SimpleSensorSystem::MocapXMLReader, 1)
+BOOST_TYPEOF_REGISTER_TEMPLATE(SimpleSensorSystem::SimpleXMLReader, 1)
#endif
#endif
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2008-12-27 04:15:32 UTC (rev 8609)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2008-12-27 05:26:57 UTC (rev 8610)
@@ -166,7 +166,7 @@
stringstream ss;
s_listResourceNames.push_back(mesh->filename + "_hi.iv");
- s_listResourceNames.push_back(mesh->filename + "_low.iv");
+ //s_listResourceNames.push_back(mesh->filename + "_low.iv");
ss << mesh->filename << "_hi.iv " << mesh->scale[0];
addKeyValue(geom, "render", ss.str());
Modified: pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
===================================================================
--- pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp 2008-12-27 04:15:32 UTC (rev 8609)
+++ pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp 2008-12-27 05:26:57 UTC (rev 8610)
@@ -260,8 +260,8 @@
CHECKERBOARD& cb = vcheckers[itype];
CvSize& s = cb.griddims;
Transform tlocal;
- tlocal.trans = Vector(vobjects[i].pose.translation.x,vobjects[i].pose.translation.y,vobjects[i].pose.translation.z);
- tlocal.rot = Vector(vobjects[i].pose.rotation.w,vobjects[i].pose.rotation.x,vobjects[i].pose.rotation.y, vobjects[i].pose.rotation.z);
+ tlocal.trans = Vector(vobjects[i].pose.position.x,vobjects[i].pose.position.y,vobjects[i].pose.position.z);
+ tlocal.rot = Vector(vobjects[i].pose.orientation.w,vobjects[i].pose.orientation.x,vobjects[i].pose.orientation.y, vobjects[i].pose.orientation.z);
CvPoint X[4];
@@ -302,7 +302,7 @@
}
}
- std_msgs::Transform FindTransformation(const vector<CvPoint2D32f> &imgpts, const vector<Vector> &objpts)
+ std_msgs::Pose FindTransformation(const vector<CvPoint2D32f> &imgpts, const vector<Vector> &objpts)
{
CvMat *objpoints = cvCreateMat(3,objpts.size(),CV_32FC1);
for(size_t i=0; i<objpts.size(); ++i) {
@@ -311,11 +311,12 @@
cvSetReal2D(objpoints, 2,i, objpts[i].z);
}
- std_msgs::Transform pose;
+ std_msgs::Pose pose;
float fR3[3];
CvMat R3, T3;
+ assert(sizeof(pose.position.x) == sizeof(double));
cvInitMatHeader(&R3, 3, 1, CV_32FC1, fR3);
- cvInitMatHeader(&T3, 3, 1, CV_64FC1, &pose.translation.x);
+ cvInitMatHeader(&T3, 3, 1, CV_64FC1, &pose.position.x);
// for some reason distortion coeffs are needed
float kc[4] = {0};
@@ -330,17 +331,17 @@
double fang = sqrt(fR3[0]*fR3[0] + fR3[1]*fR3[1] + fR3[2]*fR3[2]);
if( fang < 1e-6 ) {
- pose.rotation.w = 1;
- pose.rotation.x = 0;
- pose.rotation.y = 0;
- pose.rotation.z = 0;
+ pose.orientation.w = 1;
+ pose.orientation.x = 0;
+ pose.orientation.y = 0;
+ pose.orientation.z = 0;
}
else {
double fmult = sin(fang/2)/fang;
- pose.rotation.w = cos(fang/2);
- pose.rotation.x = fR3[0]*fmult;
- pose.rotation.y = fR3[1]*fmult;
- pose.rotation.z = fR3[2]*fmult;
+ pose.orientation.w = cos(fang/2);
+ pose.orientation.x = fR3[0]*fmult;
+ pose.orientation.y = fR3[1]*fmult;
+ pose.orientation.z = fR3[2]*fmult;
}
return pose;
Modified: pkg/trunk/vision/checkerboard_detector/msg/Object6DPose.msg
===================================================================
--- pkg/trunk/vision/checkerboard_detector/msg/Object6DPose.msg 2008-12-27 04:15:32 UTC (rev 8609)
+++ pkg/trunk/vision/checkerboard_detector/msg/Object6DPose.msg 2008-12-27 05:26:57 UTC (rev 8610)
@@ -1,3 +1,3 @@
-std_msgs/Transform pose # 6D pose of object
+std_msgs/Pose pose # 6D pose of object
string type # type of object
int32 uid # unique id of the object
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|