Unable to show pointcloud in Rviz [closed]

edit

Hi, I have two PointCloud2 topics which are /map and /scene and I want to show them in two frames, "map" and "scene", respectively. I can successfully read their info using command rostopic echo /map. Also I am sure that both "map" and "scene" frame exist. However, I still could not see them in rviz. Can anyone provide some hints and sorry for any grammar mistakes. I will be really appreciated.

Here is my rviz screen shot: Rviz

Here is my tf tree:tf tree

Below is my code:

#include <iostream>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/registration/icp.h>
#include <Eigen/Dense>

//Convert euler angle to quaterion
float* euler_to_quaterion(float q[], float roll, float pitch, float yaw)
{
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);

    q[0] = cy * sr * cp - sy * cr * sp;
    q[1] = cy * cr * sp + sy * sr * cp;
    q[2] = sy * cr * cp - cy * sr * sp;
    q[3] = cy * cr * cp + sy * sr * sp;

    return q;
}

int main(int argc, char **argv)
{
    //Initialize ROS
    ros::init(argc, argv, "hw4_node");
    ros::NodeHandle nh;

    ros::Publisher map_pub;
    ros::Publisher scene_pub;
    tf::StampedTransform transform;
    tf::TransformBroadcaster br;

    //Create a ROS publisher for the output point cloud
    map_pub = nh.advertise<sensor_msgs::PointCloud2>("/map", 100);
    scene_pub = nh.advertise<sensor_msgs::PointCloud2>("/scene", 100);

    //Load .pcd files
    const pcl::PointCloud<pcl::PointXYZ>::Ptr map (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("/home/parallels/SDC_ws/src/0310126_hw4/target.pcd", *map);
    const pcl::PointCloud<pcl::PointXYZ>::Ptr scene (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("/home/parallels/SDC_ws/src/0310126_hw4/scene.pcd", *scene);

    //Perform ICP
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    //Set the input source and target
    icp.setInputSource(scene);
    icp.setInputTarget(map);
    //Set the max correspondence distance
    icp.setMaxCorrespondenceDistance(0.05);
    //Set the maximum number of iterations (criterion 1)
    icp.setMaximumIterations(1000);
    //Set the transformation epsilon (criterion 2)
    icp.setTransformationEpsilon(1e-8);
    //Set the euclidean distance difference epsilon (criterion 3)
    icp.setEuclideanFitnessEpsilon(1);
    //Perform the alignment
    pcl::PointCloud<pcl::PointXYZ> scene_cloud_registered;
    icp.align(scene_cloud_registered);

    //Obtain the transformation that aligned scene_cloud to scene_cloud_registered
    Eigen::Matrix4f transformation = icp.getFinalTransformation();
    std::cout << "Transformation matrix : " << std::endl << transformation;
    Eigen::Affine3f tROTA(transformation);
    float x, y, z, roll, pitch, yaw;
    pcl::getTranslationAndEulerAngles(tROTA, x, y, z, roll, pitch, yaw);

    //Convert euler angle to quaterion
    float arr[3];
    float *q = euler_to_quaterion(arr, roll, pitch, yaw);

    //Send /map and /scene transform
    br.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.0, 0.0, 0.0)), ros::Time(0), "/world", "/map"));
    br.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(q[0], q[1], q[2], q[2]), tf::Vector3(x, y, z)), ros::Time(0), "/map", "/scene"));

    //Convert pcd data type ...