various improvements

imu_monitor/CMakeLists.txt

@@ -1,5 +1,7 @@
-# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.12)
+if(POLICY CMP0048)
+  cmake_policy(SET CMP0048 NEW)
+endif()
 project(imu_monitor)
 
 find_package(catkin REQUIRED)

imu_monitor/scripts/imu_monitor.py

@@ -26,7 +26,9 @@ def __init__(self):
         self.odom_sub = rospy.Subscriber('base_odometry/odometer', Odometer, self.odom_cb)
 
         # diagnostics
-        self.pub_diag = rospy.Publisher('/diagnostics', DiagnosticArray)
+        self.pub_diag = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size= 1)
+
+        self.diag_timer = rospy.Timer(rospy.Duration(1), self.publish_diagnostics)
 
 
     def imu_cb(self, msg):
@@ -44,14 +46,16 @@ def odom_cb(self, msg):
                 self.start_time = rospy.Time.now()
                 self.start_angle = self.last_angle
                 self.dist = dist
-        
+
             # do imu test if possible
             if rospy.Time.now() > self.start_time + rospy.Duration(10.0):
                 self.drift = fabs(self.start_angle - self.last_angle)*180/(pi*10)
                 self.start_time = rospy.Time.now()
                 self.start_angle = self.last_angle
                 self.last_measured = rospy.Time.now()
-
+
+    def publish_diagnostics(self, event):
+        with self.lock:
             # publish diagnostics
             d = DiagnosticArray()
             d.header.stamp = rospy.Time.now()
@@ -77,7 +81,7 @@ def odom_cb(self, msg):
                 elif age < 3600:
                     last_measured = '%f minutes ago'%(age/60)
                 else:
-                    last_measured = '%f hours ago'%(age/3600)                    
+                    last_measured = '%f hours ago'%(age/3600)
             ds.values = [
                 KeyValue('Last measured', last_measured),
                 KeyValue('Drift (deg/sec)', str(drift)) ]

pr2_bringup/CMakeLists.txt

@@ -1,5 +1,7 @@
-# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.12)
+if(POLICY CMP0048)
+  cmake_policy(SET CMP0048 NEW)
+endif()
 project(pr2_bringup)
 
 # Load catkin and all dependencies required for this package

pr2_bringup/config/dualstereo_camera.launch

@@ -31,6 +31,7 @@
     <param name="height" type="int" value="480" />  
     <param name="imager_rate" type="double" value="25" />
 
+    <param name="trig_timestamp_topic" type="str" value="head_camera_trigger/trigger" />
     <param name="frame_id" type="str" value="wide_stereo_optical_frame" />
     <param name="first_packet_offset" type="double" value="0.0025" />
     <param name="ext_trig" type="bool" value="True" />

pr2_bringup/config/kinect2_bridge.launch

@@ -22,7 +22,7 @@
   <arg name="start_manager"     default="true" />
 
   <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager"
-        if="$(arg start_manager)" machine="pr2-head" output="screen"/>
+        if="$(arg start_manager)" machine="pr2-head" output="screen" respawn="true"/>
 
   <node pkg="nodelet" type="nodelet" name="$(arg base_name)_bridge" machine="pr2-head"
         args="load kinect2_bridge/kinect2_bridge_nodelet $(arg nodelet_manager)"

pr2_camera_synchronizer/CMakeLists.txt

@@ -1,5 +1,7 @@
-# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.12)
+if(POLICY CMP0048)
+  cmake_policy(SET CMP0048 NEW)
+endif()
 project(pr2_camera_synchronizer)
 
 # Load catkin and all dependencies required for this package

pr2_camera_synchronizer/src/pr2_camera_synchronizer/synchronizer_classes.py

@@ -604,7 +604,7 @@ def update_diagnostics(self):
     self.diagnostic_pub.publish(da)
 
   def spin(self):
-    self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray)
+    self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size= 1)
     try:
       reset_count = 0
       rospy.loginfo("Camera synchronizer is running...")

pr2_computer_monitor/CMakeLists.txt

@@ -1,8 +1,10 @@
-# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.12)
+if(POLICY CMP0048)
+  cmake_policy(SET CMP0048 NEW)
+endif()
 project(pr2_computer_monitor)
+
 # Load catkin and all dependencies required for this package
-# TODO: remove all from COMPONENTS that are not catkin packages.
 find_package(catkin REQUIRED COMPONENTS roscpp std_msgs)
 
 if(CATKIN_ENABLE_TESTING)

pr2_computer_monitor/scripts/ntp_monitor.py

@@ -111,9 +111,8 @@ def ntp_monitor(ntp_hostname, offset=500, self_offset=500, diag_hostname=None,
                 if (abs(measured_offset) > error_offset):
                     st.level = DiagnosticStatus.ERROR
                     st.message = "NTP Offset Too High"
-
             else:
-                st.level = DiagnosticStatus.ERROR
+                st.level = DiagnosticStatus.WARN
                 st.message = "Error Running ntpdate. Returned %d" % res
                 st.values = [ KeyValue("Offset (us)", "N/A"),
                               KeyValue("Offset tolerance (us)", str(off)),

pr2_controller_configuration/CMakeLists.txt

@@ -1,5 +1,7 @@
-# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.12)
+if(POLICY CMP0048)
+  cmake_policy(SET CMP0048 NEW)
+endif()
 project(pr2_controller_configuration)
 
 # Load catkin and all dependencies required for this package

pr2_ethercat/CMakeLists.txt

@@ -1,5 +1,7 @@
-# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.12)
+if(POLICY CMP0048)
+  cmake_policy(SET CMP0048 NEW)
+endif()
 project(pr2_ethercat)
 
 # Load catkin and all dependencies required for this package
@@ -10,15 +12,21 @@ find_package(catkin REQUIRED COMPONENTS
    pr2_controller_manager 
    ethercat_hardware 
    realtime_tools 
-   diagnostic_updater)
+   diagnostic_updater
+   cpp_common)
 
 find_package(Boost REQUIRED COMPONENTS system thread)
 include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
 
-catkin_package()
+catkin_package(DEPENDS CATKIN_DEPENDS cpp_common)
 
 add_definitions(-O3)
 
+# needed to circumvent LD_LIBRARY_PATH being emptied through ethercat_grant
+# in addition to not propagating ros_ethercat_loop RUNPATH to dependencies, in contrast to RPATH
+SET(GCC_NEWDTAGS_LINK_FLAGS "-Wl,--disable-new-dtags")
+SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${GCC_NEWDTAGS_LINK_FLAGS}")
+
 add_executable(pr2_ethercat src/main.cpp)
 target_link_libraries(pr2_ethercat rt ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 add_dependencies(pr2_ethercat ${catkin_EXPORTED_TARGETS})

pr2_ethercat/package.xml

@@ -20,6 +20,7 @@
   <build_depend>realtime_tools</build_depend>
   <build_depend>diagnostic_msgs</build_depend>
   <build_depend>diagnostic_updater</build_depend>
+  <build_depend>cpp_common</build_depend>
 
   <run_depend>pr2_controller_manager</run_depend>
   <run_depend>roscpp</run_depend>
@@ -28,4 +29,5 @@
   <run_depend>realtime_tools</run_depend>
   <run_depend>diagnostic_msgs</run_depend>
   <run_depend>diagnostic_updater</run_depend>
+  <run_depend>cpp_common</run_depend>
 </package>

pr2_robot/CMakeLists.txt

@@ -1,4 +1,7 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.12)
+if(POLICY CMP0048)
+  cmake_policy(SET CMP0048 NEW)
+endif()
 project(pr2_robot)
 find_package(catkin REQUIRED)
 catkin_metapackage()

pr2_run_stop_auto_restart/CMakeLists.txt

@@ -1,5 +1,7 @@
-# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.12)
+if(POLICY CMP0048)
+  cmake_policy(SET CMP0048 NEW)
+endif()
 project(pr2_run_stop_auto_restart)
 
 # Load catkin and all dependencies required for this package