Add fixup_date parameter.

Now driver can setup system date.

.gitignore

@@ -26,3 +26,4 @@
 *.exe
 *.out
 *.app
+*.pyc

.ycm_extra_conf.py

@@ -0,0 +1,159 @@
+# This file is NOT licensed under the GPLv3, which is the license for the rest
+# of YouCompleteMe.
+#
+# Here's the license text for this file:
+#
+# This is free and unencumbered software released into the public domain.
+#
+# Anyone is free to copy, modify, publish, use, compile, sell, or
+# distribute this software, either in source code form or as a compiled
+# binary, for any purpose, commercial or non-commercial, and by any
+# means.
+#
+# In jurisdictions that recognize copyright laws, the author or authors
+# of this software dedicate any and all copyright interest in the
+# software to the public domain. We make this dedication for the benefit
+# of the public at large and to the detriment of our heirs and
+# successors. We intend this dedication to be an overt act of
+# relinquishment in perpetuity of all present and future rights to this
+# software under copyright law.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+# IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+# OTHER DEALINGS IN THE SOFTWARE.
+#
+# For more information, please refer to <http://unlicense.org/>
+
+import os
+import ycm_core
+
+# These are the compilation flags that will be used in case there's no
+# compilation database set (by default, one is not set).
+# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR.
+flags = [
+'-Wall',
+'-Wextra',
+'-Werror',
+'-fexceptions',
+# THIS IS IMPORTANT! Without a "-std=<something>" flag, clang won't know which
+# language to use when compiling headers. So it will guess. Badly. So C++
+# headers will be compiled as C headers. You don't want that so ALWAYS specify
+# a "-std=<something>".
+# For a C project, you would set this to something like 'c99' instead of
+# 'c++11'.
+'-std=c++03',
+# ...and the same thing goes for the magic -x option which specifies the
+# language that the files to be compiled are written in. This is mostly
+# relevant for c++ headers.
+# For a C project, you would set this to 'c' instead of 'c++'.
+'-x',
+'c++',
+'-isystem', "/opt/ros/%s/include" % os.environ.get('ROS_DISTRO', 'indigo'),
+'-isystem', '/usr/include/Poco',
+'-isystem', '/usr/include',
+'-I', '.',
+'-I', './include',
+'-I', '../devel/include',
+# ROS flags
+'-DROSCONSOLE_BACKEND_LOG4CXX',
+#'-DROS_PACKAGE_NAME="mavros"',
+]
+
+# Set this to the absolute path to the folder (NOT the file!) containing the
+# compile_commands.json file to use that instead of 'flags'. See here for
+# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html
+#
+# You can get CMake to generate this file for you by adding:
+#   set( CMAKE_EXPORT_COMPILE_COMMANDS 1 )
+# to your CMakeLists.txt file.
+#
+# Most projects will NOT need to set this to anything; you can just change the
+# 'flags' list of compilation flags. Notice that YCM itself uses that approach.
+compilation_database_folder = ''
+
+if os.path.exists( compilation_database_folder ):
+  database = ycm_core.CompilationDatabase( compilation_database_folder )
+else:
+  database = None
+
+SOURCE_EXTENSIONS = [ '.cpp', '.cxx', '.cc', '.c', '.m', '.mm' ]
+
+def DirectoryOfThisScript():
+  return os.path.dirname( os.path.abspath( __file__ ) )
+
+
+def MakeRelativePathsInFlagsAbsolute( flags, working_directory ):
+  if not working_directory:
+    return list( flags )
+  new_flags = []
+  make_next_absolute = False
+  path_flags = [ '-isystem', '-I', '-iquote', '--sysroot=' ]
+  for flag in flags:
+    new_flag = flag
+
+    if make_next_absolute:
+      make_next_absolute = False
+      if not flag.startswith( '/' ):
+        new_flag = os.path.join( working_directory, flag )
+
+    for path_flag in path_flags:
+      if flag == path_flag:
+        make_next_absolute = True
+        break
+
+      if flag.startswith( path_flag ):
+        path = flag[ len( path_flag ): ]
+        new_flag = path_flag + os.path.join( working_directory, path )
+        break
+
+    if new_flag:
+      new_flags.append( new_flag )
+  return new_flags
+
+
+def IsHeaderFile( filename ):
+  extension = os.path.splitext( filename )[ 1 ]
+  return extension in [ '.h', '.hxx', '.hpp', '.hh' ]
+
+
+def GetCompilationInfoForFile( filename ):
+  # The compilation_commands.json file generated by CMake does not have entries
+  # for header files. So we do our best by asking the db for flags for a
+  # corresponding source file, if any. If one exists, the flags for that file
+  # should be good enough.
+  if IsHeaderFile( filename ):
+    basename = os.path.splitext( filename )[ 0 ]
+    for extension in SOURCE_EXTENSIONS:
+      replacement_file = basename + extension
+      if os.path.exists( replacement_file ):
+        compilation_info = database.GetCompilationInfoForFile(
+          replacement_file )
+        if compilation_info.compiler_flags_:
+          return compilation_info
+    return None
+  return database.GetCompilationInfoForFile( filename )
+
+
+def FlagsForFile( filename, **kwargs ):
+  if database:
+    # Bear in mind that compilation_info.compiler_flags_ does NOT return a
+    # python list, but a "list-like" StringVec object
+    compilation_info = GetCompilationInfoForFile( filename )
+    if not compilation_info:
+      return None
+
+    final_flags = MakeRelativePathsInFlagsAbsolute(
+      compilation_info.compiler_flags_,
+      compilation_info.compiler_working_dir_ )
+  else:
+    relative_to = DirectoryOfThisScript()
+    final_flags = MakeRelativePathsInFlagsAbsolute( flags, relative_to )
+
+  return {
+    'flags': final_flags,
+    'do_cache': True
+  }

CMakeLists.txt

@@ -9,8 +9,11 @@ find_package(catkin REQUIRED COMPONENTS
   message_runtime
   roscpp
   sensor_msgs
+  cmake_modules
 )
 
+find_package(Poco REQUIRED COMPONENTS Foundation)
+
 ###################################
 ## catkin specific configuration ##
 ###################################
@@ -22,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
   CATKIN_DEPENDS sensor_msgs
+  DEPENDS Poco
 )
 
 ###########
@@ -33,6 +37,7 @@ catkin_package(
 # include_directories(include)
 include_directories(
   ${catkin_INCLUDE_DIRS}
+  ${Poco_INCLUDE_DIRS}
 )
 
 ## Declare a cpp executable
@@ -41,6 +46,7 @@ add_executable(shm_driver src/shm_driver.cpp)
 ## Specify libraries to link a library or executable target against
 target_link_libraries(shm_driver
   ${catkin_LIBRARIES}
+  ${Poco_LIBRARIES}
 )
 
 #############

README.md

@@ -5,6 +5,7 @@ This ROS node listen `sensor_msgs/TimeReference` and send it to ntpd via SHM (li
 
 Parameter `~/shm_unit` define SHM unit (in ntp.conf) (int, default: 2).
 Parameter `~/time_ref_topic` define the topic to subscribe to (string, default: `"~/time_ref"`).
+Parameter `~/fixup_date` enable/disable date fixup (bool, default: false)
 
 
 System configuration
@@ -24,16 +25,29 @@ Run example:
 
     rosrun ntpd_driver shm_driver _shm_unit:=2 _time_ref_topic:=/mavros/time_reference
 
+
 ### chrony configuration
 
 Add this to `/etc/chrony/chrony.conf`:
 
     ### SHM driver
-    refclock SHM 0 delay 0.5 refid ROS stratum 12
+    refclock SHM 0 delay 0.5 refid ROS
 
 And then restart chrony service.
 
 Run example:
 
     rosrun ntpd_driver shm_driver _shm_unit:=0 _time_ref_topic:=/mavros/time_reference
 
+
+### Date fixup configuration (sudo)
+
+On my Raspberry Pi 2 ntpd reject SHM data if system date is not set (e.g. JAN 1970).
+To fix that `shm_driver` now can set system time if it unset.
+
+For setting date program requires root privileges, so used `sudo`.
+
+Add this to `/etc/sudoers` (using `visudo`):
+
+    %sudo	ALL=NOPASSWD: /bin/date
+

package.xml

@@ -19,6 +19,8 @@
   <depend>message_runtime</depend>
   <depend>roscpp</depend>
   <depend>sensor_msgs</depend>
+  <depend>cmake_modules</depend>
+  <depend>libpoco-dev</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
@@ -38,6 +40,7 @@
             <sub name="~/time_ref" type="sensor_msgs/TimeReference">Time source topic. (default topic name)</sub>
             <param name="~/shm_unit" type="int" default="2">SHM Unit (must be same as in ntp server config).</param>
             <param name="~/time_ref_topic" type="string" default="time_ref">Topic name (may be used instead of remap).</param>
+            <param name="~/fixup_date" type="bool" default="false">Enable date fixup.</param>
           </ros_api>
         </node>
       </nodes>

src/shm_driver.cpp

@@ -8,6 +8,8 @@
  *
  * Copyright 2014 Vladimir Ermakov.
  * Based on ntpshm.c from gpsd.
+ *
+ * @file
  */
 
 #include <ros/ros.h>
@@ -22,6 +24,14 @@
 #include <cstring>
 #include <cerrno>
 
+// there may be another library, but Poco already used by class_loader,
+// so it definetly exist in your system.
+#include <Poco/Process.h>
+#include <Poco/Pipe.h>
+#include <Poco/PipeStream.h>
+#include <Poco/Format.h>
+#include <Poco/StreamCopier.h>
+
 /** the definition of shmTime is from ntpd source ntpd/refclock_shm.c */
 struct shmTime
 {
@@ -95,6 +105,7 @@ static void put_shmTime(volatile struct shmTime **shm)
 
 /** global SHM time handle */
 volatile struct shmTime *g_shm = NULL;
+static bool g_set_date = false;
 
 static void sig_handler(int sig)
 {
@@ -131,6 +142,48 @@ static void time_ref_cb(const sensor_msgs::TimeReference::ConstPtr &time_ref)
   ROS_DEBUG_THROTTLE(10, "Got time_ref: %lu.%09lu",
       (long unsigned) time_ref->time_ref.sec,
       (long unsigned) time_ref->time_ref.nsec);
+
+  /* It is a hack for rtc-less system like Raspberry Pi
+   * We check that system time is unset (less than some magick)
+   * and set time.
+   *
+   * Sudo configuration required for that feature
+   * date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
+   */
+  if (g_set_date && ros::Time::now().sec < 1234567890ULL) {
+
+    const double stamp = time_ref->time_ref.toSec();
+
+    ROS_INFO("Setting system date to: %f", stamp);
+
+    // construct commad: sudo -n date -u -s @1234567890.000
+    Poco::Pipe outp, errp;
+    Poco::Process::Args args;
+    args.push_back("-n");
+    args.push_back("date");
+    args.push_back("-u");
+    args.push_back("-s");
+    args.push_back(Poco::format("@%f", stamp));
+    Poco::ProcessHandle ph = Poco::Process::launch("sudo", args, 0, &outp, &errp);
+
+    int rc = ph.wait();
+    Poco::PipeInputStream outs(outp), errs(errp);
+    std::string out, err;
+
+    Poco::StreamCopier::copyToString(outs, out, 4096);
+    Poco::StreamCopier::copyToString(errs, err, 4096);
+
+    if (rc == 0) {
+      ROS_INFO("The system date is set.");
+      ROS_DEBUG_STREAM("OUT: " << out);
+      ROS_DEBUG_STREAM("ERR: " << err);
+    }
+    else {
+      ROS_ERROR("Setting system date failed.");
+      ROS_ERROR_STREAM("OUT: " << out);
+      ROS_ERROR_STREAM("ERR: " << err);
+    }
+  }
 }
 
 int main(int argc, char *argv[])
@@ -148,6 +201,7 @@ int main(int argc, char *argv[])
 
   // Read Parameters
   nh.param("shm_unit", shm_unit, 2);
+  nh.param("fixup_date", g_set_date, false);
   nh.param<std::string>("time_ref_topic", time_ref_topic, "time_ref");
 
   g_shm = get_shmTime(shm_unit);