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

[PCDLoader] add offset service and disable to output when number of loaded point cloud is zero #1281

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

[PCDLoader] add offset service and disable to output when number of loaded point cloud is zero #1281

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
2 changes: 2 additions & 0 deletions idl/PCDLoaderService.idl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,5 +24,7 @@ module OpenHRP
@return true if loaded successfully, false otherwise
*/
boolean load(in string filename, in string label);

void offset(in string label, in double cx, in double cy, in double cz, in double ox, in double oy, in double oz, in double r, in double p, in double y);
};
};
68 changes: 56 additions & 12 deletions rtc/PCDLoader/PCDLoader.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,7 @@ RTC::ReturnCode_t PCDLoader::onInitialize()
addPort(m_PCDLoaderServicePort);
m_service0.setComp(this);
m_isOutput.data = true;
m_isSetOffset = false;

// Set service consumers to Ports

Expand All @@ -88,7 +89,7 @@ RTC::ReturnCode_t PCDLoader::onInitialize()
return RTC::RTC_OK;
}

void PCDLoader::setCloudXYZ(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_raw)
bool PCDLoader::setCloudXYZ(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_raw)
{
int npoint = cloud_raw->points.size();

Expand Down Expand Up @@ -121,9 +122,11 @@ void PCDLoader::setCloudXYZ(PointCloudTypes::PointCloud& cloud, const pcl::Point
ptr[2] = cloud_raw->points[i].z;
ptr += 4;
}

return (npoint > 0);
}

void PCDLoader::setCloudXYZRGB(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_raw)
bool PCDLoader::setCloudXYZRGB(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_raw)
{
int npoint = cloud_raw->points.size();

Expand Down Expand Up @@ -172,9 +175,11 @@ void PCDLoader::setCloudXYZRGB(PointCloudTypes::PointCloud& cloud, const pcl::Po
rgb[2] = cloud_raw->points[i].b;
ptr += 4;
}

return (npoint > 0);
}

void PCDLoader::updateOffsetToCloudXYZ(void)
bool PCDLoader::updateOffsetToCloudXYZ(void)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr clouds(new pcl::PointCloud<pcl::PointXYZ>);
for (unsigned int i=0; i<m_offset.length(); i++){
Expand Down Expand Up @@ -205,10 +210,10 @@ void PCDLoader::updateOffsetToCloudXYZ(void)
*clouds += *cloud_new;
}
}
setCloudXYZ(m_cloud, clouds);
return setCloudXYZ(m_cloud, clouds);
}

void PCDLoader::updateOffsetToCloudXYZRGB(void)
bool PCDLoader::updateOffsetToCloudXYZRGB(void)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr clouds(new pcl::PointCloud<pcl::PointXYZRGB>);
for (unsigned int i=0; i<m_offset.length(); i++){
Expand Down Expand Up @@ -239,7 +244,7 @@ void PCDLoader::updateOffsetToCloudXYZRGB(void)
*clouds += *cloud_new;
}
}
setCloudXYZRGB(m_cloud, clouds);
return setCloudXYZRGB(m_cloud, clouds);
}

/*
Expand Down Expand Up @@ -310,14 +315,32 @@ RTC::ReturnCode_t PCDLoader::onExecute(RTC::UniqueId ec_id)
if( m_offsetIn.isNew() ){
m_offsetIn.read();
if( !m_clouds_xyz.empty() ){
updateOffsetToCloudXYZ();
m_cloudOut.write();
m_isOutputOut.write();
if( updateOffsetToCloudXYZ() ){
m_cloudOut.write();
m_isOutputOut.write();
}
}
else if( !m_clouds_xyzrgb.empty() ){
if( updateOffsetToCloudXYZRGB() ){
m_cloudOut.write();
m_isOutputOut.write();
}
}
}

if( m_isSetOffset ){
m_isSetOffset = false;
if( !m_clouds_xyz.empty() ){
if( updateOffsetToCloudXYZ() ){
m_cloudOut.write();
m_isOutputOut.write();
}
}
else if( !m_clouds_xyzrgb.empty() ){
updateOffsetToCloudXYZRGB();
m_cloudOut.write();
m_isOutputOut.write();
if( updateOffsetToCloudXYZRGB() ){
m_cloudOut.write();
m_isOutputOut.write();
}
}
}

Expand Down Expand Up @@ -389,6 +412,27 @@ bool PCDLoader::load(const std::string& filename, const std::string& label)
}
}

void PCDLoader::offset(const std::string& label, const hrp::Vector3& center,
const hrp::Vector3& offsetP, const hrp::Matrix33& offsetR)
{
m_offset.length(1);
OpenHRP::PCDOffset& offset = m_offset[0];
offset.center.x = center(0);
offset.center.y = center(1);
offset.center.z = center(2);

offset.data.position.x = offsetP(0);
offset.data.position.y = offsetP(1);
offset.data.position.z = offsetP(2);

hrp::Vector3 rpy(hrp::rpyFromRot(offsetR));
offset.data.orientation.r = rpy(0);
offset.data.orientation.p = rpy(1);
offset.data.orientation.y = rpy(2);

m_isSetOffset = true;
}

extern "C"
{

Expand Down
15 changes: 9 additions & 6 deletions rtc/PCDLoader/PCDLoader.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -100,8 +100,10 @@ class PCDLoader
// The action that is invoked when execution context's rate is changed
// no corresponding operation exists in OpenRTm-aist-0.2.0
// virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);

bool load(const std::string& filename, const std::string& label);

void offset(const std::string& label, const hrp::Vector3& center, const hrp::Vector3& offsetP, const hrp::Matrix33& offsetR);

protected:
// Configuration variable declaration
Expand All @@ -112,7 +114,7 @@ class PCDLoader
PointCloudTypes::PointCloud m_cloud;
OpenHRP::PCDOffsetSeq m_offset;
RTC::TimedBoolean m_isOutput;

// DataInPort declaration
// <rtc-template block="inport_declare">

Expand Down Expand Up @@ -144,17 +146,18 @@ class PCDLoader

// </rtc-template>

void setCloudXYZ(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_raw);
bool setCloudXYZ(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_raw);

void setCloudXYZRGB(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_raw);
bool setCloudXYZRGB(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_raw);

void updateOffsetToCloudXYZ(void);
bool updateOffsetToCloudXYZ(void);

void updateOffsetToCloudXYZRGB(void);
bool updateOffsetToCloudXYZRGB(void);

private:
std::string m_path, m_fields;
int dummy;
bool m_isSetOffset;
boost::unordered_map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr> m_clouds_xyz;
boost::unordered_map<std::string, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> m_clouds_xyzrgb;
};
Expand Down
7 changes: 7 additions & 0 deletions rtc/PCDLoader/PCDLoaderService_impl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,3 +19,10 @@ ::CORBA::Boolean PCDLoaderService_impl::load(const char* filename, const char* l
{
return m_comp->load(filename, label);
}

void PCDLoaderService_impl::offset(const char* label, CORBA::Double cx, CORBA::Double cy, CORBA::Double cz,
CORBA::Double ox, CORBA::Double oy, CORBA::Double oz,
CORBA::Double r, CORBA::Double p, CORBA::Double y)
{
return m_comp->offset(label, hrp::Vector3(cx, cy, cz), hrp::Vector3(ox, oy, oz), hrp::rotFromRpy(r, p, y));
}
6 changes: 5 additions & 1 deletion rtc/PCDLoader/PCDLoaderService_impl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,12 @@ class PCDLoaderService_impl
void setComp(PCDLoader *i_comp);
//
::CORBA::Boolean load(const char* filename, const char* label);

void offset(const char* label, CORBA::Double cx, CORBA::Double cy, CORBA::Double cz,
CORBA::Double ox, CORBA::Double oy, CORBA::Double oz,
CORBA::Double r, CORBA::Double p, CORBA::Double y);
private:
PCDLoader *m_comp;
};

#endif