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/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;


    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, 7200);
    nh_private_.param<int>("head_id", head_id, -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::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::spin();
        }

        Channel::Destroy(d);
        return 0;

    } catch (const std::exception& e) {
        ROS_ERROR("multisense_ros: caught exception: %s", e.what());
        Channel::Destroy(d);
        return -4;
    }
}