lms1xx_node.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 
 
// standard includes
#include <csignal>
#include <cstdio>
 
// ROS includes
#include "ros/ros.h"
 
// ROS message includes
#include <sensor_msgs/LaserScan.h>
#include <diagnostic_msgs/DiagnosticArray.h>
 
// external includes
#include <lms1xx.h>
 
#define DEG2RAD M_PI/180.0
 
 
class SickLMS1xxNode
{
public:
 
    SickLMS1xxNode();
 
    bool initalize();
 
    void startScanner();
    void publish();
    void stopScanner();
 
    ros::NodeHandle nh;
 
private:
 
    bool initalizeLaser();
    bool initalizeMessage();
    void setScanDataConfig();
    void publishError(std::string error_str);
 
    ros::Publisher scan_pub;
    ros::Publisher diagnostic_pub;
 
    // laser data
    LMS1xx laser;
    scanCfg cfg;
    scanDataCfg dataCfg;
    scanData data;
    // published data
    sensor_msgs::LaserScan scan_msg;
    // parameters
    std::string host;
    std::string frame_id;
    bool inverted;
    double resolution;
    double frequency;
    bool set_config;
    double min_range;
    double max_range;
};
 
SickLMS1xxNode::SickLMS1xxNode()
{
    ros::NodeHandle nh;
 
    scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
    diagnostic_pub = nh.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
 
    if(!nh.hasParam("host")) ROS_WARN("Used default parameter for host");
    nh.param<std::string>("host", host, "192.168.1.2");
    if(!nh.hasParam("frame_id")) ROS_WARN("Used default parameter for frame_id");
    nh.param<std::string>("frame_id", frame_id, "base_laser_link");
    if(!nh.hasParam("inverted")) ROS_WARN("Used default parameter for inverted");
    nh.param<bool>("inverted", inverted, false);
    if(!nh.hasParam("angle_resolution")) ROS_WARN("Used default parameter for resolution");
    nh.param<double>("angle_resolution", resolution, 0.5);
    if(!nh.hasParam("scan_frequency")) ROS_WARN("Used default parameter for frequency");
    nh.param<double>("scan_frequency", frequency, 25);
    if(!nh.hasParam("set_config")) ROS_WARN("Used default parameter for set_config");
    nh.param<bool>("set_config", set_config, false);
    if(!nh.hasParam("min_range")) ROS_WARN("Used default parameter for min_range");
    nh.param<double>("min_range", min_range, 0.01);
    if(!nh.hasParam("max_range")) ROS_WARN("Used default parameter for max_range");
    nh.param<double>("max_range", max_range, 20.0);
 
    ROS_INFO("connecting to laser at : %s", host.c_str());
    ROS_INFO("using frame_id : %s", frame_id.c_str());
    ROS_INFO("inverted : %s", (inverted)?"true":"false");
    ROS_INFO("using res : %f", resolution);
    ROS_INFO("using freq : %f", frequency);
}
 
bool SickLMS1xxNode::initalize()
{
    bool ret = false;
 
    if (initalizeLaser() && initalizeMessage()) {
 
    setScanDataConfig();
    ret = true;
    }
 
    return ret;
}
 
bool SickLMS1xxNode::initalizeLaser()
{
    bool ret = false;
 
    laser.connect(host);
 
    if (laser.isConnected()) {
 
    ROS_INFO("Connected to laser.");
    ret = true;
 
    //setup laserscanner config
    laser.login();
    cfg = laser.getScanCfg();
 
    if(set_config)
    {
      ROS_DEBUG("Set angle resolution to %f deg",resolution);
      cfg.angleResolution = (int)(resolution * 10000);
      ROS_DEBUG("Set scan frequency to %f hz",frequency);
      cfg.scaningFrequency = (int)(frequency * 100);
 
      laser.setScanCfg(cfg);
      laser.saveConfig();
    }
 
    cfg = laser.getScanCfg();
 
    if(cfg.angleResolution != (int)(resolution * 10000))
      ROS_ERROR("Setting angle resolution failed: Current angle resolution is %f.", cfg.angleResolution/10000.0);
    if(cfg.scaningFrequency != (int)(frequency * 100))
      ROS_ERROR("Setting scan frequency failed: Current scan frequency is %f.", cfg.scaningFrequency/100.0);
 
    } else {
      ROS_ERROR("Connection to device failed");
      publishError("Connection to device failed");
    }
    return ret;
}
 
bool SickLMS1xxNode::initalizeMessage()
{
    bool ret = true;
 
    //init scan msg
    scan_msg.header.frame_id = frame_id;
 
    scan_msg.range_min = min_range;
    scan_msg.range_max = max_range;
 
    scan_msg.scan_time = 100.0/cfg.scaningFrequency;
 
    scan_msg.angle_increment = (double)cfg.angleResolution/10000.0 * DEG2RAD;
    scan_msg.angle_min = (double)cfg.startAngle/10000.0 * DEG2RAD - M_PI/2;
    scan_msg.angle_max = (double)cfg.stopAngle/10000.0 * DEG2RAD - M_PI/2;
 
    int num_values;
    if (cfg.angleResolution == 2500)
    {
      num_values = 1081;
    }
    else if (cfg.angleResolution == 5000)
    {
      num_values = 541;
    }
    else
    {
      ROS_ERROR("Unsupported resolution");
      publishError("Unsupported resolution");
      ret = false;
    }
 
    scan_msg.time_increment = scan_msg.scan_time/num_values;
 
    scan_msg.ranges.resize(num_values);
    scan_msg.intensities.resize(num_values);
 
    if(not inverted)
      scan_msg.time_increment *= -1.;
 
    return ret;
}
 
void SickLMS1xxNode::setScanDataConfig()
{
    //set scandata config
    dataCfg.outputChannel = 1;
    dataCfg.remission = true;
    dataCfg.resolution = 1;
    dataCfg.encoder = 0;
    dataCfg.position = false;
    dataCfg.deviceName = false;
    dataCfg.outputInterval = 1;
 
    laser.setScanDataCfg(dataCfg);
    ROS_DEBUG("setScanDataCfg");
 
    laser.startMeas();
    ROS_DEBUG("startMeas");
}
 
void SickLMS1xxNode::startScanner()
{
    status_t stat;
    do // wait for ready status
    {
      stat = laser.queryStatus();
      ros::Duration(1.0).sleep();
    }
    while (stat != ready_for_measurement);
 
    laser.startDevice(); // Log out to properly re-enable system after config
    ROS_DEBUG("startDevice");
 
    laser.scanContinous(1);
    ROS_DEBUG("scanContinous true");
}
 
void SickLMS1xxNode::publish()
{
    scan_msg.header.stamp = ros::Time::now();
    ++scan_msg.header.seq;
 
    if(laser.getData(data))
    {
    for (int i = 0; i < data.dist_len1; i++)
    {
      if(not inverted) {
      scan_msg.ranges[i] = data.dist1[data.dist_len1-1-i] * 0.001;
      scan_msg.intensities[i] = data.rssi1[data.rssi_len1-1-i];
      } else {
      scan_msg.ranges[i] = data.dist1[i] * 0.001;
      scan_msg.intensities[i] = data.rssi1[i];
      }
    }
    scan_pub.publish(scan_msg);
 
    //Diagnostics
    diagnostic_msgs::DiagnosticArray diagnostics;
    diagnostics.status.resize(1);
    diagnostics.status[0].level = 0;
    diagnostics.status[0].name = nh.getNamespace();
    diagnostics.status[0].message = "sick scanner running";
    diagnostic_pub.publish(diagnostics);
    }
}
 
void SickLMS1xxNode::stopScanner()
{
    laser.scanContinous(0);
    ROS_DEBUG("scanContinous false");
    laser.stopMeas();
    ROS_DEBUG("stopMeas");
    laser.disconnect();
    ROS_DEBUG("disconnect");
}
 
void SickLMS1xxNode::publishError(std::string error_str)
{
    diagnostic_msgs::DiagnosticArray diagnostics;
    diagnostics.status.resize(1);
    diagnostics.status[0].level = 2;
    diagnostics.status[0].name = nh.getNamespace();
    diagnostics.status[0].message = error_str;
    diagnostic_pub.publish(diagnostics);
}
 
 
//#######################
//#### main programm ####
int main(int argc, char** argv)
{
    ros::init(argc, argv, "sick_lms1xx_node");
 
    SickLMS1xxNode node;
 
    if (!node.initalize()) {
      return 1;
    }
 
    node.startScanner();
 
    while(ros::ok())
    {
      node.publish();
 
      ros::spinOnce();
    }
 
    node.stopScanner();
 
    return 0;
}