Ask Your Question
0

Unable to show pointcloud in Rviz [closed]

asked 2018-03-28 12:16:09 -0500

arthur0304 gravatar image

updated 2018-04-02 01:43:57 -0500

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 ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by arthur0304
close date 2018-03-29 12:13:24.114660

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-03-29 12:12:23 -0500

arthur0304 gravatar image

Need to add true in these two publishers:

map_pub = nh.advertise<sensor_msgs::PointCloud2>("/map", 100, true);

scene_pub = nh.advertise<sensor_msgs::PointCloud2>("/scene", 100, true);

Since the default is false and it will not store data in a latch.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2018-03-28 12:16:09 -0500

Seen: 488 times

Last updated: Apr 02 '18