ros_driver.cpp

Go to the documentation of this file.

 
#include <functional>
 
#include <multisense_ros/laser.h>
#include <multisense_ros/camera.h>
#include <multisense_ros/pps.h>
#include <multisense_ros/imu.h>
#include <multisense_ros/status.h>
#include <multisense_ros/statistics.h>
#include <multisense_ros/reconfigure.h>
#include <ros/ros.h>
 
using namespace crl::multisense;
 
int main(int argc, char** argvPP)
{
    ros::init(argc, argvPP, "multisense_driver");
    ros::NodeHandle nh;
    ros::NodeHandle nh_private_("~");
 
    //
    // Get parameters from ROS/command-line
 
    std::string   sensor_ip;
    std::string   tf_prefix;
    int           sensor_mtu;
    int           head_id;
    int           timeout_s;
 
 
    nh_private_.param<std::string>("sensor_ip", sensor_ip, "10.66.171.21");
    nh_private_.param<std::string>("tf_prefix", tf_prefix, "multisense");
    nh_private_.param<int>("sensor_mtu", sensor_mtu, 1500);
    nh_private_.param<int>("head_id", head_id, -1);
    nh_private_.param<int>("camera_timeout_s", timeout_s, -1);
 
    Channel *d = NULL;
 
    try {
 
        RemoteHeadChannel rh_channel = 0;
        switch(head_id) {
            case 0:
                rh_channel = Remote_Head_0;
                break;
            case 1:
                rh_channel = Remote_Head_1;
                break;
            case 2:
                rh_channel = Remote_Head_2;
                break;
            case 3:
                rh_channel = Remote_Head_3;
                break;
            default:
                rh_channel = Remote_Head_VPB;
                break;
        }
 
        d = Channel::Create(sensor_ip, rh_channel);
        if (NULL == d) {
            ROS_ERROR("multisense_ros: failed to create communication channel to sensor @ \"%s\" with head ID %d",
                      sensor_ip.c_str(), rh_channel);
            return -2;
        }
 
        Status status = d->setMtu(sensor_mtu);
        if (Status_Ok != status) {
            ROS_ERROR("multisense_ros: failed to set sensor MTU to %d: %s",
                      sensor_mtu, Channel::statusString(status));
            Channel::Destroy(d);
            return -3;
        }
 
        //
        // Anonymous namespace so objects can deconstruct before channel is destroyed
        {
            multisense_ros::Laser        laser(d, tf_prefix);
            multisense_ros::Camera       camera(d, tf_prefix);
            multisense_ros::Pps          pps(d);
            multisense_ros::Imu          imu(d, tf_prefix);
            multisense_ros::Status       status(d);
            multisense_ros::Statistics   statistics(d);
            multisense_ros::Reconfigure  rec(d,
                                             std::bind(&multisense_ros::Camera::updateConfig, &camera, std::placeholders::_1),
                                             std::bind(&multisense_ros::Camera::borderClipChanged, &camera,
                                                       std::placeholders::_1, std::placeholders::_2),
                                             std::bind(&multisense_ros::Camera::maxPointCloudRangeChanged, &camera,
                                                       std::placeholders::_1),
                                             std::bind(&multisense_ros::Camera::extrinsicsChanged, &camera,
                                                       std::placeholders::_1),
                                             std::bind(&multisense_ros::Camera::groundSurfaceSplineDrawParametersChanged, &camera,
                                                       std::placeholders::_1));
 
            ros::Rate rate(50);
            ros::Time last_status{ros::Time::now()};
            const ros::Duration timeout{static_cast<double>(timeout_s)};
 
            ros::Time last_warning{};
            ros::Duration warn_delay{1.1};
            while (ros::ok()) {
                ros::spinOnce();
 
                system::StatusMessage statusMessage;
                const auto status_result = d->getDeviceStatus(statusMessage);
                if (Status_Ok != status_result) {
 
                    if (ros::Time::now() - last_warning > warn_delay) {
                        ROS_WARN("multisense_ros: missed camera status message");
                        last_warning = ros::Time::now();
                    }
 
                    if (timeout > ros::Duration{0} && ros::Time::now() - last_status > timeout) {
                        ROS_ERROR("multisense_ros: shutting down due to connection timeout");
                        Channel::Destroy(d);
                        return -5;
                    }
 
                } else {
                    last_warning = ros::Time::now();
                    last_status = ros::Time::now();
                }
 
                rate.sleep();
            }
        }
 
        Channel::Destroy(d);
        return 0;
 
    } catch (const std::exception& e) {
        ROS_ERROR("multisense_ros: caught exception: %s", e.what());
        Channel::Destroy(d);
        return -4;
    }
}