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;


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

    Channel *d = NULL;

    try {

        d = Channel::Create(sensor_ip);
        if (NULL == d) {
            ROS_ERROR("multisense_ros: failed to create communication channel to sensor @ \"%s\"",
                      sensor_ip.c_str());
            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));
            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;
    }
}