virtual void onInit()
{
NODELET_WARN_STREAM("Initializing nodelet..." << getName());
lp_.reset(new bta_tof_driver::BtaRos(getNodeHandle(), getPrivateNodeHandle(), getName()));
stream_thread_.reset(new boost::thread(boost::bind(&BtaRos::initialize, lp_.get())));
};
bool ServoInterface::setServo(const std::string& channel, const double target)
{
if(target > 1.0 || target < -1.0)
{
return false;
NODELET_WARN_STREAM("Servo value " << target <<
" for channel " << channel <<
" out of [-1,1] range");
}
std::map<std::string, ServoSettings>::const_iterator mapIt;
double pos = target;
if( (mapIt = m_servoSettings.find(channel)) != m_servoSettings.end())
{
if(mapIt->second.reverse)
{
pos = -pos;
}
if(pos > 0.0)
{
pos = mapIt->second.center+pos*(mapIt->second.max-mapIt->second.center);
} else if(pos < 0.0)
{
pos = mapIt->second.center+pos*(mapIt->second.center-mapIt->second.min);
} else
{
pos = mapIt->second.center;
}
unsigned int val = static_cast<unsigned int>(pos*4);
m_maestro.setTargetMS(mapIt->second.port, val);
return true;
}
// m_maestro.diag_error("Servo does not exist:"+channel);
return false;
}
void VoxelGridDownsampleManager::pointCB(const sensor_msgs::PointCloud2ConstPtr &input)
{
try {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
if (grid_.size() == 0) {
ROS_INFO("the number of registered grids is 0, skipping");
return;
}
fromROSMsg(*input, *cloud);
for (size_t i = 0; i < grid_.size(); i++)
{
visualization_msgs::Marker::ConstPtr target_grid = grid_[i];
// not yet tf is supported
int id = target_grid->id;
// solve tf with ros::Time 0.0
// transform pointcloud to the frame_id of target_grid
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl_ros::transformPointCloud(target_grid->header.frame_id,
*cloud,
*transformed_cloud,
tf_listener);
double center_x = target_grid->pose.position.x;
double center_y = target_grid->pose.position.y;
double center_z = target_grid->pose.position.z;
double range_x = target_grid->scale.x * 1.0; // for debug
double range_y = target_grid->scale.y * 1.0;
double range_z = target_grid->scale.z * 1.0;
double min_x = center_x - range_x / 2.0;
double max_x = center_x + range_x / 2.0;
double min_y = center_y - range_y / 2.0;
double max_y = center_y + range_y / 2.0;
double min_z = center_z - range_z / 2.0;
double max_z = center_z + range_z / 2.0;
double resolution = target_grid->color.r;
// filter order: x -> y -> z -> downsample
pcl::PassThrough<pcl::PointXYZRGB> pass_x;
pass_x.setFilterFieldName("x");
pass_x.setFilterLimits(min_x, max_x);
pcl::PassThrough<pcl::PointXYZRGB> pass_y;
pass_y.setFilterFieldName("y");
pass_y.setFilterLimits(min_y, max_y);
pcl::PassThrough<pcl::PointXYZRGB> pass_z;
pass_z.setFilterFieldName("z");
pass_z.setFilterLimits(min_z, max_z);
ROS_INFO_STREAM(id << " filter x: " << min_x << " - " << max_x);
ROS_INFO_STREAM(id << " filter y: " << min_y << " - " << max_y);
ROS_INFO_STREAM(id << " filter z: " << min_z << " - " << max_z);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_after_x (new pcl::PointCloud<pcl::PointXYZRGB>);
pass_x.setInputCloud (transformed_cloud);
pass_x.filter(*cloud_after_x);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_after_y (new pcl::PointCloud<pcl::PointXYZRGB>);
pass_y.setInputCloud (cloud_after_x);
pass_y.filter(*cloud_after_y);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_after_z (new pcl::PointCloud<pcl::PointXYZRGB>);
pass_z.setInputCloud (cloud_after_y);
pass_z.filter(*cloud_after_z);
// downsample
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_after_z);
sor.setLeafSize (resolution, resolution, resolution);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
sor.filter (*cloud_filtered);
// reverse transform
pcl::PointCloud<pcl::PointXYZRGB>::Ptr reverse_transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl_ros::transformPointCloud(input->header.frame_id,
*cloud_filtered,
*reverse_transformed_cloud,
tf_listener);
// adding the output into *output_cloud
// tmp <- cloud_filtered + output_cloud
// output_cloud <- tmp
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp (new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::concatenatePointCloud (*cloud_filtered, *output_cloud, tmp);
//output_cloud = tmp;
ROS_INFO_STREAM(id << " includes " << reverse_transformed_cloud->points.size() << " points");
for (size_t i = 0; i < reverse_transformed_cloud->points.size(); i++) {
output_cloud->points.push_back(reverse_transformed_cloud->points[i]);
}
}
sensor_msgs::PointCloud2 out;
toROSMsg(*output_cloud, out);
out.header = input->header;
pub_.publish(out); // for debugging
// for concatenater
size_t cluster_num = output_cloud->points.size() / max_points_ + 1;
ROS_INFO_STREAM("encoding into " << cluster_num << " clusters");
for (size_t i = 0; i < cluster_num; i++) {
size_t start_index = max_points_ * i;
size_t end_index = max_points_ * (i + 1) > output_cloud->points.size() ?
output_cloud->points.size(): max_points_ * (i + 1);
sensor_msgs::PointCloud2 cluster_out_ros;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr
cluster_out_pcl(new pcl::PointCloud<pcl::PointXYZRGB>);
cluster_out_pcl->points.resize(end_index - start_index);
// build cluster_out_pcl
ROS_INFO_STREAM("make cluster from " << start_index << " to " << end_index);
for (size_t j = start_index; j < end_index; j++) {
cluster_out_pcl->points[j - start_index] = output_cloud->points[j];
}
// conevrt cluster_out_pcl into ros msg
toROSMsg(*cluster_out_pcl, cluster_out_ros);
jsk_pcl_ros::SlicedPointCloud publish_point_cloud;
cluster_out_ros.header = input->header;
publish_point_cloud.point_cloud = cluster_out_ros;
publish_point_cloud.slice_index = i;
publish_point_cloud.sequence_id = sequence_id_;
pub_encoded_.publish(publish_point_cloud);
ros::Duration(1.0 / rate_).sleep();
}
}
catch (std::runtime_error e) { // catch any error
NODELET_WARN_STREAM("error has occured in VoxelGridDownsampleManager but ignore it: " << e.what());
ros::Duration(1.0 / rate_).sleep();
}
}
