sick_generic_caller.cpp

Go to the documentation of this file.

 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
 
#include "sick_scan/sick_generic_laser.h"
#include "sick_scan/dataDumper.h"
#include "sick_scan/helper/angle_compensator.h"
 
#define MAX_NAME_LEN (1024)
 
 
#include <algorithm> // for std::min
 
 
// std::string getVersionInfo();
 
int main(int argc, char** argv)
{
 
    DataDumper::instance().writeToFileNameWhenBufferIsFull("/tmp/sickscan_debug.csv");
    char nameId[] = "__name:=";
    char nameVal[MAX_NAME_LEN] = { 0 };
    char** argv_tmp; // argv_tmp[0][0] argv_tmp[0] identisch ist zu (*argv_tmp)
    int argc_tmp;
    std::string scannerName = "sick_scan";
 
    // sick_scan_xd::SickScanImu::imuParserTest();
 
    argc_tmp = argc;
    argv_tmp = argv;
 
    const int MAX_STR_LEN = 1024;
    char nameTagVal[MAX_STR_LEN] = { 0 };
    char logTagVal[MAX_STR_LEN] = { 0 };
    char internalDebugTagVal[MAX_STR_LEN] = { 0 };
    char sensorEmulVal[MAX_STR_LEN] = { 0 };
 
    if (argc == 1) // just for testing without calling by roslaunch
    {
        strcpy(nameTagVal, "__name:=sick_tim_5xx");
        strcpy(logTagVal, "__log:=/tmp/tmp.log");
        strcpy(internalDebugTagVal, "__internalDebug:=1");
        // strcpy(sensorEmulVal, "__emulSensor:=1");
        strcpy(sensorEmulVal, "__emulSensor:=0");
        argc_tmp = 5;
        argv_tmp = (char**)malloc(sizeof(char*) * argc_tmp);
 
        argv_tmp[0] = argv[0];
        argv_tmp[1] = nameTagVal;
        argv_tmp[2] = logTagVal;
        argv_tmp[3] = internalDebugTagVal;
        argv_tmp[4] = sensorEmulVal;
 
    }
    //
    std::string versionInfo = std::string("sick_generic_caller V. ") + getVersionInfo();
    setVersionInfo(versionInfo);
 
#if defined __ROS_VERSION && __ROS_VERSION == 2
    // Pass command line arguments to rclcpp.
    rclcpp::init(argc, argv);
    bool ros_signal_handler = rclcpp::signal_handlers_installed();
    ROS_INFO_STREAM("ROS2 signal handler are " << (ros_signal_handler? "" : " NOT") << "installed");
    if (rclcpp::uninstall_signal_handlers())
      ROS_INFO_STREAM("ROS2 signal handler uninstalled");
    else
      ROS_ERROR_STREAM("## ERROR: Failed to uninstall ROS2 signal handler");
    rclcpp::NodeOptions node_options;
    node_options.allow_undeclared_parameters(true);
    //node_options.automatically_declare_initial_parameters(true);
    rosNodePtr node = rclcpp::Node::make_shared("sick_scan", "", node_options);
#else
  ros::init(argc, argv, scannerName, ros::init_options::NoSigintHandler);  // scannerName holds the node-name
  // signal(SIGINT, rosSignalHandler);
  ros::NodeHandle nh("~");
  rosNodePtr node = &nh;
#endif
  signal(SIGINT, rosSignalHandler);  // SIGINT = 2, Ctrl-C or kill -2
  signal(SIGTERM, rosSignalHandler); // SIGTERM = 15, default kill level
 
  ROS_INFO_STREAM(versionInfo);
  for (int i = 0; i < argc_tmp; i++)
  {
    if (strstr(argv_tmp[i], nameId) == argv_tmp[i])
    {
      strcpy(nameVal, argv_tmp[i] + strlen(nameId));
      scannerName = nameVal;
    }
    ROS_INFO_STREAM("Program argument " << (i+1) << ": " << argv_tmp[i]);
  }
  ROS_INFO_STREAM("==========================================");
 
  int result = 0;
  try
  {
    // result = mainGenericLaser(argc_tmp, argv_tmp, scannerName, node);
    if (!startGenericLaser(argc_tmp, argv_tmp, scannerName, node, &result))
    {
      ROS_ERROR_STREAM("## ERROR in sick_generic_caller::main(): startGenericLaser() failed, could not start generic laser event loop");
    }
    else
    {
      rosSpin(node);
    }
    stopScannerAndExit();
    joinGenericLaser();
  }
  catch(const std::exception& e)
  {
    ROS_ERROR_STREAM("## ERROR in sick_generic_caller::main(): exception " << e.what());
  }
  catch(...)
  {
    ROS_ERROR_STREAM("## ERROR in sick_generic_caller::main(): unknown exception ");
  }
 
  return result;
}