node.cpp

Go to the documentation of this file.

#include <LeicaStateMachine.h>
#include "leica_scanstation_utils/LeicaUtils.h"

typedef pcl::PointCloud<pcl::PointXYZ> PointCloudXYZ;
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloudRGB;

const float FREQ = 0.1;   // Hz

int main(int argc, char** argv)
{
    ros::init(argc, argv, "statemachine");
    ros::NodeHandle nh;
    ros::Rate r(FREQ);

    PointCloudRGB::Ptr source_cloud(new PointCloudRGB);
    PointCloudRGB::Ptr target_cloud(new PointCloudRGB);

    sensor_msgs::PointCloud2ConstPtr cloud_msg; 
    cloud_msg = ros::topic::waitForMessage<sensor_msgs::PointCloud2>(SOURCE_CLOUD_TOPIC);
    pcl::fromROSMsg(*cloud_msg, *source_cloud);
    ROS_INFO("Received cloud from topic: %s", SOURCE_CLOUD_TOPIC.c_str());
    
    cloud_msg = ros::topic::waitForMessage<sensor_msgs::PointCloud2>(TARGET_CLOUD_TOPIC);
    pcl::fromROSMsg(*cloud_msg, *target_cloud);
    ROS_INFO("Received cloud from topic: %s", TARGET_CLOUD_TOPIC.c_str());

    if (Utils::isValidCloud(source_cloud) && Utils::isValidCloud(target_cloud))
    {
        std::shared_ptr<StateMachine> FSM = std::make_shared<StateMachine>(source_cloud, target_cloud, nh);
        FSM->initiate();
        FSM->process_event(StartEvent());
        
        while(ros::ok())
        {
            FSM->process_event(PublishEvent());
            ros::spinOnce();
            r.sleep();
        }
    }

    ROS_ERROR("NO VALID MSGS");

    ros::spin();

    return 0;
}