void GraphManager::sendAllCloudsImpl()
{
ScopedTimer s(__FUNCTION__);
if (batch_cloud_pub_.getNumSubscribers() == 0){
ROS_WARN("No Subscribers: Sending of clouds cancelled");
return;
}
ROS_INFO("Sending out all clouds");
batch_processing_runs_ = true;
ros::Rate r(ParameterServer::instance()->get<double>("send_clouds_rate")); //slow down a bit, to allow for transmitting to and processing in other nodes
for (graph_it it = graph_.begin(); it != graph_.end(); ++it)
{
Node* node = it->second;
if(!node->valid_tf_estimate_) {
ROS_INFO("Skipping node %i: No valid estimate", node->id_);
continue;
}
tf::StampedTransform base_to_fixed = this->computeFixedToBaseTransform(node, true);
br_.sendTransform(base_to_fixed);
publishCloud(node, base_to_fixed.stamp_, batch_cloud_pub_);
QString message;
Q_EMIT setGUIInfo(message.sprintf("Sending pointcloud and map transform (%i/%i) on topics %s and /tf", it->first, (int)graph_.size(), ParameterServer::instance()->get<std::string>("individual_cloud_out_topic").c_str()) );
r.sleep();
}
batch_processing_runs_ = false;
Q_EMIT sendFinished();
}
void SegmentStitching::tmpPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr cl, const std::vector<Line>& lines){
ros::Rate loopRate(10);
while (ros::ok()){
publishCloud(cl);
visualization_msgs::Marker m = makeLineMarkers(lines, "unspecifiedMarker");
linemarker_pub.publish(m);
loopRate.sleep();
}
}
void BorderEstimator::computeBorder(
const pcl::RangeImage& range_image,
const std_msgs::Header& header)
{
pcl::RangeImageBorderExtractor border_extractor (&range_image);
pcl::PointCloud<pcl::BorderDescription> border_descriptions;
border_extractor.compute (border_descriptions);
pcl::PointIndices border_indices, veil_indices, shadow_indices;
for (int y = 0; y < (int)range_image.height; ++y) {
for (int x = 0; x < (int)range_image.width; ++x) {
if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__OBSTACLE_BORDER]) {
border_indices.indices.push_back (y*range_image.width + x);
}
if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__VEIL_POINT]) {
veil_indices.indices.push_back (y*range_image.width + x);
}
if (border_descriptions.points[y*range_image.width + x].traits[pcl::BORDER_TRAIT__SHADOW_BORDER]) {
shadow_indices.indices.push_back (y*range_image.width + x);
}
}
}
publishCloud(pub_border_, border_indices, header);
publishCloud(pub_veil_, veil_indices, header);
publishCloud(pub_shadow_, shadow_indices, header);
cv::Mat image;
jsk_recognition_utils::rangeImageToCvMat(range_image, image);
pub_range_image_.publish(
cv_bridge::CvImage(header,
sensor_msgs::image_encodings::BGR8,
image).toImageMsg());
// publish pointcloud
sensor_msgs::PointCloud2 ros_cloud;
pcl::toROSMsg(range_image, ros_cloud);
ros_cloud.header = header;
pub_cloud_.publish(ros_cloud);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle nh;
ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_sharp", 2, laserCloudSharpHandler);
ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler);
ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_flat", 2, laserCloudFlatHandler);
ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>
("/velodyne_cloud_2", 2, laserCloudFullResHandler);
ros::Subscriber subImuTrans = nh.subscribe<sensor_msgs::PointCloud2>
("/imu_trans", 5, imuTransHandler);
ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_corner_last", 2);
ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_surf_last", 2);
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
("/velodyne_cloud_3", 2);
ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);
nav_msgs::Odometry laserOdometry;
laserOdometry.header.frame_id = "/camera_init";
laserOdometry.child_frame_id = "/laser_odom";
tf::TransformBroadcaster tfBroadcaster;
tf::StampedTransform laserOdometryTrans;
laserOdometryTrans.frame_id_ = "/camera_init";
laserOdometryTrans.child_frame_id_ = "/laser_odom";
int frameCount = skipFrameNum;
ros::Rate rate(100);
bool status = ros::ok();
bool systemInited = false;
while (status) {
ros::spinOnce();
if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat &&
newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans &&
fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005) {
ROS_DEBUG_STREAM("[laserOdometry] started with frame from " << timeLaserCloudFullRes);
ecl::StopWatch stopWatch;
newCornerPointsSharp = false;
newCornerPointsLessSharp = false;
newSurfPointsFlat = false;
newSurfPointsLessFlat = false;
newLaserCloudFullRes = false;
newImuTrans = false;
if (!systemInited) {
publishCloud(*inputs.cornerPointsLessSharp, pubLaserCloudCornerLast, ros::Time().fromSec(timeSurfPointsLessFlat), "/camera");
publishCloud(*inputs.surfPointsLessFlat, pubLaserCloudSurfLast, ros::Time().fromSec(timeSurfPointsLessFlat), "/camera");
}
LaserOdometry::Outputs outputs;
laserOdom.run(inputs, outputs);
if(systemInited) {
geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(outputs.t[2], -outputs.t[0], -outputs.t[1]);
laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserOdometry.pose.pose.orientation.x = -geoQuat.y;
laserOdometry.pose.pose.orientation.y = -geoQuat.z;
laserOdometry.pose.pose.orientation.z = geoQuat.x;
laserOdometry.pose.pose.orientation.w = geoQuat.w;
laserOdometry.pose.pose.position.x = outputs.t[3];
laserOdometry.pose.pose.position.y = outputs.t[4];
laserOdometry.pose.pose.position.z = outputs.t[5];
pubLaserOdometry.publish(laserOdometry);
laserOdometryTrans.stamp_ = ros::Time().fromSec(timeSurfPointsLessFlat);
laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
laserOdometryTrans.setOrigin(tf::Vector3(outputs.t[3], outputs.t[4], outputs.t[5]));
tfBroadcaster.sendTransform(laserOdometryTrans);
frameCount++;
if (frameCount > skipFrameNum) {
frameCount = 0;
publishCloud(*outputs.corners, pubLaserCloudCornerLast, ros::Time().fromSec(timeSurfPointsLessFlat), "/camera");
publishCloud(*outputs.surfels, pubLaserCloudSurfLast, ros::Time().fromSec(timeSurfPointsLessFlat), "/camera");
publishCloud(*outputs.cloud, pubLaserCloudFullRes, ros::Time().fromSec(timeSurfPointsLessFlat), "/camera");
}
}
systemInited = true;
ROS_DEBUG_STREAM("[laserOdometry] took " << stopWatch.elapsed());
}
status = ros::ok();
rate.sleep();
}
return 0;
}
