cob_sick_s300.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.
 */
 

//##################
//#### includes ####

// standard includes
//--

// ROS includes
#include <ros/ros.h>
#include <XmlRpcException.h>

// ROS message includes
#include <std_msgs/Bool.h>
#include <sensor_msgs/LaserScan.h>
#include <diagnostic_msgs/DiagnosticArray.h>

// ROS service includes
//--

// external includes
#include <cob_sick_s300/ScannerSickS300.h>

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread.hpp>
#include <boost/lexical_cast.hpp>

#define ROS_LOG_FOUND

//####################
//#### node class ####
class NodeClass
{
        //
        public:

                ros::NodeHandle nh;
                // topics to publish
                ros::Publisher topicPub_LaserScan;
                ros::Publisher topicPub_InStandby;
                ros::Publisher topicPub_Diagnostic_;

                // topics to subscribe, callback is called for new messages arriving
                //--

                // service servers
                //--

                // service clients
                //--

                // global variables
                std::string port;
                std::string node_name;
                int baud, scan_id;
                bool inverted;
                double scan_duration, scan_cycle_time;
                std::string frame_id;
                ros::Time syncedROSTime;
                unsigned int syncedSICKStamp;
                bool syncedTimeReady;
                bool debug_;
                double communication_timeout;
                ScannerSickS300 scanner_;
                std_msgs::Bool inStandby_;

                // Constructor
                NodeClass()
                {
                        // create a handle for this node, initialize node
                        //nh = ros::NodeHandle("~");

                        if(!nh.hasParam("port")) ROS_WARN("Used default parameter for port");
                        nh.param("port", port, std::string("/dev/ttyUSB0"));

                        if(!nh.hasParam("baud")) ROS_WARN("Used default parameter for baud");
                        nh.param("baud", baud, 500000);

                        if(!nh.hasParam("scan_id")) ROS_WARN("Used default parameter for scan_id");
                        nh.param("scan_id", scan_id, 7);

                        if(!nh.hasParam("inverted")) ROS_WARN("Used default parameter for inverted");
                        nh.param("inverted", inverted, false);

                        if(!nh.hasParam("frame_id")) ROS_WARN("Used default parameter for frame_id");
                        nh.param("frame_id", frame_id, std::string("base_laser_link"));

                        if(!nh.hasParam("scan_duration")) ROS_WARN("Used default parameter for scan_duration");
                        nh.param("scan_duration", scan_duration, 0.025); //no info about that in SICK-docu, but 0.025 is believable and looks good in rviz

                        if(!nh.hasParam("scan_cycle_time")) ROS_WARN("Used default parameter for scan_cycle_time");
                        nh.param("scan_cycle_time", scan_cycle_time, 0.040); //SICK-docu says S300 scans every 40ms

                        if(nh.hasParam("debug")) nh.param("debug", debug_, false);

                        if(!nh.hasParam("communication_timeout")) ROS_WARN("Used default parameter for communication timeout");
                        nh.param("communication_timeout", communication_timeout, 0.2);

                        try
                        {
                                //get params for each measurement
                                XmlRpc::XmlRpcValue field_params;
                                if(nh.getParam("fields",field_params) && field_params.getType() == XmlRpc::XmlRpcValue::TypeStruct)
                                {
                                        for(XmlRpc::XmlRpcValue::iterator field=field_params.begin(); field!=field_params.
                                        end(); field++)
                                        {
                                                int field_number = boost::lexical_cast<int>(field->first);
                                                ROS_DEBUG("Found field %d in params", field_number);

                                                if(!field->second.hasMember("scale"))
                                                {
                                                        ROS_ERROR("Missing parameter scale");
                                                        continue;
                                                }

                                                if(!field->second.hasMember("start_angle"))
                                                {
                                                        ROS_ERROR("Missing parameter start_angle");
                                                        continue;
                                                }

                                                if(!field->second.hasMember("stop_angle"))
                                                {
                                                        ROS_ERROR("Missing parameter stop_angle");
                                                        continue;
                                                }

                                                ScannerSickS300::ParamType param;
                                                param.dScale = field->second["scale"];
                                                param.dStartAngle = field->second["start_angle"];
                                                param.dStopAngle = field->second["stop_angle"];
                                                scanner_.setRangeField(field_number, param);

                                                ROS_DEBUG("params %f %f %f", param.dScale, param.dStartAngle, param.dStopAngle);
                                        }
                                }
                                else
                                {
                                        //ROS_WARN("No params for the Sick S300 fieldset were specified --> will using default, but it's deprecated now, please adjust parameters!!!");

                                        //setting defaults to be backwards compatible
                                        ScannerSickS300::ParamType param;
                                        param.dScale = 0.01;
                                        param.dStartAngle = -135.0/180.0*M_PI;
                                        param.dStopAngle = 135.0/180.0*M_PI;
                                        scanner_.setRangeField(1, param);
                                }
                        } catch(XmlRpc::XmlRpcException e)
                        {
                                ROS_ERROR_STREAM("Not all params for the Sick S300 fieldset could be read: " << e.getMessage() << "! Error code: " << e.getCode());
                                ROS_ERROR("Node is going to shut down.");
                                exit(-1);
                        }

                        syncedSICKStamp = 0;
                        syncedROSTime = ros::Time::now();
                        syncedTimeReady = false;

                        node_name = ros::this_node::getName();

                        // implementation of topics to publish
                        topicPub_LaserScan = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
                        topicPub_InStandby = nh.advertise<std_msgs::Bool>("scan_standby", 1);
                        topicPub_Diagnostic_ = nh.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
                }

                bool open() {
                        return scanner_.open(port.c_str(), baud, scan_id);
                }

                bool receiveScan() {
                        std::vector< double > ranges, rangeAngles, intensities;
                        unsigned int iSickTimeStamp, iSickNow;

                        int result = scanner_.getScan(ranges, rangeAngles, intensities, iSickTimeStamp, iSickNow, debug_);
                        static boost::posix_time::ptime point_time_communiaction_ok = boost::posix_time::microsec_clock::local_time();

                        if(result)
                        {
                                if(scanner_.isInStandby())
                                {
                                        publishWarn("scanner in standby");
                                        ROS_WARN_THROTTLE(30, "scanner %s on port %s in standby", node_name.c_str(), port.c_str());
                                        publishStandby(true);
                                }
                                else
                                {
                                        publishStandby(false);
                                        publishLaserScan(ranges, rangeAngles, intensities, iSickTimeStamp, iSickNow);
                                }

                                point_time_communiaction_ok = boost::posix_time::microsec_clock::local_time();
                        }
                        else
                        {
                                boost::posix_time::time_duration diff = boost::posix_time::microsec_clock::local_time() - point_time_communiaction_ok;

                                if (diff.total_milliseconds() > static_cast<int>(1000*communication_timeout))
                                {
                                        ROS_WARN("Communiaction timeout");
                                        return false;
                                }
                        }

                        return true;
                }

                // Destructor
                ~NodeClass()
                {
                }

                void publishStandby(bool inStandby)
                {
                        this->inStandby_.data = inStandby;
                        topicPub_InStandby.publish(this->inStandby_);
                }

                // other function declarations
                void publishLaserScan(std::vector<double> vdDistM, std::vector<double> vdAngRAD, std::vector<double> vdIntensAU, unsigned int iSickTimeStamp, unsigned int iSickNow)
                {
                        // fill message
                        int start_scan, stop_scan;
                        int num_readings = vdDistM.size(); // initialize with max scan size
                        start_scan = 0;
                        stop_scan = vdDistM.size();

                        // Sync handling: find out exact scan time by using the syncTime-syncStamp pair:
                        // Timestamp: "This counter is internally incremented at each scan, i.e. every 40 ms (S300)"
                        if(iSickNow != 0) {
                                syncedROSTime = ros::Time::now() - ros::Duration(scan_cycle_time);
                                syncedSICKStamp = iSickNow;
                                syncedTimeReady = true;

                                ROS_DEBUG("Got iSickNow, store sync-stamp: %d", syncedSICKStamp);
                        } else syncedTimeReady = false;

                        // create LaserScan message
                        sensor_msgs::LaserScan laserScan;
                        if(syncedTimeReady) {
                                double timeDiff = (int)(iSickTimeStamp - syncedSICKStamp) * scan_cycle_time;
                                laserScan.header.stamp = syncedROSTime + ros::Duration(timeDiff);

                                ROS_DEBUG("Time::now() - calculated sick time stamp = %f",(ros::Time::now() - laserScan.header.stamp).toSec());
                        } else {
                                laserScan.header.stamp = ros::Time::now();
                        }

                        // fill message
                        laserScan.header.frame_id = frame_id;
                        laserScan.angle_increment = vdAngRAD[start_scan + 1] - vdAngRAD[start_scan];
                        laserScan.range_min = 0.001;
                        laserScan.range_max = 29.5; // though the specs state otherwise, the max range reported by the scanner is 29.96m
                        laserScan.time_increment = (scan_duration) / (vdDistM.size());

                        // rescale scan
                        num_readings = vdDistM.size();
                        laserScan.angle_min = vdAngRAD[start_scan]; // first ScanAngle
                        laserScan.angle_max = vdAngRAD[stop_scan - 1]; // last ScanAngle
                        laserScan.ranges.resize(num_readings);
                        laserScan.intensities.resize(num_readings);


                        // check for inverted laser
                        if(inverted) {
                                // to be really accurate, we now invert time_increment
                                // laserScan.header.stamp = laserScan.header.stamp + ros::Duration(scan_duration); //adding of the sum over all negative increments would be mathematically correct, but looks worse.
                                laserScan.time_increment = - laserScan.time_increment;
                        } else {
                                laserScan.header.stamp = laserScan.header.stamp - ros::Duration(scan_duration); //to be consistent with the omission of the addition above
                        }

                        for(int i = 0; i < (stop_scan - start_scan); i++)
                        {
                                if(inverted)
                                {
                                        laserScan.ranges[i] = vdDistM[stop_scan-1-i];
                                        laserScan.intensities[i] = vdIntensAU[stop_scan-1-i];
                                }
                                else
                                {
                                        laserScan.ranges[i] = vdDistM[start_scan + i];
                                        laserScan.intensities[i] = vdIntensAU[start_scan + i];
                                }
                        }

                        // publish Laserscan-message
                        topicPub_LaserScan.publish(laserScan);

                        //Diagnostics
                        diagnostic_msgs::DiagnosticArray diagnostics;
                        diagnostics.header.stamp = ros::Time::now();
                        diagnostics.status.resize(1);
                        diagnostics.status[0].level = 0;
                        diagnostics.status[0].name = nh.getNamespace();
                        diagnostics.status[0].message = "sick scanner running";
                        topicPub_Diagnostic_.publish(diagnostics);
                        }

                                void publishError(std::string error_str) {
                                        diagnostic_msgs::DiagnosticArray diagnostics;
                                        diagnostics.header.stamp = ros::Time::now();
                                        diagnostics.status.resize(1);
                                        diagnostics.status[0].level = 2;
                                        diagnostics.status[0].name = nh.getNamespace();
                                        diagnostics.status[0].message = error_str;
                                        topicPub_Diagnostic_.publish(diagnostics);
                                }

                                void publishWarn(std::string warn_str) {
                                        diagnostic_msgs::DiagnosticArray diagnostics;
                                        diagnostics.header.stamp = ros::Time::now();
                                        diagnostics.status.resize(1);
                                        diagnostics.status[0].level = 1;
                                        diagnostics.status[0].name = nh.getNamespace();
                                        diagnostics.status[0].message = warn_str;
                                        topicPub_Diagnostic_.publish(diagnostics);
                                }
};

//#######################
//#### main programm ####
int main(int argc, char** argv)
{
        // initialize ROS, spezify name of node
        ros::init(argc, argv, "sick_s300");

        NodeClass nodeClass;

        while (ros::ok())
        {
                bool bOpenScan = false;
                while (!bOpenScan && ros::ok()) {
                        ROS_INFO("Opening scanner... (port:%s)", nodeClass.port.c_str());

                        bOpenScan = nodeClass.open();

                        // check, if it is the first try to open scanner
                        if (!bOpenScan) {
                                ROS_ERROR("...scanner not available on port %s. Will retry every second.", nodeClass.port.c_str());
                                nodeClass.publishError("...scanner not available on port");
                        }
                        sleep(1); // wait for scan to get ready if successfull, or wait befor retrying
                }
                ROS_INFO("...scanner opened successfully on port %s", nodeClass.port.c_str());

                // main loop
                while (ros::ok()) {
                        // read scan
                        if (!nodeClass.receiveScan())
                        {
                                break;
                        }
                        ros::spinOnce();
                }
        }
        return 0;
}