Skip to content

Commit 1d6f345

Browse files
committed
Add fixup_date parameter.
Now driver can setup system date.
1 parent d20a8c6 commit 1d6f345

File tree

6 files changed

+238
-1
lines changed

6 files changed

+238
-1
lines changed

.gitignore

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,3 +26,4 @@
2626
*.exe
2727
*.out
2828
*.app
29+
*.pyc

.ycm_extra_conf.py

Lines changed: 159 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,159 @@
1+
# This file is NOT licensed under the GPLv3, which is the license for the rest
2+
# of YouCompleteMe.
3+
#
4+
# Here's the license text for this file:
5+
#
6+
# This is free and unencumbered software released into the public domain.
7+
#
8+
# Anyone is free to copy, modify, publish, use, compile, sell, or
9+
# distribute this software, either in source code form or as a compiled
10+
# binary, for any purpose, commercial or non-commercial, and by any
11+
# means.
12+
#
13+
# In jurisdictions that recognize copyright laws, the author or authors
14+
# of this software dedicate any and all copyright interest in the
15+
# software to the public domain. We make this dedication for the benefit
16+
# of the public at large and to the detriment of our heirs and
17+
# successors. We intend this dedication to be an overt act of
18+
# relinquishment in perpetuity of all present and future rights to this
19+
# software under copyright law.
20+
#
21+
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
22+
# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
23+
# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
24+
# IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
25+
# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
26+
# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
27+
# OTHER DEALINGS IN THE SOFTWARE.
28+
#
29+
# For more information, please refer to <http://unlicense.org/>
30+
31+
import os
32+
import ycm_core
33+
34+
# These are the compilation flags that will be used in case there's no
35+
# compilation database set (by default, one is not set).
36+
# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR.
37+
flags = [
38+
'-Wall',
39+
'-Wextra',
40+
'-Werror',
41+
'-fexceptions',
42+
# THIS IS IMPORTANT! Without a "-std=<something>" flag, clang won't know which
43+
# language to use when compiling headers. So it will guess. Badly. So C++
44+
# headers will be compiled as C headers. You don't want that so ALWAYS specify
45+
# a "-std=<something>".
46+
# For a C project, you would set this to something like 'c99' instead of
47+
# 'c++11'.
48+
'-std=c++03',
49+
# ...and the same thing goes for the magic -x option which specifies the
50+
# language that the files to be compiled are written in. This is mostly
51+
# relevant for c++ headers.
52+
# For a C project, you would set this to 'c' instead of 'c++'.
53+
'-x',
54+
'c++',
55+
'-isystem', "/opt/ros/%s/include" % os.environ.get('ROS_DISTRO', 'indigo'),
56+
'-isystem', '/usr/include/Poco',
57+
'-isystem', '/usr/include',
58+
'-I', '.',
59+
'-I', './include',
60+
'-I', '../devel/include',
61+
# ROS flags
62+
'-DROSCONSOLE_BACKEND_LOG4CXX',
63+
#'-DROS_PACKAGE_NAME="mavros"',
64+
]
65+
66+
# Set this to the absolute path to the folder (NOT the file!) containing the
67+
# compile_commands.json file to use that instead of 'flags'. See here for
68+
# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html
69+
#
70+
# You can get CMake to generate this file for you by adding:
71+
# set( CMAKE_EXPORT_COMPILE_COMMANDS 1 )
72+
# to your CMakeLists.txt file.
73+
#
74+
# Most projects will NOT need to set this to anything; you can just change the
75+
# 'flags' list of compilation flags. Notice that YCM itself uses that approach.
76+
compilation_database_folder = ''
77+
78+
if os.path.exists( compilation_database_folder ):
79+
database = ycm_core.CompilationDatabase( compilation_database_folder )
80+
else:
81+
database = None
82+
83+
SOURCE_EXTENSIONS = [ '.cpp', '.cxx', '.cc', '.c', '.m', '.mm' ]
84+
85+
def DirectoryOfThisScript():
86+
return os.path.dirname( os.path.abspath( __file__ ) )
87+
88+
89+
def MakeRelativePathsInFlagsAbsolute( flags, working_directory ):
90+
if not working_directory:
91+
return list( flags )
92+
new_flags = []
93+
make_next_absolute = False
94+
path_flags = [ '-isystem', '-I', '-iquote', '--sysroot=' ]
95+
for flag in flags:
96+
new_flag = flag
97+
98+
if make_next_absolute:
99+
make_next_absolute = False
100+
if not flag.startswith( '/' ):
101+
new_flag = os.path.join( working_directory, flag )
102+
103+
for path_flag in path_flags:
104+
if flag == path_flag:
105+
make_next_absolute = True
106+
break
107+
108+
if flag.startswith( path_flag ):
109+
path = flag[ len( path_flag ): ]
110+
new_flag = path_flag + os.path.join( working_directory, path )
111+
break
112+
113+
if new_flag:
114+
new_flags.append( new_flag )
115+
return new_flags
116+
117+
118+
def IsHeaderFile( filename ):
119+
extension = os.path.splitext( filename )[ 1 ]
120+
return extension in [ '.h', '.hxx', '.hpp', '.hh' ]
121+
122+
123+
def GetCompilationInfoForFile( filename ):
124+
# The compilation_commands.json file generated by CMake does not have entries
125+
# for header files. So we do our best by asking the db for flags for a
126+
# corresponding source file, if any. If one exists, the flags for that file
127+
# should be good enough.
128+
if IsHeaderFile( filename ):
129+
basename = os.path.splitext( filename )[ 0 ]
130+
for extension in SOURCE_EXTENSIONS:
131+
replacement_file = basename + extension
132+
if os.path.exists( replacement_file ):
133+
compilation_info = database.GetCompilationInfoForFile(
134+
replacement_file )
135+
if compilation_info.compiler_flags_:
136+
return compilation_info
137+
return None
138+
return database.GetCompilationInfoForFile( filename )
139+
140+
141+
def FlagsForFile( filename, **kwargs ):
142+
if database:
143+
# Bear in mind that compilation_info.compiler_flags_ does NOT return a
144+
# python list, but a "list-like" StringVec object
145+
compilation_info = GetCompilationInfoForFile( filename )
146+
if not compilation_info:
147+
return None
148+
149+
final_flags = MakeRelativePathsInFlagsAbsolute(
150+
compilation_info.compiler_flags_,
151+
compilation_info.compiler_working_dir_ )
152+
else:
153+
relative_to = DirectoryOfThisScript()
154+
final_flags = MakeRelativePathsInFlagsAbsolute( flags, relative_to )
155+
156+
return {
157+
'flags': final_flags,
158+
'do_cache': True
159+
}

CMakeLists.txt

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,11 @@ find_package(catkin REQUIRED COMPONENTS
99
message_runtime
1010
roscpp
1111
sensor_msgs
12+
cmake_modules
1213
)
1314

15+
find_package(Poco REQUIRED COMPONENTS Foundation)
16+
1417
###################################
1518
## catkin specific configuration ##
1619
###################################
@@ -22,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
2225
## DEPENDS: system dependencies of this project that dependent projects also need
2326
catkin_package(
2427
CATKIN_DEPENDS sensor_msgs
28+
DEPENDS Poco
2529
)
2630

2731
###########
@@ -33,6 +37,7 @@ catkin_package(
3337
# include_directories(include)
3438
include_directories(
3539
${catkin_INCLUDE_DIRS}
40+
${Poco_INCLUDE_DIRS}
3641
)
3742

3843
## Declare a cpp executable
@@ -41,6 +46,7 @@ add_executable(shm_driver src/shm_driver.cpp)
4146
## Specify libraries to link a library or executable target against
4247
target_link_libraries(shm_driver
4348
${catkin_LIBRARIES}
49+
${Poco_LIBRARIES}
4450
)
4551

4652
#############

README.md

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@ This ROS node listen `sensor_msgs/TimeReference` and send it to ntpd via SHM (li
55

66
Parameter `~/shm_unit` define SHM unit (in ntp.conf) (int, default: 2).
77
Parameter `~/time_ref_topic` define the topic to subscribe to (string, default: `"~/time_ref"`).
8+
Parameter `~/fixup_date` enable/disable date fixup (bool, default: false)
89

910

1011
System configuration
@@ -24,16 +25,29 @@ Run example:
2425

2526
rosrun ntpd_driver shm_driver _shm_unit:=2 _time_ref_topic:=/mavros/time_reference
2627

28+
2729
### chrony configuration
2830

2931
Add this to `/etc/chrony/chrony.conf`:
3032

3133
### SHM driver
32-
refclock SHM 0 delay 0.5 refid ROS stratum 12
34+
refclock SHM 0 delay 0.5 refid ROS
3335

3436
And then restart chrony service.
3537

3638
Run example:
3739

3840
rosrun ntpd_driver shm_driver _shm_unit:=0 _time_ref_topic:=/mavros/time_reference
3941

42+
43+
### Date fixup configuration (sudo)
44+
45+
On my Raspberry Pi 2 ntpd reject SHM data if system date is not set (e.g. JAN 1970).
46+
To fix that `shm_driver` now can set system time if it unset.
47+
48+
For setting date program requires root privileges, so used `sudo`.
49+
50+
Add this to `/etc/sudoers` (using `visudo`):
51+
52+
%sudo ALL=NOPASSWD: /bin/date
53+

package.xml

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,8 @@
1919
<depend>message_runtime</depend>
2020
<depend>roscpp</depend>
2121
<depend>sensor_msgs</depend>
22+
<depend>cmake_modules</depend>
23+
<depend>libpoco-dev</depend>
2224

2325

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

src/shm_driver.cpp

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,8 @@
88
*
99
* Copyright 2014 Vladimir Ermakov.
1010
* Based on ntpshm.c from gpsd.
11+
*
12+
* @file
1113
*/
1214

1315
#include <ros/ros.h>
@@ -22,6 +24,14 @@
2224
#include <cstring>
2325
#include <cerrno>
2426

27+
// there may be another library, but Poco already used by class_loader,
28+
// so it definetly exist in your system.
29+
#include <Poco/Process.h>
30+
#include <Poco/Pipe.h>
31+
#include <Poco/PipeStream.h>
32+
#include <Poco/Format.h>
33+
#include <Poco/StreamCopier.h>
34+
2535
/** the definition of shmTime is from ntpd source ntpd/refclock_shm.c */
2636
struct shmTime
2737
{
@@ -95,6 +105,7 @@ static void put_shmTime(volatile struct shmTime **shm)
95105

96106
/** global SHM time handle */
97107
volatile struct shmTime *g_shm = NULL;
108+
static bool g_set_date = false;
98109

99110
static void sig_handler(int sig)
100111
{
@@ -131,6 +142,48 @@ static void time_ref_cb(const sensor_msgs::TimeReference::ConstPtr &time_ref)
131142
ROS_DEBUG_THROTTLE(10, "Got time_ref: %lu.%09lu",
132143
(long unsigned) time_ref->time_ref.sec,
133144
(long unsigned) time_ref->time_ref.nsec);
145+
146+
/* It is a hack for rtc-less system like Raspberry Pi
147+
* We check that system time is unset (less than some magick)
148+
* and set time.
149+
*
150+
* Sudo configuration required for that feature
151+
* date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
152+
*/
153+
if (g_set_date && ros::Time::now().sec < 1234567890ULL) {
154+
155+
const double stamp = time_ref->time_ref.toSec();
156+
157+
ROS_INFO("Setting system date to: %f", stamp);
158+
159+
// construct commad: sudo -n date -u -s @1234567890.000
160+
Poco::Pipe outp, errp;
161+
Poco::Process::Args args;
162+
args.push_back("-n");
163+
args.push_back("date");
164+
args.push_back("-u");
165+
args.push_back("-s");
166+
args.push_back(Poco::format("@%f", stamp));
167+
Poco::ProcessHandle ph = Poco::Process::launch("sudo", args, 0, &outp, &errp);
168+
169+
int rc = ph.wait();
170+
Poco::PipeInputStream outs(outp), errs(errp);
171+
std::string out, err;
172+
173+
Poco::StreamCopier::copyToString(outs, out, 4096);
174+
Poco::StreamCopier::copyToString(errs, err, 4096);
175+
176+
if (rc == 0) {
177+
ROS_INFO("The system date is set.");
178+
ROS_DEBUG_STREAM("OUT: " << out);
179+
ROS_DEBUG_STREAM("ERR: " << err);
180+
}
181+
else {
182+
ROS_ERROR("Setting system date failed.");
183+
ROS_ERROR_STREAM("OUT: " << out);
184+
ROS_ERROR_STREAM("ERR: " << err);
185+
}
186+
}
134187
}
135188

136189
int main(int argc, char *argv[])
@@ -148,6 +201,7 @@ int main(int argc, char *argv[])
148201

149202
// Read Parameters
150203
nh.param("shm_unit", shm_unit, 2);
204+
nh.param("fixup_date", g_set_date, false);
151205
nh.param<std::string>("time_ref_topic", time_ref_topic, "time_ref");
152206

153207
g_shm = get_shmTime(shm_unit);

0 commit comments

Comments
 (0)