ros/src/evaluation/evaluation/src/loam_to_pose_node.cpp
/**
 * Lego Loam publishes its path as a pointcloud, therefore convert to pose
 * @param pcl_path PointCloud containing path data
 */
Lego Loam publishes its path as a pointcloud, therefore convert to pose
@param pcl_path PointCloud containing path data
Lego Loam publishes its path as a pointcloud therefore convert to pose @param pcl_path PointCloud containing path data
void pointCloud2Callback(const sensor_msgs::PointCloud2::ConstPtr& pcl_path)
{
    pcl::PointCloud<PointType>::Ptr pcl(new pcl::PointCloud<PointType>());
    pcl::fromROSMsg(*pcl_path, *pcl);
    auto point = pcl->points.back();
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header = pcl_path->header;
    pose_stamped.pose.orientation.w = 1;
    pose_stamped.pose.position.x = point.x;
    pose_stamped.pose.position.y = point.y;
    pose_stamped.pose.position.z = point.z;
    ROS_DEBUG_STREAM("Converted LOAM PCL to Pose");
    evaluation::SavePoseStamped save_pose;
    save_pose.id = "loam";
    save_pose.pose_stamped = pose_stamped;
    updatePosePath(pose_stamped);
    save_path_pub.publish(save_pose);
}
void pointCloud2Callback ( const sensor_msgs :: PointCloud2 :: ConstPtr & pcl_path ) { pcl :: PointCloud < PointType > :: Ptr pcl ( new pcl :: PointCloud < PointType > ( ) ) ; pcl :: fromROSMsg ( * pcl_path , * pcl ) ; auto point = pcl -> points . back ( ) ; geometry_msgs :: PoseStamped pose_stamped ; pose_stamped . header = pcl_path -> header ; pose_stamped . pose . orientation . w = 1 ; pose_stamped . pose . position . x = point . x ; pose_stamped . pose . position . y = point . y ; pose_stamped . pose . position . z = point . z ; ROS_DEBUG_STREAM ( " " ) ; evaluation :: SavePoseStamped save_pose ; save_pose . id = " " ; save_pose . pose_stamped = pose_stamped ; updatePosePath ( pose_stamped ) ; save_path_pub . publish ( save_pose ) ; }
Lego Loam publishes its path as a pointcloud, therefore convert to pose
 @param pcl_path PointCloud containing path data
Lego Loam publishes its path as a pointcloud therefore convert to pose @param pcl_path PointCloud containing path data