ROS Karmic + Unbuntu 16.04 + catkin support added

python/gps/gps_main.py

@@ -1,7 +1,20 @@
 """ This file defines the main object that runs experiments. """
 
 import matplotlib as mpl
-mpl.use('Qt4Agg')
+qt_found=False
+try:
+    import PyQt4
+    mpl.use('Qt4Agg')
+    qt_found = True
+except ImportError:
+    qt_found = False
+if qt_found == False:
+    try:
+        import PyQt5
+        mpl.use('Qt5Agg')
+        qt_found = True
+    except ImportError:
+        qt_found = False
 
 import logging
 import imp

src/3rdparty/Boost.NumPy/CMakeLists.txt

@@ -11,7 +11,7 @@ set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/libs/numpy/cmake ${CMAKE_MODUL
 # matches the source tree.
 
 # find required python packages
-find_package(PythonLibs 2.7 REQUIRED)
+#find_package(PythonLibs 2.7 REQUIRED)
 find_package(NumPy REQUIRED)
 
 # find boost

src/gps_agent_pkg/CMakeLists.txt

@@ -1,5 +1,30 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+cmake_minimum_required(VERSION 2.8.6)
+project(gps_agent_pkg)
+
+find_package(catkin REQUIRED COMPONENTS
+    message_generation
+    std_msgs
+    sensor_msgs
+    # pr2_controller_interface
+    # pr2_mechanism_model
+    pluginlib
+    roscpp
+    roslib
+    rospy
+    # pr2_controllers_msgs
+    # control_toolbox
+    # realtime_tools
+    # orocos_kdl
+    geometry_msgs
+    tf
+    # pr2_controller_manager
+)
+
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+find_package(orocos_kdl)
+find_package(Eigen3 REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIRS})
 
 # Set the build type.  Options are:
 #  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
@@ -9,15 +34,32 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 #  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
 set(ROS_BUILD_TYPE RelWithDebInfo)
 
-rosbuild_init()
+#rosbuild_init()
 
 #set the default path for built executables to the "bin" directory
 set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 #set the default path for built libraries to the "lib" directory
 set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 
+add_message_files(FILES
+    CaffeParams.msg
+    ControllerParams.msg
+    DataRequest.msg
+    DataType.msg
+    LinGaussParams.msg
+    PositionCommand.msg
+    RelaxCommand.msg
+    SampleResult.msg
+    TfActionCommand.msg
+    TfObsData.msg
+    TfParams.msg
+    TrialCommand.msg
+
+)
+
 #uncomment if you have defined messages
-rosbuild_genmsg()
+generate_messages(DEPENDENCIES std_msgs)
+#rosbuild_genmsg()
 #uncomment if you have defined services
 #rosbuild_gensrv()
 
@@ -31,18 +73,27 @@ rosbuild_genmsg()
 include_directories($ENV{GPS_ROOT_DIR}/build/gps)
 
 #Include boost
-rosbuild_add_boost_directories()
-#find_package(Boost 1.46.0 COMPONENTS)
-#if(Boost_FOUND)
-    #include_directories(${Boost_INCLUDE_DIRS})
-#endif()
-
-#Use C++0x
-#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+#rosbuild_add_boost_directories()
+find_package(Boost 1.46.0 COMPONENTS)
+if(Boost_FOUND)
+    include_directories(${Boost_INCLUDE_DIRS})
+endif()
+
+# Enable C++11 if available
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+elseif(COMPILER_SUPPORTS_CXX0X)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+else()
+        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support, or our tests failed to detect it correctly, not enabling C++11.")
+endif()
+
+include_directories(include)
 
 set(DDP_FILES src/robotplugin.cpp
-              src/pr2plugin.cpp
               src/sample.cpp
               src/sensor.cpp
               src/neuralnetwork.cpp
@@ -57,6 +108,22 @@ set(DDP_FILES src/robotplugin.cpp
 	      src/rostopicsensor.cpp
               src/util.cpp)
 
+
+if(${roscpp_VERSION_MAJOR} EQUAL 1 AND ${roscpp_VERSION_MINOR} LESS 12)
+   # pr2_controller_interface is not available to ROS KINETIC (1.12) but is in ROS JADE (1.11)
+   # http://rosindex.github.io/p/pr2_controller_interface/#jade
+   # http://rosindex.github.io/p/pr2_controller_interface/#kinetic
+   #
+   # if pr2_controller_interface is available
+   # add pr2plugin to the list of DDP_FILES to compile
+   list(APPEND DDP_FILES 
+              src/pr2plugin.cpp
+       )
+endif()
+
+#uncomment for test executable
+option(BUILD_TEST "Build the test executables" OFF)
+
 # Include Caffe
 if (USE_CAFFE)
     # add definitions for the C++ code
@@ -76,31 +143,33 @@ if (USE_CAFFE)
     # add neural network to DDP controller files
     set(DDP_FILES ${DDP_FILES} src/neuralnetworkcaffe.cpp src/caffenncontroller.cpp)
     # compile Caffe test
-    #rosbuild_add_executable(caffe_test src/caffe_test.cpp src/neural_network_caffe.cpp)
-    #target_link_libraries(caffe_test caffe protobuf)
-
-    # compile image processor node
-    #rosbuild_add_executable(caffe_img_processor src/img_processor.cpp src/neural_network_caffe.cpp)
-    #target_link_libraries(caffe_img_processor caffe protobuf)
+    if(BUILD_TEST)
+        add_executable(caffe_test src/caffe_test.cpp src/neural_network_caffe.cpp)
+        target_link_libraries(caffe_test caffe protobuf)
+
+        # compile image processor node
+        add_executable(caffe_img_processor src/img_processor.cpp src/neural_network_caffe.cpp)
+        target_link_libraries(caffe_img_processor caffe protobuf)
+    endif()
 endif (USE_CAFFE)
 
-rosbuild_add_library(gps_agent_lib ${DDP_FILES})
-#uncomment for test executable
-#rosbuild_add_executable(main src/main.cpp)
-
-#rosbuild_add_executable(kinematic_baseline src/kinematic_baseline.cpp)
-
-#rosbuild_add_executable(controller_switcher src/controller_switcher.cpp)
-
-#rosbuild_add_executable(pointcloud_solver src/pointcloud_solver.cpp
-#                                          src/keypoint_detector.cpp)
+add_library(gps_agent_lib ${DDP_FILES})
+add_dependencies(gps_agent_lib  ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
 
-#rosbuild_add_executable(point_head src/point_head.cpp)
-#rosbuild_add_executable(torso src/torso.cpp)
+if(BUILD_TEST)
+    add_executable(main src/main.cpp)   
+    add_executable(kinematic_baseline src/kinematic_baseline.cpp)   
+    add_executable(controller_switcher src/controller_switcher.cpp) 
+    add_executable(pointcloud_solver src/pointcloud_solver.cpp
+                                     src/keypoint_detector.cpp) 
+    add_executable(point_head src/point_head.cpp)
+    add_executable(torso src/torso.cpp)
+endif()
 
 # Include Caffe in controller
 if (USE_CAFFE)
     target_link_libraries(gps_agent_lib caffe protobuf)
 endif (USE_CAFFE)
 
-rosbuild_link_boost(gps_agent_lib thread)# smart_ptr)
+#rosbuild_link_boost(gps_agent_lib thread)# smart_ptr)
+catkin_package(CATKIN_DEPENDS message_runtime std_msgs)

src/gps_agent_pkg/package.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>gps_agent_pkg</name>
+  <version>0.0.0</version>
+
+  <maintainer email="[email protected]">Chelsea Finn</maintainer>
+
+  <description>Reimplementation of the Guided Policy Search (GPS) algorithm and LQG-based trajectory optimization.</description>
+  <author></author>
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_export_depend>message_runtime</build_export_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <build_depend>pr2_controller_interface</build_depend>
+  <build_depend>pr2_mechanism_model</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>roslib</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>pr2_controllers_msgs</build_depend>
+  <build_depend>control_toolbox</build_depend>
+  <build_depend>realtime_tools</build_depend>
+  <build_depend>orocos_kdl</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>pr2_controller_manager</build_depend>
+
+  <export>
+    <pr2_controller_interface plugin="${prefix}/controller_plugins.xml" />
+  </export>
+
+</package>
+
+

src/gps_agent_pkg/src/rostopicsensor.cpp

@@ -33,7 +33,7 @@ void ROSTopicSensor::update_data_vector(const std_msgs::Float64MultiArray::Const
     }
     for (int i = 0; i < data_size_; i++)
 	{
-	    if (isnan(msg->data[i])) {
+	    if (boost::math::isnan(msg->data[i])) {
 		    ROS_ERROR("data %d is nan %e", i, msg->data[i]);
 		}
 	    latest_data_[i] = msg->data[i];