diff --git a/python/gps/gps_main.py b/python/gps/gps_main.py index d3a0ea883..f391186a8 100755 --- a/python/gps/gps_main.py +++ b/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 diff --git a/src/3rdparty/Boost.NumPy/CMakeLists.txt b/src/3rdparty/Boost.NumPy/CMakeLists.txt index cffd33f98..7eac02fe2 100644 --- a/src/3rdparty/Boost.NumPy/CMakeLists.txt +++ b/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 diff --git a/src/gps_agent_pkg/CMakeLists.txt b/src/gps_agent_pkg/CMakeLists.txt index c669f7c9f..a56d181c8 100644 --- a/src/gps_agent_pkg/CMakeLists.txt +++ b/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) \ No newline at end of file diff --git a/src/gps_agent_pkg/manifest.xml b/src/gps_agent_pkg/manifest.xml deleted file mode 100644 index b0d983dad..000000000 --- a/src/gps_agent_pkg/manifest.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - gps_agent_pkg - - - BSD - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/gps_agent_pkg/package.xml b/src/gps_agent_pkg/package.xml new file mode 100644 index 000000000..c7cfe743b --- /dev/null +++ b/src/gps_agent_pkg/package.xml @@ -0,0 +1,37 @@ + + + gps_agent_pkg + 0.0.0 + + Chelsea Finn + + Reimplementation of the Guided Policy Search (GPS) algorithm and LQG-based trajectory optimization. + + BSD + catkin + message_generation + message_runtime + message_runtime + 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 + + + + + + + + diff --git a/src/gps_agent_pkg/src/rostopicsensor.cpp b/src/gps_agent_pkg/src/rostopicsensor.cpp index 9986ac4ba..9930ff53f 100644 --- a/src/gps_agent_pkg/src/rostopicsensor.cpp +++ b/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];