Official 14.04 version on Indigo

Author: cassinaj

CMakeLists.txt

@@ -31,8 +31,11 @@ find_package(catkin REQUIRED
     kdl_parser
     rosbag
     message_filters
-    robot_state_pub
+    robot_state_publisher
     image_transport
+    tf
+    tf_conversions
+    tf2_ros
     fl
     dbot
     dbot_ros
@@ -46,11 +49,9 @@ set(CMAKE_MODULE_PATH ${PROJECT_MODULE_PATH})
 find_package(Boost REQUIRED)
 find_package(Eigen REQUIRED)
 find_package(OpenCV REQUIRED)
-#find_package(PCL 1.3 REQUIRED)
 
 include_directories(${Boost_INCLUDE_DIRS})
 include_directories(${Eigen_INCLUDE_DIRS})
-#include_directories(${PCL_INCLUDE_DIRS})
 include_directories(${OpenCV_INCLUDE_DIRS})
 
 ##########################################
@@ -98,8 +99,11 @@ catkin_package(
         urdf
         kdl_parser
         message_filters
-        robot_state_pub
+        robot_state_publisher
         image_transport
+        tf
+        tf_conversions
+        tf2_ros
         fl
         dbot
         dbot_ros
@@ -124,6 +128,7 @@ set(sources
     source/${PROJECT_NAME}/urdf_object_loader.cpp
     source/${PROJECT_NAME}/kinematics_from_urdf.cpp
     source/${PROJECT_NAME}/robot_transformer.cpp
+    source/${PROJECT_NAME}/robot_transforms_provider.cpp
     source/${PROJECT_NAME}/tracker/robot_tracker.cpp
     source/${PROJECT_NAME}/tracker/fusion_tracker.cpp
     source/${PROJECT_NAME}/tracker/visual_tracker.cpp
@@ -142,18 +147,17 @@ add_library(${PROJECT_NAME} ${dbot_headers}
                             ${headers}
                             ${sources})
 
-target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}
-                                      ${catkin_LIBRARIES}
-                                      ${OpenCV_LIBRARIES}
-                                      assimp)
+target_link_libraries(${PROJECT_NAME} 
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+  assimp)
 
 add_executable(visual_tracker
      source/${PROJECT_NAME}/tracker/visual_tracker_node.cpp)
 target_link_libraries(visual_tracker
      ${PROJECT_NAME}
      ${catkin_LIBRARIES}
      ${OpenCV_LIBS}
-     ${PCL_LIBRARIES}
      yaml-cpp)
 
 add_executable(rotary_tracker
@@ -162,15 +166,13 @@ target_link_libraries(rotary_tracker
      ${PROJECT_NAME}
      ${catkin_LIBRARIES}
      ${OpenCV_LIBS}
-     ${PCL_LIBRARIES}
      yaml-cpp)
 
 add_executable(fusion_tracker
      source/${PROJECT_NAME}/tracker/fusion_tracker_node.cpp)
 target_link_libraries(fusion_tracker
      ${PROJECT_NAME}
      ${catkin_LIBRARIES}
-     ${OpenCV_LIBS}
      ${PCL_LIBRARIES}
      yaml-cpp)
 
@@ -180,6 +182,5 @@ target_link_libraries(robot_emulator
      ${PROJECT_NAME}
      ${catkin_LIBRARIES}
      ${OpenCV_LIBS}
-     ${PCL_LIBRARIES}
      yaml-cpp)
 

package.xml

@@ -8,29 +8,30 @@
 
   <license>GPL</license>
 
-  <url type="website">http://github.com/jan-issac/rbot</url>
+  <url type="website">http://github.com/bayesian-object-tracking</url>
 
+  <author email="[email protected]">Jan Issac</author>
   <author email="[email protected]">Manuel Wuthrich</author>
   <author email="[email protected]">Jeannette Bohg</author>
-  <author email="[email protected]">Jan Issac</author>
 
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>roscpp</build_depend>
   <build_depend>roslib</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>VTK</build_depend>
-  <build_depend>OpenCV</build_depend>
   <build_depend>eigen</build_depend>
   <build_depend>assmp</build_depend>
-  <build_depend>pcl</build_depend>
   <build_depend>cv_bridge</build_depend>
   <build_depend>urdf</build_depend>
   <build_depend>orocos_kdl</build_depend>
   <build_depend>kdl_parser</build_depend>
   <build_depend>message_filters</build_depend>
-  <build_depend>robot_state_pub</build_depend>
+  <build_depend>robot_state_publisher</build_depend>
   <build_depend>image_transport</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>tf_conversions</build_depend>
+  <build_depend>tf2_ros</build_depend>
   <build_depend>fl</build_depend>
   <build_depend>dbot</build_depend>
   <build_depend>dbot_ros</build_depend>
@@ -39,15 +40,16 @@
   <run_depend>roslib</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>VTK</run_depend>
-  <run_depend>OpenCV</run_depend>
   <run_depend>eigen</run_depend>
   <run_depend>assimp</run_depend>
-  <run_depend>pcl</run_depend>
   <run_depend>urdf</run_depend>
   <run_depend>orocos_kdl</run_depend>
   <run_depend>kdl_parser</run_depend>
   <run_depend>message_filters</run_depend>
-  <run_depend>robot_state_pub</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>tf_conversions</run_depend>
+  <run_depend>tf2_ros</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>fl</run_depend>
   <run_depend>dbot</run_depend>

source/dbrt/builder/exceptions.hpp source/dbrt/builder/exceptions.h

@@ -23,7 +23,6 @@
 
 namespace dbrt
 {
-
 /**
  * \brief Represents an exception thrown if the number of joint sigmas in the
  * of the joint_transition model does not match the state dimension.
@@ -38,7 +37,6 @@ class InvalidNumberOfJointSigmasException : public std::exception
     }
 };
 
-
 /**
  * \brief Represents an exception thrown if the number of indices in the
  * sampling block does not match the state dimension.
@@ -48,13 +46,13 @@ class InvalidNumberOfSamplingBlocksException : public std::exception
 public:
     const char* what() const noexcept
     {
-        return "The number of indices in the sampling blocks does not match the "
+        return "The number of indices in the sampling blocks does not match "
+               "the "
                "number of joints (joint state dimension) of the robot.";
     }
 };
 
-
-class JointIndexOutOfBoundsException: public std::exception
+class JointIndexOutOfBoundsException : public std::exception
 {
 public:
     const char* what() const noexcept
@@ -63,7 +61,4 @@ class JointIndexOutOfBoundsException: public std::exception
                " of available joints!";
     }
 };
-
-
-
 }

source/dbrt/builder/factorized_transition_builder.hpp source/dbrt/builder/factorized_transition_builder.h

@@ -12,25 +12,20 @@
  */
 
 /**
- * \file factorized_transition_builder.hpp
+ * \file factorized_transition_builder.h
  * \date January 2016
  * \author Jan Issac ([email protected])
  */
 
 #pragma once
 
-#include <memory>
-
-#include <fl/util/profiling.hpp>
-#include <fl/util/meta.hpp>
-#include <fl/model/transition/linear_transition.hpp>
-
 #include <Eigen/Dense>
-
-#include <dbot/builder/transition_function_builder.hpp>
-
-#include <dbrt/builder/exceptions.hpp>
-#include <dbrt/builder/exceptions.hpp>
+#include <dbot/builder/transition_function_builder.h>
+#include <dbrt/builder/exceptions.h>
+#include <fl/model/transition/linear_transition.hpp>
+#include <fl/util/meta.hpp>
+#include <fl/util/profiling.hpp>
+#include <memory>
 
 namespace dbrt
 {
@@ -52,7 +47,7 @@ class FactorizedTransitionBuilder
 
     struct Parameters
     {
-//        double joint_sigma;
+        //        double joint_sigma;
         std::vector<double> joint_sigmas;
         std::vector<double> bias_sigmas;
         std::vector<double> bias_factors;
@@ -80,8 +75,7 @@ class FactorizedTransitionBuilder
             throw JointIndexOutOfBoundsException();
         }
 
-
-        if(StateDim != 2 || NoiseDim != 2 || InputDim != 1)
+        if (StateDim != 2 || NoiseDim != 2 || InputDim != 1)
         {
             std::cout << "damn you screwed up dimensions" << std::endl;
             exit(-1);
@@ -94,11 +88,11 @@ class FactorizedTransitionBuilder
         auto C = model->create_input_matrix();
 
         A.setIdentity();
-        A(1,1) = param_.bias_factors[joint_index];
+        A(1, 1) = param_.bias_factors[joint_index];
 
         B.setIdentity();
-        B(0,0) = param_.joint_sigmas[joint_index];
-        B(1,1) = param_.bias_sigmas[joint_index];
+        B(0, 0) = param_.joint_sigmas[joint_index];
+        B(1, 1) = param_.bias_sigmas[joint_index];
 
         model->dynamics_matrix(A);
         model->noise_matrix(B);

source/dbrt/builder/robot_rb_sensor_builder.cpp

@@ -28,12 +28,12 @@
  * \author Jan Issac ([email protected])
  */
 
-#include <dbrt/robot_state.hpp>
+// #include <kdl_parser/kdl_parser.hpp>
+#include <dbrt/robot_state.h>
 
 #include <dbot/builder/rb_sensor_builder.h>
 #include <dbot/builder/rb_sensor_builder.hpp>
 
-
 namespace dbot
 {
 template class RbSensorBuilder<dbrt::RobotState<>>;

source/dbrt/builder/rotary_sensor_builder.hpp source/dbrt/builder/rotary_sensor_builder.h

@@ -12,29 +12,23 @@
  */
 
 /**
- * \file robot_joint_sensor_builder.hpp
+ * \file robot_joint_sensor_builder.h
  * \date January 2016
  * \author Jan Issac ([email protected])
  */
 
 #pragma once
 
-#include <memory>
-
-#include <fl/util/profiling.hpp>
-#include <fl/util/meta.hpp>
-#include <fl/model/sensor/linear_gaussian_sensor.hpp>
-
 #include <Eigen/Dense>
-
-#include <dbot/builder/transition_function_builder.hpp>
-
-#include <dbrt/builder/exceptions.hpp>
-#include <dbrt/builder/exceptions.hpp>
+#include <dbot/builder/transition_function_builder.h>
+#include <dbrt/builder/exceptions.h>
+#include <fl/model/sensor/linear_gaussian_sensor.hpp>
+#include <fl/util/meta.hpp>
+#include <fl/util/profiling.hpp>
+#include <memory>
 
 namespace dbrt
 {
-
 template <typename Tracker>
 class RotarySensorBuilder
 {
@@ -55,10 +49,7 @@ class RotarySensorBuilder
         int joint_count;
     };
 
-    RotarySensorBuilder(const Parameters& param) : param_(param)
-    {
-    }
-
+    RotarySensorBuilder(const Parameters& param) : param_(param) {}
     virtual std::shared_ptr<Model> build(int joint_index) const
     {
         if (param_.joint_count != param_.joint_sigmas.size())
@@ -71,8 +62,7 @@ class RotarySensorBuilder
             throw JointIndexOutOfBoundsException();
         }
 
-
-        if(StateDim != 2 || ObsrvDim != 1)
+        if (StateDim != 2 || ObsrvDim != 1)
         {
             std::cout << "dawg you screwed up dimensions" << std::endl;
             exit(-1);

source/dbrt/builder/rotary_tracker_builder.hpp source/dbrt/builder/rotary_tracker_builder.h

@@ -12,23 +12,21 @@
  */
 
 /**
- * \file particle_tracker_builder.hpp
+ * \file rotary_sensor_builder.h
  * \date November 2015
  * \author Jan Issac ([email protected])
  */
 
 #pragma once
 
-#include <exception>
-
-#include <dbot/object_resource_identifier.hpp>
-#include <dbot/tracker/tracker.hpp>
-#include <dbot/builder/particle_tracker_builder.hpp>
-
+#include <dbot/builder/particle_tracker_builder.h>
+#include <dbot/object_resource_identifier.h>
+#include <dbot/tracker/tracker.h>
+#include <dbrt/builder/factorized_transition_builder.h>
+#include <dbrt/builder/rotary_sensor_builder.h>
 #include <dbrt/kinematics_from_urdf.h>
 #include <dbrt/tracker/rotary_tracker.h>
-#include <dbrt/builder/factorized_transition_builder.hpp>
-#include <dbrt/builder/rotary_sensor_builder.hpp>
+#include <exception>
 
 namespace dbrt
 {