Groovy

.gitignore

@@ -9,5 +9,5 @@ src/lisp/
 src/sot_pr2/
 srv/lisp/
 srv_gen/
-
+*~
 *.cfgc

.travis.yml

@@ -0,0 +1,14 @@
+language: cpp
+compiler:
+- gcc
+matrix:
+notifications:
+  email:
+  - [email protected]
+branches:
+  only:
+  - master
+  - groovy
+script: ./.travis/build
+before_install: ./.travis/before_install
+after_success: ./.travis/after_success

.travis/after_success

@@ -0,0 +1,6 @@
+#!/bin/sh
+set -ev
+
+git config --global user.name "Travis CI"
+git config --global user.email "[email protected]"
+

.travis/before_install

@@ -0,0 +1,56 @@
+#!/bin/bash
+set -ev
+
+export UBUNTU_CODENAME=`lsb_release -s -c`
+export ROS_VERSION=groovy
+
+echo "Ubuntu codename: $UBUNTU_CODENAME"
+echo "ROS version: $ROS_VERSION"
+
+if `test x$UBUNTU_CODENAME = x`; then
+    echo "failed to determine Ubuntu codename"
+    return 1
+fi
+if `test x$UBUNTU_CODENAME = x`; then
+    echo "failed to determine ROS version"
+    return 1
+fi
+
+git submodule update --init --recursive
+
+sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $UBUNTU_CODENAME main\" > /etc/apt/sources.list.d/ros-latest.list"
+wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+
+sudo apt-get update -qq
+
+sudo apt-get install -qq	\
+    doxygen doxygen-latex	\
+    libboost-all-dev		\
+    libeigen3-dev		\
+    liblapack-dev		\
+    libblas-dev			\
+    gfortran			\
+    python-dev			\
+    python-sphinx		\
+    python-numpy    \
+    libbullet-dev
+
+
+sudo apt-get install -qq		\
+    ros-$ROS_VERSION-ros-base		\
+    ros-$ROS_VERSION-ros-control	\
+    ros-$ROS_VERSION-pr2-mechanism	\
+    ros-$ROS_VERSION-pr2-controllers	\
+    ros-$ROS_VERSION-control-msgs	\
+    ros-$ROS_VERSION-common-rosdeps	\
+    ros-$ROS_VERSION-bullet		\
+    ros-$ROS_VERSION-ros-realtime	\
+    ros-$ROS_VERSION-tf			\
+    ros-$ROS_VERSION-resource-retriever	\
+    ros-$ROS_VERSION-urdf		\
+    ros-$ROS_VERSION-urdf-interface	\
+    ros-$ROS_VERSION-common-msgs	\
+    ros-$ROS_VERSION-rosdoc-lite
+
+sudo rosdep init
+rosdep update

.travis/build

@@ -0,0 +1,88 @@
+#!/bin/sh
+set -ev
+
+# Directories.
+root_dir=`pwd`
+build_dir="$root_dir/_travis/build"
+install_dir="$root_dir/_travis/install"
+
+# Shortcuts.
+git_clone="git clone --quiet --recursive"
+
+# Source ROS setup file
+. /opt/ros/*/setup.sh
+
+# Create layout.
+rm -rf "$build_dir" "$install_dir"
+mkdir -p "$build_dir"
+mkdir -p "$install_dir"
+
+# Setup environment variables.
+export LD_LIBRARY_PATH="$install_dir/lib:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`:$LD_LIBRARY_PATH"
+export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:$PKG_CONFIG_PATH"
+export PKG_CONFIG_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`/pkgconfig:$PKG_CONFIG_PATH"
+
+export ROS_PACKAGE_PATH="$root_dir:$ROS_PACKAGE_PATH"
+
+install_dependency()
+{
+    echo "--> Compiling $1"
+    mkdir -p "$build_dir/$1"
+    cd "$build_dir"
+    $git_clone "git://github.com/$1" "$1"
+    cd "$build_dir/$1"
+    cmake . -DCMAKE_INSTALL_PREFIX:STRING="$install_dir"
+    make install
+}
+
+install_ros_stack()
+{
+    echo "--> Compiling $1"
+    mkdir -p "$build_dir/$1"
+    cd "$build_dir"
+    if [ "$#" -eq "2" ]; then
+      $git_clone "git://github.com/$1" "$1" -b $2
+    else
+      $git_clone "git://github.com/$1" "$1"
+    fi
+    export ROS_PACKAGE_PATH="$build_dir/$1:$ROS_PACKAGE_PATH"
+}
+
+# Retrieve jrl-mathtools
+install_dependency jrl-umi3218/jrl-mathtools
+install_dependency jrl-umi3218/jrl-mal
+install_dependency laas/abstract-robot-dynamics
+install_dependency jrl-umi3218/jrl-dynamics
+install_dependency stack-of-tasks/dynamic-graph
+install_dependency stack-of-tasks/dynamic-graph-python
+install_dependency stack-of-tasks/sot-core
+install_dependency stack-of-tasks/sot-tools
+install_dependency stack-of-tasks/sot-dynamic
+install_dependency laas/jrl_dynamics_urdf
+install_ros_stack stack-of-tasks/redundant_manipulator_control groovy
+
+cd "$root_dir"
+# build dependencies
+rosdep install dynamic_graph_bridge_msgs
+rosmake dynamic_graph_bridge_msgs
+
+rosdep install dynamic_graph_bridge
+rosmake dynamic_graph_bridge
+cd "$build_dir"
+cd stack-of-tasks/redundant_manipulator_control/dynamic_graph_bridge/build
+cmake .. -DCMAKE_INSTALL_PREFIX:STRING="$install_dir"
+rosmake dynamic_graph_bridge
+make install
+
+
+# rosmake and install sot_pr2
+cd "$root_dir"
+export ROS_PACKAGE_PATH="$root_dir:$ROS_PACKAGE_PATH"
+rosdep install sot_pr2
+rosmake sot_pr2
+cd build
+cmake .. -DCMAKE_INSTALL_PREFIX:STRING="$install_dir"
+rosmake sot_pr2
+make install
+

CMakeLists.txt

@@ -3,6 +3,10 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 
 include(cmake/base.cmake)
 include(cmake/python.cmake)
+if(NOT DEFINED CMAKE_INSTALL_LIBDIR)
+  set(CMAKE_INSTALL_LIBDIR lib)
+endif()
+include(cmake/GNUInstallDirs.cmake)
 
 set(ROS_BUILD_TYPE RelWithDebInfo)
 
@@ -17,10 +21,19 @@ ADD_REQUIRED_DEPENDENCY("jrl-mal")
 ADD_REQUIRED_DEPENDENCY("dynamic-graph")
 ADD_REQUIRED_DEPENDENCY("dynamic-graph-python")
 ADD_REQUIRED_DEPENDENCY("sot-core")
+ADD_REQUIRED_DEPENDENCY("jrl-dynamics-urdf >= 2.0.1-14")
 
 # This is required by the Python prologue.
 ADD_REQUIRED_DEPENDENCY("sot-dynamic")
 
+
+# Handle rpath necessary to handle ROS multiplace packages
+# libraries inclusion
+SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
+SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
+SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
 # Python
 INCLUDE(cmake/python.cmake)
 FINDPYTHON()
@@ -29,11 +42,13 @@ LINK_DIRECTORIES(${PYTHON_LIBRARY_DIRS})
 
 set(SOURCES
     src/pr2_controller_plugin.cpp
+    src/pr2_threaded_sot_controller.cpp
     src/pr2_sot_controller.cpp
     src/pr2_device.cpp)
 
 set(HEADERS
     include/sot_pr2/pr2_controller_plugin.h
+    include/sot_pr2/pr2_threaded_sot_controller.h
     include/sot_pr2/pr2_sot_controller.h
     include/sot_pr2/pr2_device.h)
 
@@ -42,6 +57,17 @@ rosbuild_add_library(sot_pr2 ${HEADERS} ${SOURCES})
 PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "dynamic-graph")
 PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "sot-core")
 PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "dynamic-graph-python")
+PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "jrl-dynamics-urdf")
+install(TARGETS sot_pr2 DESTINATION ${CMAKE_INSTALL_LIBDIR})
+
+rosbuild_add_executable(sot_pr2_fake_controller ${HEADERS} src/fake_controller.cpp)
+PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "dynamic-graph")
+PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "sot-core")
+PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "dynamic-graph-python")
+PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "jrl-dynamics-urdf")
+target_link_libraries(sot_pr2_fake_controller sot_pr2)
+install(TARGETS sot_pr2_fake_controller DESTINATION ${CMAKE_INSTALL_LIBDIR})
 
 ADD_SUBDIRECTORY(src)
 ADD_SUBDIRECTORY(python)
+

include/sot_pr2/pr2_controller_plugin.h

@@ -3,12 +3,14 @@
 
 #include <pr2_controller_interface/controller.h>
 #include <pr2_mechanism_model/joint.h>
-#include <sot_pr2/pr2_sot_controller.h>
+#include <sot_pr2/pr2_threaded_sot_controller.h>
 #include <control_toolbox/pid.h>
 #include <boost/scoped_ptr.hpp>
 #include <realtime_tools/realtime_publisher.h>
 #include <control_msgs/JointTrajectoryControllerState.h>
 #include <tf/transform_listener.h>
+#include <actionlib/client/simple_action_client.h>
+#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>
 
 namespace sot_pr2 {
 
@@ -31,7 +33,7 @@ class Pr2ControllerPlugin : public pr2_controller_interface::Controller {
 
 private:
     // SoT Controller
-    Pr2SotController sot_controller_;
+    Pr2ThreadedSotController sot_controller_;
     SensorMap sensorsIn_;
     ControlMap controlValues_;
 
@@ -49,6 +51,12 @@ class Pr2ControllerPlugin : public pr2_controller_interface::Controller {
     std::vector<control_toolbox::Pid> pids_;
     pr2_mechanism_model::RobotState *robot_;
 
+    // Gripper
+    actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> *r_gripper_client_;
+    actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> *l_gripper_client_;
+    double r_gripper_position;
+    double l_gripper_position;
+
     // ROS interface
     //ros::NodeHandle node_;
     boost::scoped_ptr<
@@ -60,6 +68,9 @@ class Pr2ControllerPlugin : public pr2_controller_interface::Controller {
     tf::TransformListener listener_;
 
     double timeFromStart_;
+
+    int _iter;
+    double _mean;
 };
 
 }

include/sot_pr2/pr2_device.h

@@ -26,6 +26,7 @@ namespace sot_pr2 {
         void cleanupSetSensors(SensorMap &sensorsIn);
 
         void getControl(ControlMap &controlOut);
+        void getFakeControl(ControlMap &controlOut);
 
     protected:
         void updateRobotState(const std::vector<double> &anglesIn);

include/sot_pr2/pr2_sot_controller.h

@@ -13,7 +13,7 @@ class Pr2SotController : public dynamicgraph::sot::AbstractSotExternalInterface
     static const std::string LOG_PYTHON;
 
 public:
-    explicit Pr2SotController(std::string name);
+    explicit Pr2SotController();
     virtual ~Pr2SotController();
 
     void setupSetSensors(SensorMap &sensorsIn);

include/sot_pr2/pr2_threaded_sot_controller.h

@@ -0,0 +1,51 @@
+#ifndef PR2_SOT_CONTROLLER_H
+#define PR2_SOT_CONTROLLER_H
+
+#include <sot_pr2/pr2_device.h>
+#include <dynamic_graph_bridge/ros_interpreter.hh>
+#include <sot/core/abstract-sot-external-interface.hh>
+
+namespace sot_pr2 {
+
+class Pr2ThreadedSotController : public dynamicgraph::sot::AbstractSotExternalInterface
+{
+public:
+    static const std::string LOG_PYTHON;
+
+public:
+    explicit Pr2ThreadedSotController();
+    virtual ~Pr2ThreadedSotController();
+
+    void init();
+
+    void setupSetSensors(SensorMap &sensorsIn);
+    void nominalSetSensors(SensorMap &sensorsIn);
+    void cleanupSetSensors(SensorMap &sensorsIn);
+
+    void getControl(ControlMap &controlOut);
+
+    boost::shared_ptr<dynamicgraph::Interpreter> interpreter_;
+    ros::NodeHandle node_;
+
+protected:
+    void runPython(std::ostream &file,
+                   const std::string &command,
+                   dynamicgraph::Interpreter &interpreter);
+
+    virtual void startupPython();
+
+private:
+    Pr2Device device_;
+
+    SensorMap _holdIn;
+    ControlMap _holdOut;
+
+public:
+    Pr2Device *device() {return &device_;}
+    SensorMap &holdIn() {return _holdIn;}
+    ControlMap &holdOut() {return _holdOut;}
+};
+
+}
+
+#endif

launch/display.launch

@@ -6,8 +6,8 @@
     <param name="robot_base_link" value="base_footprint"/>
     <!--<param name="ref_link" value="odom_combined"/>-->
 
-    <arg name="frame" value="base_footprint"/>
-    <arg name="child_frame" value="odom"/>
+    <arg name="frame" value="odom"/>
+    <arg name="child_frame" value="base_footprint"/>
 
     <node name="robot_pose_publisher" pkg="sot_pr2" type="robot_pose_publisher" args="_frame:=$(arg frame) _child_frame:=$(arg child_frame)">
         <env name="ROS_NAMESPACE" value="/dynamic_graph" />

launch/gazebo.launch

@@ -1,5 +1,9 @@
 <launch>
-    <include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
+    <include file="$(find sot_pr2)/launch/pr2_empty_world.launch">
+	<arg name="use_sim_time" value="true" />
+	<arg name="throttled" default="false"/>
+        <arg name="physics" default="true"/>
+    </include>
 
     <!-- Startup PR2 without any mechanism controllers -->
     <include file="$(find sot_pr2)/launch/spawn_pr2.launch"/>
@@ -8,12 +12,5 @@
         <env name="ROS_NAMESPACE" value="/dynamic_graph"/>
     </node>-->
 
-    <!--<rosparam file="$(find pr2_controller_configuration_gazebo)/pr2_base_controller.yaml" command="load" />
-    <rosparam file="$(find pr2_controller_configuration_gazebo)/pr2_odometry.yaml" command="load" />
-    <node name="pr2_base_controllers_spawner" pkg="pr2_controller_manager" type="spawner" output="screen"
-        args="--wait-for=/calibrated base_controller base_odometry" />-->
-
-    <include file="$(find sot_pr2)/launch/spawn_pr2_controller.launch" />
-
 </launch>