/**
* \brief cloud callback
* \param pointcloud2_msg input point cloud to be processed
*/
void NormalViz::cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc2_msg) {
static unsigned int lastsize = 0;
ROS_INFO("Received PointCloud message.");
unsigned int validfields = 0;
for (unsigned int i = 0; i < pc2_msg->fields.size(); i++) {
if (!strcmp(pc2_msg->fields[i].name.c_str(), "x"))
validfields++;
else if (!strcmp(pc2_msg->fields[i].name.c_str(), "y"))
validfields++;
else if (!strcmp(pc2_msg->fields[i].name.c_str(), "z"))
validfields++;
else if (!strcmp(pc2_msg->fields[i].name.c_str(), "normal_x"))
validfields++;
else if (!strcmp(pc2_msg->fields[i].name.c_str(), "normal_y"))
validfields++;
else if (!strcmp(pc2_msg->fields[i].name.c_str(), "normal_z"))
validfields++;
ROS_INFO("read field: %s", pc2_msg->fields[i].name.c_str());
}
//don't process if neccessary field were not found
if ( validfields != 6 ) {
ROS_INFO("PointCloud message does not contain neccessary fields!");
return;
}
//Converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
pcl::PointCloud<pcl::PointNormal> pcl_cloud;
pcl::fromROSMsg(*pc2_msg, pcl_cloud);
unsigned int size = pc2_msg->height * pc2_msg->width;
ROS_INFO("size: %i", size);
if (size >= lastsize) {
normals_marker_array_msg_.markers.resize(size);
}
for (unsigned int i = 0; i < size; ++i)
{
geometry_msgs::Point pos;
pos.x = pcl_cloud.points[i].x;
pos.y = pcl_cloud.points[i].y;
pos.z = pcl_cloud.points[i].z;
normals_marker_array_msg_.markers[i].pose.position = pos;
//axis-angle rotation
btVector3 axis(pcl_cloud.points[i].normal[0],pcl_cloud.points[i].normal[1],pcl_cloud.points[i].normal[2]);
btVector3 marker_axis(1, 0, 0);
btQuaternion qt(marker_axis.cross(axis.normalize()), marker_axis.angle(axis.normalize()));
geometry_msgs::Quaternion quat_msg;
tf::quaternionTFToMsg(qt, quat_msg);
normals_marker_array_msg_.markers[i].pose.orientation = quat_msg;
normals_marker_array_msg_.markers[i].header.frame_id = pcl_cloud.header.frame_id;
normals_marker_array_msg_.markers[i].header.stamp = pcl_cloud.header.stamp;
normals_marker_array_msg_.markers[i].id = i;
normals_marker_array_msg_.markers[i].ns = "Normals";
normals_marker_array_msg_.markers[i].color.r = 1.0f;
normals_marker_array_msg_.markers[i].color.g = 0.0f;
normals_marker_array_msg_.markers[i].color.b = 0.0f;
normals_marker_array_msg_.markers[i].color.a = 0.5f;
normals_marker_array_msg_.markers[i].lifetime = ros::Duration::Duration();
normals_marker_array_msg_.markers[i].type = visualization_msgs::Marker::ARROW;
normals_marker_array_msg_.markers[i].scale.x = 0.2;
normals_marker_array_msg_.markers[i].scale.y = 0.2;
normals_marker_array_msg_.markers[i].scale.z = 0.2;
normals_marker_array_msg_.markers[i].action = visualization_msgs::Marker::ADD;
}
if (lastsize > size) {
for (unsigned int i = size; i < lastsize; ++i) {
normals_marker_array_msg_.markers[i].action = visualization_msgs::Marker::DELETE;
}
}
lastsize = size;
normals_marker_array_publisher_.publish(normals_marker_array_msg_);
ROS_INFO("Published marker array with normals");
}
bool determine_normal_of_point_cloud_at_clicked_point() {
visualization_msgs::Marker marker;
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(pcl_input_point_cloud);
//transform selected point from robot frame (BASE_LINK) to Kinect frame (/kinect_rgb_optical_frame)
tf::Stamped<tf::Vector3> searchPointInRobotFrame;
tf::pointStampedMsgToTF(desired_pickup_point, searchPointInRobotFrame);
tf::StampedTransform transformRobotToPointCloud;
try {
tf_listener->lookupTransform(pcl_input_point_cloud->header.frame_id,
BASE_LINK, ros::Time(0), transformRobotToPointCloud);
} catch (tf::TransformException& ex) {
ROS_ERROR("%s", ex.what());
}
tf::Vector3 searchPointPointCloudFrame = transformRobotToPointCloud
* searchPointInRobotFrame;
pcl::PointXYZ searchPoint;
searchPoint.x = searchPointPointCloudFrame.getX();
searchPoint.y = searchPointPointCloudFrame.getY();
searchPoint.z = searchPointPointCloudFrame.getZ();
float radius = 0.02;
ROS_INFO(
"Search searchPointWorldFrame set to: x: %f, y: %f, z: %f ", searchPoint.x, searchPoint.y, searchPoint.z);
// Neighbors within radius search
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
ROS_DEBUG_STREAM(
"Input cloud frame id: " << pcl_input_point_cloud->header.frame_id);
if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch,
pointRadiusSquaredDistance) > 5) {
for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) {
//ROS_DEBUG_STREAM(
// " " << cloud->points[pointIdxRadiusSearch[i]].x << " " << cloud->points[pointIdxRadiusSearch[i]].y << " " << cloud->points[pointIdxRadiusSearch[i]].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl);
marker.pose.position.x =
pcl_input_point_cloud->points[pointIdxRadiusSearch[i]].x;
marker.pose.position.y =
pcl_input_point_cloud->points[pointIdxRadiusSearch[i]].y;
marker.pose.position.z =
pcl_input_point_cloud->points[pointIdxRadiusSearch[i]].z;
//show markers in kinect frame
marker.header.frame_id = pcl_input_point_cloud->header.frame_id;
marker.id = i;
marker.ns = "selection";
marker.header.stamp = ros::Time::now();
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration();
marker.type = visualization_msgs::Marker::SPHERE;
marker.scale.x = 0.003;
marker.scale.y = 0.003;
marker.scale.z = 0.003;
marker.color.r = 1;
marker.color.g = 0;
marker.color.b = 0;
marker.color.a = 1.0;
vis_marker_publisher.publish(marker);
}
ROS_INFO_STREAM(
"Number of points in radius: " << pointIdxRadiusSearch.size());
//Determine Normal from points in radius
Eigen::Vector4f plane_parameters;
float curvature;
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimator;
normalEstimator.computePointNormal(*pcl_input_point_cloud,
pointIdxRadiusSearch, plane_parameters, curvature);
normal.getVector4fMap() = plane_parameters;
ROS_INFO(
"Normal: x: %f, y: %f, z: %f ", normal.x, normal.y, normal.z);
ROS_DEBUG_STREAM("Normal: " << normal);
/*
tf::Vector3 normalTF(normal.x, normal.y, normal.z);
normalTF = transformWorldToPointCloud.inverse() * normalTF;
*/
geometry_msgs::PoseStamped normalPosePointCloudFrame;
normalPosePointCloudFrame.header.frame_id =
pcl_input_point_cloud->header.frame_id;
normalPosePointCloudFrame.pose.position.x = searchPoint.x;
normalPosePointCloudFrame.pose.position.y = searchPoint.y;
normalPosePointCloudFrame.pose.position.z = searchPoint.z;
//determine orientation of normal for marker
btVector3 axis(normal.x, normal.y, normal.z);
//tf::Vector3 axis(normal.x, normal.y, normal.z);
btVector3 marker_axis(1, 0, 0);
//tf::Vector3 marker_axis(1,0,0);
btQuaternion qt(marker_axis.cross(axis.normalize()),
marker_axis.angle(axis.normalize()));
qt.normalize();
//tf::Quaternion qt2(marker_axis.cross(axis.normalize()),
// marker_axis.angle(axis.normalize()));
/*
geometry_msgs::Quaternion quat_msg;
tf::quaternionTFToMsg(qt, quat_msg);
normalPosePointCloudFrame.pose.orientation = quat_msg;
*/
normalPosePointCloudFrame.pose.orientation.x = qt.getX();
normalPosePointCloudFrame.pose.orientation.y = qt.getY();
normalPosePointCloudFrame.pose.orientation.z = qt.getZ();
normalPosePointCloudFrame.pose.orientation.w = qt.getW();
ROS_DEBUG_STREAM(
"Pose in Point Cloud Frame: " << normalPosePointCloudFrame.pose);
//transform normal pose to Katana base
try {
tf_listener->transformPose(BASE_LINK, normalPosePointCloudFrame,
normalPoseRobotFrame);
} catch (const tf::TransformException &ex) {
ROS_ERROR("%s", ex.what());
} catch (const std::exception &ex) {
ROS_ERROR("%s", ex.what());
}
ROS_DEBUG_STREAM(
"base link frame frame id: " << normalPoseRobotFrame.header.frame_id);
marker.pose = normalPoseRobotFrame.pose;
//marker.pose = normalPose.pose;
marker.header.frame_id = BASE_LINK;
marker.id = 12345;
marker.ns = "normal";
marker.header.stamp = ros::Time::now();
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration();
marker.type = visualization_msgs::Marker::ARROW;
marker.scale.x = 0.05;
marker.scale.y = 0.005;
marker.scale.z = 0.005;
marker.color.r = 1;
marker.color.g = 0;
marker.color.b = 0;
marker.color.a = 1.0;
vis_marker_publisher.publish(marker);
ROS_DEBUG_STREAM(
"Nomal pose in base link frame: " << normalPoseRobotFrame.pose);
return true;
} else {
ROS_ERROR(
"Normal:No Points found inside search radius! Search radios too small?");
return false;
}
}
